Merged pcl_integration branch to trunk

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1014 f169173b-cf89-36c8-b27e-44dbe73f0c83

utilite/include/rtabmap/utilite/UEventsHandler.h

@@ -22,6 +22,7 @@
 
 #include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines
 
+#include "rtabmap/utilite/UEventsSender.h"
 class UEvent;
 
 /**
@@ -124,8 +125,11 @@ class UEvent;
  * @see UThreadNode
  *
  */
-class UTILITE_EXP UEventsHandler{
+class UTILITE_EXP UEventsHandler : public UEventsSender {
 public:
+
+	void registerToEventsManager();
+	void unregisterFromEventsManager();
     
 
 protected:
@@ -172,11 +176,6 @@ protected:
      */
     virtual ~UEventsHandler();
 
-    /**
-     * For convenience to post an event. This is the same than calling UEventsManager::post().
-     */
-    void post(UEvent * event, bool async = true);
-
 private:
     
 };

utilite/include/rtabmap/utilite/UEventsManager.h

@@ -28,6 +28,7 @@
 #include "rtabmap/utilite/UDestroyer.h"
 
 #include <list>
+#include <map>
 
 // TODO Not implemented... for multithreading event handling
 class UEventDispatcher : public UThread
@@ -106,8 +107,21 @@ public:
      * @param event the event to be posted.
      * @param async if true, the event is dispatched by the UEventsManager thread, otherwise it's in the caller thread (synchronous).
      */
-    static void post(UEvent * event, bool async = true);
-    
+    static void post(UEvent * event, bool async = true, const UEventsSender * sender = 0);
+
+    static void createPipe(
+		const UEventsSender * sender,
+		const UEventsHandler * receiver,
+		const std::string & eventName);
+
+    static void removePipe(
+		const UEventsSender * sender,
+		const UEventsHandler * receiver,
+		const std::string & eventName);
+
+    static void removeAllPipes(const UEventsSender * sender);
+    static void removeNullPipes(const UEventsSender * sender);
+
 protected:
 
     /*
@@ -161,7 +175,7 @@ private:
     /*
 	 * This method dispatches an event to all handlers.
 	 */
-    virtual void dispatchEvent(UEvent * event);
+    virtual void dispatchEvent(UEvent * event, const UEventsSender * sender);
 
     /*
      * This method is used to add an events 
@@ -191,17 +205,49 @@ private:
      * @param event the event to be posted.
      * @param async if true, the event is dispatched by the UEventsManager thread, otherwise it's in the caller thread (synchronous).
      */
-    void _postEvent(UEvent * event, bool async = true);
-    
+    void _postEvent(UEvent * event, bool async = true, const UEventsSender * sender = 0);
+
+    std::list<UEventsHandler*> getPipes(
+    		const UEventsSender * sender,
+    		const std::string & eventName);
+
+    void _createPipe(
+		const UEventsSender * sender,
+		const UEventsHandler * receiver,
+		const std::string & eventName);
+
+    void _removePipe(
+		const UEventsSender * sender,
+		const UEventsHandler * receiver,
+		const std::string & eventName);
+
+    void _removeAllPipes(const UEventsSender * sender);
+    void _removeNullPipes(const UEventsSender * sender);
+
 private:
     
+    class Pipe
+    {
+    public:
+    	Pipe(const UEventsSender * sender, const UEventsHandler * receiver, const std::string & eventName) :
+    		sender_(sender),
+    		receiver_(receiver),
+    		eventName_(eventName)
+    	{}
+    	const UEventsSender * sender_;
+    	const UEventsHandler * receiver_;
+    	const std::string eventName_;
+    };
+
     static UEventsManager* instance_;            /* The EventsManager instance pointer. */
     static UDestroyer<UEventsManager> destroyer_; /* The EventsManager's destroyer. */
-    std::list<UEvent*> events_;                /* The events list. */
+    std::list<std::pair<UEvent*, const UEventsSender * > > events_; /* The events list. */
     std::list<UEventsHandler*> handlers_;      /* The handlers list. */
     UMutex eventsMutex_;                         /* The mutex of the events list, */
     UMutex handlersMutex_;                       /* The mutex of the handlers list. */
     USemaphore postEventSem_;                    /* Semaphore used to signal when an events is posted. */
+    std::list<Pipe> pipes_;
+    UMutex pipesMutex_;
 };
 
 #endif // UEVENTSMANAGER_H

