Made Freenect dependency optional

fixed some toro3d warnings

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

CMakeLists.txt

@@ -136,7 +136,7 @@ FIND_PACKAGE(OpenCV REQUIRED)
 FIND_PACKAGE(PCL 1.7 REQUIRED)
 FIND_PACKAGE(VTK REQUIRED)
 FIND_PACKAGE(ZLIB REQUIRED)
-FIND_PACKAGE(Freenect REQUIRED)
+FIND_PACKAGE(Freenect)
 
 # If Qt is here, the GUI will be built
 FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg) 
@@ -318,4 +318,9 @@ ENDIF(NOT WIN32)
 IF(APPLE)
 MESSAGE(STATUS "  BUILD_AS_BUNDLE = ${BUILD_AS_BUNDLE}")
 ENDIF(APPLE)
+IF(Freenect_FOUND)
+MESSAGE(STATUS "  Freenect found.")
+ELSE()
+MESSAGE(STATUS "  Freenect (libfreenect) not found.")
+ENDIF()
 MESSAGE(STATUS "--------------------------------------------")

cmake_modules/FindFreenect.cmake

@@ -6,7 +6,7 @@
 #  Freenect_INCLUDE_DIRS - The Freenect include directory.
 #  Freenect_LIBRARIES     - The Freenect library to link against.
 
-FIND_PATH(Freenect_INCLUDE_DIRS libfreenect.hpp PATH_SUFFIXES libfreenect)
+FIND_PATH(Freenect_INCLUDE_DIRS libfreenect.h PATH_SUFFIXES libfreenect)
 
 FIND_LIBRARY(Freenect_LIBRARY NAMES freenect)
 FIND_LIBRARY(Freenect_sync_LIBRARY NAMES freenect_sync)

corelib/include/rtabmap/core/CameraFreenect.h