utilite/include/rtabmap/utilite/UEventsSender.h

@@ -0,0 +1,31 @@
+/*
+ * UEventsSender.h
+ *
+ *  Created on: 2013-10-14
+ *      Author: Mathieu
+ */
+
+#ifndef UEVENTSSENDER_H_
+#define UEVENTSSENDER_H_
+
+#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines
+
+class UEvent;
+
+class UTILITE_EXP UEventsSender
+{
+public:
+	UEventsSender(){}
+	virtual ~UEventsSender();
+
+protected:
+
+	/**
+	 * For convenience to post an event. This is the same than calling UEventsManager::post()
+	 * with the sender reference.
+	 */
+	void post(UEvent * event, bool async = true) const;
+};
+
+
+#endif /* UEVENTSSENDER_H_ */

utilite/include/rtabmap/utilite/UFile.h

@@ -70,8 +70,17 @@ public:
 	 */
 	static std::string getName(const std::string & filePath);
 
+	/**
+	 * Get the file extension.
+	 * @return the file extension
+	 */
 	static std::string getExtension(const std::string &filePath);
 
+	/**
+	 * Copy a file.
+	 * @param from the file path
+	 * @param to destination file path
+	 */
 	static void copy(const std::string & from, const std::string & to);
 
 public:
@@ -130,6 +139,10 @@ public:
 	 */
 	std::string getExtension() {return getExtension(path_);}
 
+	/**
+	 * Copy a file.
+	 * @param to destination file path
+	 */
 	void copy(const std::string & to) {copy(path_, to);}
 
 private:

utilite/include/rtabmap/utilite/UMath.h

@@ -30,6 +30,39 @@
 #include <list>
 #include <vector>
 