@@ -62,6 +62,9 @@ class RTABMAP_EXP FreenectDevice {
 
 class RTABMAP_EXP CameraFreenect : public UEventsSender, public UThread
 {
+public:
+	static bool available();
+
 public:
 	// default local transform z in, x right, y down));
 	CameraFreenect(int deviceId= 0,

corelib/src/CMakeLists.txt

@@ -44,9 +44,26 @@ SET(INCLUDE_DIRS
     ${OpenCV_INCLUDE_DIRS}
 	${PCL_INCLUDE_DIRS}
 	${ZLIB_INCLUDE_DIRS}
-	${Freenect_INCLUDE_DIRS}
 )
 
+SET(LIBRARIES
+	${OpenCV_LIBS} 
+	${PCL_LIBRARIES} 
+	${ZLIB_LIBRARIES} 
+)
+
+IF(Freenect_FOUND)
+	ADD_DEFINITIONS("-DWITH_FREENECT")
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${Freenect_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${Freenect_LIBRARIES}
+	)
+ENDIF(Freenect_FOUND)
+
 ####################################
 # Generate resources files
 ####################################
@@ -83,7 +100,7 @@ INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 # Add binary that is built from the source file "main.cpp".
 # The extension is automatically found.
 ADD_LIBRARY(rtabmap_core ${SRC_FILES} ${RESOURCES_HEADERS})
-TARGET_LINK_LIBRARIES(rtabmap_core rtabmap_utilite ${OpenCV_LIBS} ${PCL_LIBRARIES} ${ZLIB_LIBRARIES} ${Freenect_LIBRARIES})
+TARGET_LINK_LIBRARIES(rtabmap_core rtabmap_utilite ${LIBRARIES})
 
 INSTALL(TARGETS rtabmap_core
         RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT runtime

corelib/src/CameraFreenect.cpp

@@ -13,7 +13,9 @@
 #include <rtabmap/utilite/UEventsManager.h>
 #include <opencv2/imgproc/imgproc.hpp>
 
+#ifdef WITH_FREENECT
 #include <libfreenect.h>
+#endif
 
 namespace rtabmap {
 
@@ -32,6 +34,7 @@ FreenectDevice::FreenectDevice(freenect_context * ctx, int index) :
 	UASSERT(ctx_ != 0);
 }
 
+#ifdef WITH_FREENECT
 FreenectDevice::~FreenectDevice() {
 	if(device_ && freenect_close_device(device_) < 0){} //FN_WARNING("Device did not shutdown in a clean fashion");
 }
@@ -71,6 +74,21 @@ void FreenectDevice::freenect_video_callback(freenect_device *dev, void *video,
 	FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
 	device->VideoCallback(video, timestamp);
 }
+#else
+FreenectDevice::~FreenectDevice() {}
+void FreenectDevice::startVideo() {}
+void FreenectDevice::stopVideo() {}
+void FreenectDevice::startDepth() {}
+void FreenectDevice::stopDepth() {}
+
+bool FreenectDevice::init()
+{
+	UERROR("RTAB-Map is not built with Freenect support!");
+	return false;
+}
+void FreenectDevice::freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp) {}
+void FreenectDevice::freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp) {}
+#endif
 
 // Do not call directly even in child
 void FreenectDevice::VideoCallback(void* _rgb, uint32_t timestamp)
@@ -120,8 +138,17 @@ cv::Mat FreenectDevice::getDepth()
 
 
 //
-// CameraOpenKinect
+// CameraFreenect
 //
+bool CameraFreenect::available()
+{
+#ifdef WITH_FREENECT
+	return true;
+#else
+	return false;
+#endif
+}
+
 CameraFreenect::CameraFreenect(int deviceId, float inputRate, const Transform & localTransform) :
 		deviceId_(deviceId),
 		rate_(inputRate),
@@ -131,10 +158,12 @@ CameraFreenect::CameraFreenect(int deviceId, float inputRate, const Transform &
 		ctx_(0),
 		freenectDevice_(0)
 {
+#ifdef WITH_FREENECT
 	if(freenect_init(&ctx_, NULL) < 0) UERROR("Cannot initialize freenect library");
 	// We claim both the motor and camera devices, since this class exposes both.
 	// It does not support audio, so we do not claim it.
 	freenect_select_subdevices(ctx_, static_cast<freenect_device_flags>(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
+#endif
 }
 
 CameraFreenect::~CameraFreenect()
@@ -146,12 +175,15 @@ CameraFreenect::~CameraFreenect()
 		delete freenectDevice_;
 		freenectDevice_ = 0;
 	}
+#ifdef WITH_FREENECT
 	if(freenect_shutdown(ctx_) < 0){} //FN_WARNING("Freenect did not shutdown in a clean fashion");
+#endif
 	delete frameRateTimer_;
 }
 
 bool CameraFreenect::init()
 {
+#ifdef WITH_FREENECT
 	if(!this->isRunning())
 	{
 		if(freenectDevice_)
@@ -174,13 +206,16 @@ bool CameraFreenect::init()
 		}
 		else
 		{
-			UERROR("CameraOpenKinect: No devices connected!");
+			UERROR("CameraFreenect: No devices connected!");
 		}
 	}
 	else
 	{
-		UERROR("CameraOpenKinect: Cannot initialize the camera because it is already running...");
+		UERROR("CameraFreenect: Cannot initialize the camera because it is already running...");
 	}
+#else
+	UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
+#endif
 	return false;
 }
 
@@ -200,12 +235,14 @@ void CameraFreenect::mainLoopBegin()
 	}
 	else
 	{
-		UERROR("CameraOpenKinect: init should be called before starting the camera.");
+		UERROR("CameraFreenect: init should be called before starting the camera.");
+		this->kill();
 	}
 }
 
 void CameraFreenect::mainLoop()
 {
+#ifdef WITH_FREENECT
 	timeval t;
 	t.tv_sec = 0;
 	t.tv_usec = 10000;
@@ -226,13 +263,13 @@ void CameraFreenect::mainLoop()
 
 			if(depth.empty())
 			{
-				UWARN("CameraOpenKinect: Depth not ready! Try to reduce the image rate to avoid this warning...");
+				UWARN("CameraFreenect: Depth not ready! Try to reduce the image rate to avoid this warning...");
 				return;
 			}
 
 			if(rgb.empty())
 			{
-				UWARN("CameraOpenKinect: Rgb not ready! Try to reduce the image rate to avoid this warning...");
+				UWARN("CameraFreenect: Rgb not ready! Try to reduce the image rate to avoid this warning...");
 				return;
 			}
 
@@ -240,6 +277,7 @@ void CameraFreenect::mainLoop()
 			this->post(new CameraEvent(rgb, depth, constant, localTransform_, ++seq_));
 		}
 	}
+#endif
 }
 
 void CameraFreenect::mainLoopEnd()

corelib/src/toro3d/posegraph.hxx