+#if _MSC_VER
+	#undef min
+	#undef max
+#endif
+
+/**
+ * Return true if the number is NAN.
+ */
+template<class T>
+inline bool uIsNan(const T & value)
+{
+#ifdef __APPLE__
+	return std::isnan(value);
+#elif _MSC_VER
+	return _isnan(value);
+#else
+	return isnan(value);
+#endif
+}
+
+/**
+ * Return true if the number is finite.
+ */
+template<class T>
+inline bool uIsFinite(const T & value)
+{
+#if _MSC_VER
+	return _finite(value);
+#else
+	return std::isfinite(value);
+#endif
+}
+
 /**
  * Get the maximum of a vector.
  * @param v the array
@@ -49,7 +82,7 @@ inline T uMax(const T * v, unsigned int size, unsigned int & index)
 	max = v[0];
 	for(unsigned int i=1; i<size; ++i)
 	{
-		if(max < v[i])
+		if(uIsNan(max) || (max < v[i] && !uIsNan(v[i])))
 		{
 			max = v[i];
 			index = i;
@@ -114,7 +147,7 @@ inline T uMin(const T * v, unsigned int size, unsigned int & index)
 	min = v[0];
 	for(unsigned int i=1; i<size; ++i)
 	{
-		if(min > v[i])
+		if(uIsNan(min) || (min > v[i] && !uIsNan(v[i])))
 		{
 			min = v[i];
 			index = i;
@@ -182,15 +215,16 @@ inline void uMinMax(const T * v, unsigned int size, T & min, T & max, unsigned i
 	}
 	min = v[0];
 	max = v[0];
+
 	for(unsigned int i=1; i<size; ++i)
 	{
-		if(min > v[i])
+		if(uIsNan(min) || (min > v[i] && !uIsNan(v[i])))
 		{
 			min = v[i];
 			indexMin = i;
 		}
 
-		if(max < v[i])
+		if(uIsNan(max) || (max < v[i] && !uIsNan(v[i])))
 		{
 			max = v[i];
 			indexMax = i;
@@ -627,7 +661,7 @@ inline std::vector<T> uXMatch(const T * vA, const T * vB, unsigned int sizeA, un
 	}
 	else if(method == UXCorrBiased || method == UXCovBiased)
 	{
-		den = std::max(sizeA, sizeB);
+		den = (T)std::max(sizeA, sizeB);
 	}
 
 	if(sizeA == sizeB)
@@ -755,7 +789,7 @@ inline T uXMatch(const T * vA, const T * vB, unsigned int sizeA, unsigned int si
 	}
 	else if(method == UXCorrBiased || method == UXCovBiased)
 	{
-		den = std::max(sizeA, sizeB);
+		den = (T)std::max(sizeA, sizeB);
 	}
 	else if(method == UXCorrUnbiased || method == UXCovUnbiased)
 	{
@@ -821,7 +855,7 @@ inline std::vector<float> uHamming(unsigned int L)
 	float pi = 3.14159265f;
 	for(unsigned int n=0; n<N; ++n)
 	{
-		w[n] = 0.54-0.46*std::cos(2*pi*n/N);
+		w[n] = 0.54f-0.46f*std::cos(2.0f*pi*float(n)/float(N));
 	}
 	return w;
 }

utilite/include/rtabmap/utilite/UtiLite.h

@@ -49,7 +49,7 @@
   * \section logger ULogger
   * The ULogger can be used anywhere in the application to log messages (formated like a printf). The
   * logger can be set (ULogger::setType()) to print in a file or in the console (with colors depending on the severity). Convenient
-  * macros are given, working like a printf(), as UDEBUG(), UINFO(), UWARN(), UERROR(), UFATAL(). Small example:
+  * macros are given, working like a printf(), as UDEBUG(), UINFO(), UWARN(), UERROR(), UFATAL(), UASSERT(). Small example:
   * @code
   * ...
   * UINFO("A simple message with number %d", 42);
@@ -77,17 +77,17 @@
   * // The UEventsHandler::handleEvent() of "handler" will then be called by the UEventsManager's events dispatching thread.
   * ...
   * @endcode
-  * Look at the <b>full example</b> in page of UEventsHandler on how communication works with threads (UThreadNode).
+  * Look at the <b>full example</b> in page of UEventsHandler on how communication works with threads (UThread).
   *
-  * \section threadnode UThreadNode, UMutex, USemaphore
-  * The multi-threading framework use a UThreadNode as an abstract class to implement a
-  * thread in object-style. Reimplement UThreadNode::mainLoop() then call UThreadNode::start() to
-  * start the main loop of the thread. Threads can be joined by calling UThreadNode::join() and
-  * killed by calling UThreadNode::kill(). Classes UMutex and USemaphore provide blocking mechanisms to
+  * \section thread UThread, UMutex, USemaphore
+  * The multi-threading framework use a UThread as an abstract class to implement a
+  * thread in object-style. Reimplement UThread::mainLoop() then call UThread::start() to
+  * start the main loop of the thread. Threads can be joined by calling UThread::join() and
+  * killed by calling UThread::kill(). Classes UMutex and USemaphore provide blocking mechanisms to
   * protect data between threads.
   * @code
   * ...
-  * MyThread t; // MyThread is an implementation of UThreadNode
+  * MyThread t; // MyThread is an implementation of UThread
   * t.start();
   * t.join(); // Wait the thread to finish
   * ...
@@ -168,6 +168,9 @@
   * useful widgets is built. Use class UPlot to create a plot like MATLAB, and incrementally add
   * new values like a scope. USpectrogram is used to
   * show audio frequency frames.
+  * - UPlot,
+  * - USpectrogram,
+  * - UImageView.
   * @image html UPlot.gif
   * @image html USpectrogram.png
   *
@@ -175,13 +178,26 @@
   * If FMOD is found on the system, the UtiLite audio
   * library is built (libutilite_audio.so, libutilite_audio.dll). It is a wrapper
   * of FMOD methods with a convenient interface to extract audio frames.
-  * - UAudioRecorder,
-  * - UAudioRecorderFile,
-  * - UAudioRecorderMic
+  * - UAudioCapture,
+  * - UAudioCaptureFile,
+  * - UAudioCaptureMic,
+  * - UAudioCaptureFFT,
   * - UAudioPlayer,
   * - UAudioPlayerTone,
   * - UWav,
   * - UMp3Encoder (only if Lame is also found on the system).
+  *
+  * \section cvLib OpenCV stuff (libutilite_cv.so : OPTIONAL)
+  * If OpenCV is found on the system, the UtiLite cv
+  * library is built (libutilite_cv.so, libutilite_cv.dll). It provides
+  * image capture classes used to read from a webcam, a video file
+  * or a directory of images. If UtiLite is also built with Qt, a
+  * convenient function uCvMat2QImage() can be used to convert a cv::Mat
+  * image to a QImage.
+  * - UVideoCapture,
+  * - UImageFolderCapture,
+  * - UColorTable,
+  * - uCvMat2QImage() (only if Qt is also found on the system).
   */
 
 /*! \page uResourceGeneratorPage uResourceGenerator