@@ -63,7 +63,7 @@ template <class Ops>
 typename TreePoseGraph<Ops>::Edge* TreePoseGraph<Ops>::edge(int id1, int id2){
   Vertex* v1=vertex(id1);
   if (!v1)
-    return false;
+    return 0;
   typename EdgeList::iterator it=v1->edges.begin();
   while(it!=v1->edges.end()){
     if ((*it)->v1->id==id1 && (*it)->v2->id==id2)
@@ -117,7 +117,7 @@ typename TreePoseGraph<Ops>::Vertex* TreePoseGraph<Ops>::removeVertex (int id){
   Vertex* v=it->second;
 
   if (v==0)
-    return false;
+    return 0;
 
   typename TreePoseGraph<Ops>::EdgeList el=v->edges;
   for(typename EdgeList::iterator it=el.begin(); it!=el.end(); it++){

corelib/src/toro3d/posegraph3.cpp

@@ -326,7 +326,7 @@ void TreePoseGraph3::collapseEdge(Edge* e){
   Transformation    T12=e->transformation;
   Pose              p12=T12.toPoseType();
 
-  Transformation iT12=T12.inv();
+  //Transformation iT12=T12.inv();
 
   //compute the marginal information of the nodes in the path v1-v2-v*
   for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
@@ -339,7 +339,7 @@ void TreePoseGraph3::collapseEdge(Edge* e){
       
       //compute the estimate of the vertex based on the path v1-v2-vx
       
-      Transformation tr=iT12*T2x;
+      //Transformation tr=iT12*T2x;
       
       CovarianceMatrix CM=C2x;
       

corelib/src/toro3d/treeoptimizer3_iteration.cpp

@@ -98,7 +98,7 @@ void TreeOptimizer3::computePreconditioner(){
       DEBUG(1) << "m";
 
     Edge* e=*it;
-    Transformation t=e->transformation;
+    //Transformation t=e->transformation;
     InformationMatrix W=e->informationMatrix;
     
     Vertex* top=e->top;
@@ -189,8 +189,8 @@ void TreeOptimizer3::propagateErrors(bool usePreconditioner){
     }
 
     //store the transformations relative to the top node
-    Transformation topTransformation=top->transformation;
+    //Transformation topTransformation=top->transformation;
-    Transformation topParameters=top->parameters;
+    //Transformation topParameters=top->parameters;
 
     //END: Path and weight computation
     
@@ -258,7 +258,7 @@ void TreeOptimizer3::propagateErrors(bool usePreconditioner){
     recomputeTransformations(v2,top);
 
     //BEGIN: Translational Error
-    Translation topTranslation=top->transformation.translation();
+    //Translation topTranslation=top->transformation.translation();
    
     Transformation tr12=v1->transformation*e->transformation;
     Translation tR=tr12.translation()-v2->transformation.translation();

guilib/src/MainWindow.cpp

@@ -2012,10 +2012,19 @@ void MainWindow::startDetection()
 
 			if(!_cameraOpenKinect->init())
 			{
-				ULOGGER_WARN("init CameraOpenKinect failed... ");
+				ULOGGER_WARN("init CameraFreenect failed... ");
-				QMessageBox::warning(this,
+				if(!_cameraOpenKinect->available())
-									   tr("RTAB-Map"),
+				{
-									   tr("OpenKinect camera initialization failed..."));
+					QMessageBox::warning(this,
+					   tr("RTAB-Map"),
+					   tr("Freenect camera unavailable! RTAB-Map is not built with Freenect support (libfreenect)."));
+				}
+				else
+				{
+					QMessageBox::warning(this,
+					   tr("RTAB-Map"),
+					   tr("Freenect camera initialization failed!"));
+				}
 				emit stateChanged(kIdle);
 				delete _cameraOpenKinect;
 				_cameraOpenKinect = 0;
@@ -2043,7 +2052,7 @@ void MainWindow::startDetection()
 				ULOGGER_WARN("init CameraOpenni failed... ");
 				QMessageBox::warning(this,
 									   tr("RTAB-Map"),
-									   tr("Openni camera initialization failed..."));
+									   tr("Openni camera initialization failed!"));
 				emit stateChanged(kIdle);
 				delete _cameraOpenni;
 				_cameraOpenni = 0;

guilib/src/PreferencesDialog.cpp

@@ -2708,6 +2708,18 @@ void PreferencesDialog::testOdometry(OdomType type)
 				this->getSourceOpenniLocalTransform());
 		if(!_odomCameraFreenect->init())
 		{
+			if(!_odomCameraFreenect->available())
+			{
+				QMessageBox::warning(this,
+				   tr("RTAB-Map"),
+				   tr("Freenect camera unavailable! RTAB-Map is not built with Freenect support (libfreenect)."));
+			}
+			else
+			{
+				QMessageBox::warning(this,
+				   tr("RTAB-Map"),
+				   tr("Freenect camera initialization failed!"));
+			}
 			delete _odomCameraFreenect;
 			_odomCameraFreenect = 0;
 		}
@@ -2720,6 +2732,9 @@ void PreferencesDialog::testOdometry(OdomType type)
 				this->getSourceOpenniLocalTransform());
 		if(!_odomCameraOpenNI->init())
 		{
+			QMessageBox::warning(this,
+				   tr("RTAB-Map"),
+				   tr("OpenNI camera initialization failed!"));
 			delete _odomCameraOpenNI;
 			_odomCameraOpenNI = 0;
 		}
@@ -2787,10 +2802,6 @@ void PreferencesDialog::testOdometry(OdomType type)
 			_odomCameraOpenNI->start();
 		}
 	}
-	else
-	{
-		QMessageBox::warning(this, "Initialization failed!", tr("RGB-D camera initialization failed!"));
-	}
 }
 
 void PreferencesDialog::cleanOdometryTest()