Explicitly show licenses of optional dependencies on CMake

corelib/include/rtabmap/core/Optimizer.h

@@ -69,12 +69,20 @@ public:
 
 	virtual Type type() const = 0;
 
+	// getters
 	int iterations() const {return iterations_;}
 	bool isSlam2d() const {return slam2d_;}
 	bool isCovarianceIgnored() const {return covarianceIgnored_;}
 	double epsilon() const {return epsilon_;}
 	bool isRobust() const {return robust_;}
 
+	// setters
+	void setIterations(int iterations) {iterations_ = iterations;}
+	void setSlam2d(bool enabled) {slam2d_ = enabled;}
+	void setCovarianceIgnored(bool enabled) {covarianceIgnored_ = enabled;}
+	void setEpsilon(double epsilon) {epsilon_ = epsilon;}
+	void setRobust(bool enabled) {robust_ = enabled;}
+
 	// inherited classes should implement one of these methods
 	virtual std::map<int, Transform> optimize(
 			int rootId,

corelib/include/rtabmap/core/OptimizerTORO.h

@@ -36,6 +36,9 @@ namespace rtabmap {
 
 class RTABMAP_EXP OptimizerTORO : public Optimizer
 {
+public:
+	static bool available();
+
 public:
 	static bool saveGraph(
 			const std::string & fileName,

corelib/include/rtabmap/core/Parameters.h

@@ -220,7 +220,11 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Kp, MaxFeatures,           int, 400, 			"Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
 	RTABMAP_PARAM(Kp, BadSignRatio,          float, 0.5, 		"Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
 	RTABMAP_PARAM(Kp, NndrRatio, 	         float, 0.8,		"NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
-	RTABMAP_PARAM_COND(Kp, DetectorStrategy, int, RTABMAP_NONFREE, 0, 2, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
+#ifdef RTABMAP_NONFREE
+	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 0,            "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
+#else
+	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 2,            "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
+#endif
 	RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed,   bool, true, 		"Use of the td-idf strategy to compute the likelihood.");
 	RTABMAP_PARAM(Kp, Parallelized,          bool, true, 		"If the dictionary update and signature creation were parallelized.");
 	RTABMAP_PARAM_STR(Kp, RoiRatios, "0.0 0.0 0.0 0.0", 		"Region of interest ratios [left, right, top, bottom].");
@@ -303,7 +307,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
 	RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
 	RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
-	RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 1.0,  "Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph.");
+	RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 1.0,  "Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"Optimizer/Robust\" if enabled.");
 	RTABMAP_PARAM(RGBD, GoalReachedRadius,        float, 0.5,  "Goal reached radius (m).");
 	RTABMAP_PARAM(RGBD, PlanStuckIterations,      int, 0,      "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");
 	RTABMAP_PARAM(RGBD, PlanLinearVelocity,       float, 0.0,  "Linear velocity (m/sec) used to compute path weights.");
@@ -325,18 +329,24 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,   "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
 
 	// Graph optimization
-	RTABMAP_PARAM_COND(Optimizer, Strategy,     int, RTABMAP_GTSAM, 2, 0, "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
+#ifdef RTABMAP_GTSAM
+	RTABMAP_PARAM(Optimizer, Strategy,          int, 2,          "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
+#elif RTABMAP_G2O
+	RTABMAP_PARAM(Optimizer, Strategy,          int, 1,          "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
+#else
+	RTABMAP_PARAM(Optimizer, Strategy,          int, 0,          "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
+#endif
 	RTABMAP_PARAM(Optimizer, Iterations,        int, 100,        "Optimization iterations.");
 	RTABMAP_PARAM(Optimizer, Slam2D,            bool, false,     "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
 	RTABMAP_PARAM(Optimizer, VarianceIgnored,   bool, false,     "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
 	RTABMAP_PARAM(Optimizer, Epsilon,           double, 0.0001,  "Stop optimizing when the error improvement is less than this value.");
-	RTABMAP_PARAM(Optimizer, Robust,            bool, false,      "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies).");
+	RTABMAP_PARAM(Optimizer, Robust,            bool, false,      "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"RGBD/OptimizeMaxError\" if enabled.");
 
 	RTABMAP_PARAM(g2o, Solver,                  int, 0,          "0=csparse 1=pcg 2=cholmod");
 	RTABMAP_PARAM(g2o, Optimizer,               int, 0,          "0=Levenberg 1=GaussNewton");
 
 	// Odometry
-	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Local Map 1=Frame-to-Frame");
+	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F)");
 	RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,         "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
 	RTABMAP_PARAM(Odom, Holonomic, 		        bool, true,     "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
 	RTABMAP_PARAM(Odom, FillInfoData, 		    bool, true,     "Fill info with data (inliers/outliers features).");
@@ -376,7 +386,11 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Vis, RefineIterations,         int, 10,       "[Vis/EstimationType = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
 	RTABMAP_PARAM(Vis, PnPReprojError, 	         float, 2.0,    "[Vis/EstimationType = 1] PnP reprojection error.");
 	RTABMAP_PARAM(Vis, PnPFlags,                 int, 1,        "[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
-	RTABMAP_PARAM_COND(Vis, PnPRefineIterations, int, RTABMAP_OPENCV3, 0, 1, "[Vis/EstimationType = 1] Refine iterations.");
+#ifdef RTABMAP_OPENCV3
+	RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 0,        "[Vis/EstimationType = 1] Refine iterations.");
+#else
+	RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        "[Vis/EstimationType = 1] Refine iterations.");
+#endif
 	RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
 	RTABMAP_PARAM(Vis, MinInliers,               int, 10, 		"Minimum feature correspondences to compute/accept the transformation.");
 	RTABMAP_PARAM(Vis, Iterations,               int, 100, 		"Maximum iterations to compute the transform.");

corelib/src/CMakeLists.txt

@@ -64,14 +64,7 @@ SET(SRC_FILES
 	Stereo.cpp
 	StereoDense.cpp
 	StereoCameraModel.cpp
-	
-	toro3d/posegraph3.cpp
-	toro3d/treeoptimizer3_iteration.cpp
-	toro3d/treeoptimizer3.cpp
-	
-	toro3d/posegraph2.cpp
-	toro3d/treeoptimizer2.cpp
-	
+		
 	rtflann/ext/lz4.c
 	rtflann/ext/lz4hc.c
 	
@@ -95,7 +88,6 @@ SET(LIBRARIES
 )
 
 IF(Freenect_FOUND)
-	ADD_DEFINITIONS("-DWITH_FREENECT")
 	IF(Freenect_DASH_INCLUDES)
 	   ADD_DEFINITIONS("-DFREENECT_DASH_INCLUDES")
 	ENDIF(Freenect_DASH_INCLUDES)
@@ -110,7 +102,6 @@ IF(Freenect_FOUND)
 ENDIF(Freenect_FOUND)
 
 IF(OpenNI2_FOUND)
-	ADD_DEFINITIONS("-DWITH_OPENNI2")
 	SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}
 		${OpenNI2_INCLUDE_DIRS}
@@ -122,7 +113,6 @@ IF(OpenNI2_FOUND)
 ENDIF(OpenNI2_FOUND)
 
 IF(freenect2_FOUND)
-	ADD_DEFINITIONS("-DWITH_FREENECT2")
 	SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}
 		${freenect2_INCLUDE_DIRS}
@@ -134,7 +124,6 @@ IF(freenect2_FOUND)
 ENDIF(freenect2_FOUND)
 
 IF(DC1394_FOUND)
-	ADD_DEFINITIONS("-DWITH_DC1394")
 	SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}
 		${DC1394_INCLUDE_DIRS}
@@ -146,7 +135,6 @@ IF(DC1394_FOUND)
 ENDIF(DC1394_FOUND)
 
 IF(FlyCapture2_FOUND)
-	ADD_DEFINITIONS("-DWITH_FLYCAPTURE2")
 	SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}
 		${FlyCapture2_INCLUDE_DIRS}
@@ -157,8 +145,23 @@ IF(FlyCapture2_FOUND)
 	)
 ENDIF(FlyCapture2_FOUND)
 
+IF(WITH_TORO)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${G2O_LIBRARIES}
+	)
+	SET(SRC_FILES
+    	${SRC_FILES}
+		toro3d/posegraph3.cpp
+		toro3d/treeoptimizer3_iteration.cpp
+		toro3d/treeoptimizer3.cpp
+	
+		toro3d/posegraph2.cpp
+		toro3d/treeoptimizer2.cpp
+	)
+ENDIF(WITH_TORO)
+
 IF(G2O_FOUND)
-	ADD_DEFINITIONS("-DWITH_G2O")
 	SET(INCLUDE_DIRS 
 		${INCLUDE_DIRS} 
 		${G2O_INCLUDE_DIRS}
@@ -168,18 +171,19 @@ IF(G2O_FOUND)
 		${G2O_LIBRARIES}
 	)
 	
-	SET(SRC_FILES
-    	${SRC_FILES}
-		vertigo/g2o/edge_se2Switchable.cpp
-		vertigo/g2o/edge_se3Switchable.cpp
-		vertigo/g2o/edge_switchPrior.cpp
-		vertigo/g2o/types_g2o_robust.cpp
-		vertigo/g2o/vertex_switchLinear.cpp
-	)
+	IF(WITH_VERTIGO)
+		SET(SRC_FILES
+    		${SRC_FILES}
+			vertigo/g2o/edge_se2Switchable.cpp
+			vertigo/g2o/edge_se3Switchable.cpp
+			vertigo/g2o/edge_switchPrior.cpp
+			vertigo/g2o/types_g2o_robust.cpp
+			vertigo/g2o/vertex_switchLinear.cpp
+		)
+	ENDIF(WITH_VERTIGO)
 ENDIF(G2O_FOUND)
 
 IF(GTSAM_FOUND)
-	ADD_DEFINITIONS("-DWITH_GTSAM")
 	SET(INCLUDE_DIRS 
 		${INCLUDE_DIRS} 
 		${GTSAM_INCLUDE_DIRS}
@@ -191,7 +195,6 @@ IF(GTSAM_FOUND)
 ENDIF(GTSAM_FOUND)
 
 IF(cvsba_FOUND)
-	ADD_DEFINITIONS("-DWITH_CVSBA")
 	SET(INCLUDE_DIRS 
 		${INCLUDE_DIRS} 
 		${cvsba_INCLUDE_DIRS}

corelib/src/CameraRGBD.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util2d.h"
 #include "rtabmap/core/CameraRGB.h"
 #include "rtabmap/core/Graph.h"
+#include "rtabmap/core/Version.h"
 
 #include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -46,7 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/io/openni_camera/openni_image.h>
 #endif
 
-#ifdef WITH_FREENECT
+#ifdef RTABMAP_FREENECT
 #include <libfreenect.h>
 #ifdef FREENECT_DASH_INCLUDES
 #include <libfreenect-registration.h>
@@ -55,7 +56,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #endif
 #endif
 
-#ifdef WITH_FREENECT2
+#ifdef RTABMAP_FREENECT2
 #include <libfreenect2/libfreenect2.hpp>
 #include <libfreenect2/frame_listener_impl.h>
 #include <libfreenect2/registration.h>
@@ -63,20 +64,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <libfreenect2/config.h>
 #endif
 
-#ifdef WITH_DC1394
-#include <dc1394/dc1394.h>
-#endif
-
-#ifdef WITH_OPENNI2
+#ifdef RTABMAP_OPENNI2
 #include <OniVersion.h>
 #include <OpenNI.h>
 #endif
 
-#ifdef WITH_FLYCAPTURE2
-#include <triclops.h>
-#include <fc2triclops.h>
-#endif
-
 namespace rtabmap
 {
 
@@ -361,7 +353,7 @@ SensorData CameraOpenNICV::captureImage()
 /////////////////////////
 bool CameraOpenNI2::available()
 {
-#ifdef WITH_OPENNI2
+#ifdef RTABMAP_OPENNI2
 	return true;
 #else
 	return false;
@@ -382,7 +374,7 @@ CameraOpenNI2::CameraOpenNI2(
 		float imageRate,
 		const rtabmap::Transform & localTransform) :
 	Camera(imageRate, localTransform),
-#ifdef WITH_OPENNI2
+#ifdef RTABMAP_OPENNI2
 	_device(new openni::Device()),
 	_color(new openni::VideoStream()),
 	_depth(new openni::VideoStream()),
@@ -400,7 +392,7 @@ CameraOpenNI2::CameraOpenNI2(
 
 CameraOpenNI2::~CameraOpenNI2()
 {
-#ifdef WITH_OPENNI2
+#ifdef RTABMAP_OPENNI2
 	_color->stop();
 	_color->destroy();
 	_depth->stop();
@@ -416,7 +408,7 @@ CameraOpenNI2::~CameraOpenNI2()
 
 bool CameraOpenNI2::setAutoWhiteBalance(bool enabled)
 {
-#ifdef WITH_OPENNI2
+#ifdef RTABMAP_OPENNI2
 	if(_color && _color->getCameraSettings())
 	{
 		return _color->getCameraSettings()->setAutoWhiteBalanceEnabled(enabled) == openni::STATUS_OK;
@@ -429,7 +421,7 @@ bool CameraOpenNI2::setAutoWhiteBalance(bool enabled)
 
 bool CameraOpenNI2::setAutoExposure(bool enabled)
 {
-#ifdef WITH_OPENNI2
+#ifdef RTABMAP_OPENNI2
 	if(_color && _color->getCameraSettings())
 	{
 		return _color->getCameraSettings()->setAutoExposureEnabled(enabled) == openni::STATUS_OK;
@@ -442,7 +434,7 @@ bool CameraOpenNI2::setAutoExposure(bool enabled)
 
 bool CameraOpenNI2::setExposure(int value)
 {
-#ifdef WITH_OPENNI2
+#ifdef RTABMAP_OPENNI2
 #if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
 	if(_color && _color->getCameraSettings())
 	{
@@ -459,7 +451,7 @@ bool CameraOpenNI2::setExposure(int value)
 
 bool CameraOpenNI2::setGain(int value)
 {
-#ifdef WITH_OPENNI2
+#ifdef RTABMAP_OPENNI2
 #if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
 	if(_color && _color->getCameraSettings())
 	{
@@ -476,7 +468,7 @@ bool CameraOpenNI2::setGain(int value)
 
 bool CameraOpenNI2::setMirroring(bool enabled)
 {
-#ifdef WITH_OPENNI2
+#ifdef RTABMAP_OPENNI2
 	if(_color->isValid() && _depth->isValid())
 	{
 		return _depth->setMirroringEnabled(enabled) == openni::STATUS_OK &&
@@ -488,7 +480,7 @@ bool CameraOpenNI2::setMirroring(bool enabled)
 
 bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
-#ifdef WITH_OPENNI2
+#ifdef RTABMAP_OPENNI2
 	openni::OpenNI::initialize();
 
 	if(_device->open(_deviceId.empty()?openni::ANY_DEVICE:_deviceId.c_str()) != openni::STATUS_OK)
@@ -666,7 +658,7 @@ bool CameraOpenNI2::isCalibrated() const
 
 std::string CameraOpenNI2::getSerial() const
 {
-#ifdef WITH_OPENNI2
+#ifdef RTABMAP_OPENNI2
 	if(_device)
 	{
 		return _device->getDeviceInfo().getName();
@@ -678,7 +670,7 @@ std::string CameraOpenNI2::getSerial() const
 SensorData CameraOpenNI2::captureImage()
 {
 	SensorData data;
-#ifdef WITH_OPENNI2
+#ifdef RTABMAP_OPENNI2
 	int readyStream = -1;
 	if(_device->isValid() &&
 		_depth->isValid() &&
@@ -740,7 +732,7 @@ SensorData CameraOpenNI2::captureImage()
 	return data;
 }
 
-#ifdef WITH_FREENECT
+#ifdef RTABMAP_FREENECT
 //
 // FreenectDevice
 //
@@ -947,7 +939,7 @@ private:
 //
 bool CameraFreenect::available()
 {
-#ifdef WITH_FREENECT
+#ifdef RTABMAP_FREENECT
 	return true;
 #else
 	return false;
@@ -960,7 +952,7 @@ CameraFreenect::CameraFreenect(int deviceId, float imageRate, const Transform &
 		ctx_(0),
 		freenectDevice_(0)
 {
-#ifdef WITH_FREENECT
+#ifdef RTABMAP_FREENECT
 	if(freenect_init(&ctx_, NULL) < 0) UERROR("Cannot initialize freenect library");
 	// claim camera
 	freenect_select_subdevices(ctx_, static_cast<freenect_device_flags>(FREENECT_DEVICE_CAMERA));
@@ -969,7 +961,7 @@ CameraFreenect::CameraFreenect(int deviceId, float imageRate, const Transform &
 
 CameraFreenect::~CameraFreenect()
 {
-#ifdef WITH_FREENECT
+#ifdef RTABMAP_FREENECT
 	if(freenectDevice_)
 	{
 		freenectDevice_->join(true);
@@ -985,7 +977,7 @@ CameraFreenect::~CameraFreenect()
 
 bool CameraFreenect::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
-#ifdef WITH_FREENECT
+#ifdef RTABMAP_FREENECT
 	if(freenectDevice_)
 	{
 		freenectDevice_->join(true);
@@ -1026,7 +1018,7 @@ bool CameraFreenect::isCalibrated() const
 
 std::string CameraFreenect::getSerial() const
 {
-#ifdef WITH_FREENECT
+#ifdef RTABMAP_FREENECT
 	if(freenectDevice_)
 	{
 		return freenectDevice_->getSerial();
@@ -1038,7 +1030,7 @@ std::string CameraFreenect::getSerial() const
 SensorData CameraFreenect::captureImage()
 {
 	SensorData data;
-#ifdef WITH_FREENECT
+#ifdef RTABMAP_FREENECT
 	if(ctx_ && freenectDevice_)
 	{
 		if(freenectDevice_->isRunning())
@@ -1078,7 +1070,7 @@ SensorData CameraFreenect::captureImage()
 //
 bool CameraFreenect2::available()
 {
-#ifdef WITH_FREENECT2
+#ifdef RTABMAP_FREENECT2
 	return true;
 #else
 	return false;
@@ -1108,7 +1100,7 @@ CameraFreenect2::CameraFreenect2(
 		edgeAwareFiltering_(edgeAwareFiltering),
 		noiseFiltering_(noiseFiltering)
 {
-#ifdef WITH_FREENECT2
+#ifdef RTABMAP_FREENECT2
 	UASSERT(minKinect2Depth_ < maxKinect2Depth_ && minKinect2Depth_>0 && maxKinect2Depth_>0 && maxKinect2Depth_<=65.535f);
 	freenect2_ = new libfreenect2::Freenect2();
 	switch(type_)
@@ -1131,7 +1123,7 @@ CameraFreenect2::CameraFreenect2(
 
 CameraFreenect2::~CameraFreenect2()
 {
-#ifdef WITH_FREENECT2
+#ifdef RTABMAP_FREENECT2
 	UDEBUG("");
 	if(dev_)
 	{
@@ -1160,7 +1152,7 @@ CameraFreenect2::~CameraFreenect2()
 
 bool CameraFreenect2::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
-#ifdef WITH_FREENECT2
+#ifdef RTABMAP_FREENECT2
 	if(dev_)
 	{
 		dev_->stop();
@@ -1298,7 +1290,7 @@ bool CameraFreenect2::isCalibrated() const
 
 std::string CameraFreenect2::getSerial() const
 {
-#ifdef WITH_FREENECT2
+#ifdef RTABMAP_FREENECT2
 	if(dev_)
 	{
 		return dev_->getSerialNumber();
@@ -1310,7 +1302,7 @@ std::string CameraFreenect2::getSerial() const
 SensorData CameraFreenect2::captureImage()
 {
 	SensorData data;
-#ifdef WITH_FREENECT2
+#ifdef RTABMAP_FREENECT2
 	if(dev_ && listener_)
 	{
 		libfreenect2::FrameMap frames;

corelib/src/CameraStereo.cpp

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/CameraStereo.h"
 #include "rtabmap/core/util2d.h"
 #include "rtabmap/core/CameraRGB.h"
+#include "rtabmap/core/Version.h"
 
 #include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -38,11 +39,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UMath.h>
 
-#ifdef WITH_DC1394
+#ifdef RTABMAP_DC1394
 #include <dc1394/dc1394.h>
 #endif
 
-#ifdef WITH_FLYCAPTURE2
+#ifdef RTABMAP_FLYCAPTURE2
 #include <triclops.h>
 #include <fc2triclops.h>
 #endif
@@ -55,7 +56,7 @@ namespace rtabmap
 // Inspired from ROS camera1394stereo package
 //
 
-#ifdef WITH_DC1394
+#ifdef RTABMAP_DC1394
 class DC1394Device
 {
 public:
@@ -334,7 +335,7 @@ private:
 
 bool CameraStereoDC1394::available()
 {
-#ifdef WITH_DC1394
+#ifdef RTABMAP_DC1394
 	return true;
 #else
 	return false;
@@ -345,14 +346,14 @@ CameraStereoDC1394::CameraStereoDC1394(float imageRate, const Transform & localT
 		Camera(imageRate, localTransform),
 		device_(0)
 {
-#ifdef WITH_DC1394
+#ifdef RTABMAP_DC1394
 	device_ = new DC1394Device();
 #endif
 }
 
 CameraStereoDC1394::~CameraStereoDC1394()
 {
-#ifdef WITH_DC1394
+#ifdef RTABMAP_DC1394
 	if(device_)
 	{
 		delete device_;
@@ -362,7 +363,7 @@ CameraStereoDC1394::~CameraStereoDC1394()
 
 bool CameraStereoDC1394::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
-#ifdef WITH_DC1394
+#ifdef RTABMAP_DC1394
 	if(device_)
 	{
 		bool ok = device_->init();
@@ -401,7 +402,7 @@ bool CameraStereoDC1394::isCalibrated() const
 
 std::string CameraStereoDC1394::getSerial() const
 {
-#ifdef WITH_DC1394
+#ifdef RTABMAP_DC1394
 	if(device_)
 	{
 		return device_->guid();
@@ -413,7 +414,7 @@ std::string CameraStereoDC1394::getSerial() const
 SensorData CameraStereoDC1394::captureImage()
 {
 	SensorData data;
-#ifdef WITH_DC1394
+#ifdef RTABMAP_DC1394
 	if(device_)
 	{
 		cv::Mat left, right;
@@ -458,14 +459,14 @@ CameraStereoFlyCapture2::CameraStereoFlyCapture2(float imageRate, const Transfor
 		camera_(0),
 		triclopsCtx_(0)
 {
-#ifdef WITH_FLYCAPTURE2
+#ifdef RTABMAP_FLYCAPTURE2
 	camera_ = new FlyCapture2::Camera();
 #endif
 }
 
 CameraStereoFlyCapture2::~CameraStereoFlyCapture2()
 {
-#ifdef WITH_FLYCAPTURE2
+#ifdef RTABMAP_FLYCAPTURE2
 	// Close the camera
 	camera_->StopCapture();
 	camera_->Disconnect();
@@ -479,7 +480,7 @@ CameraStereoFlyCapture2::~CameraStereoFlyCapture2()
 
 bool CameraStereoFlyCapture2::available()
 {
-#ifdef WITH_FLYCAPTURE2
+#ifdef RTABMAP_FLYCAPTURE2
 	return true;
 #else
 	return false;
@@ -488,7 +489,7 @@ bool CameraStereoFlyCapture2::available()
 
 bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
-#ifdef WITH_FLYCAPTURE2
+#ifdef RTABMAP_FLYCAPTURE2
 	if(camera_)
 	{
 		// Close the camera
@@ -567,7 +568,7 @@ bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder, const
 
 bool CameraStereoFlyCapture2::isCalibrated() const
 {
-#ifdef WITH_FLYCAPTURE2
+#ifdef RTABMAP_FLYCAPTURE2
 	if(triclopsCtx_)
 	{
 		float fx, cx, cy, baseline;
@@ -582,7 +583,7 @@ bool CameraStereoFlyCapture2::isCalibrated() const
 
 std::string CameraStereoFlyCapture2::getSerial() const
 {
-#ifdef WITH_FLYCAPTURE2
+#ifdef RTABMAP_FLYCAPTURE2
 	if(camera_ && camera_->IsConnected())
 	{
 		FlyCapture2::CameraInfo camInfo;
@@ -596,7 +597,7 @@ std::string CameraStereoFlyCapture2::getSerial() const
 }
 
 // struct containing image needed for processing
-#ifdef WITH_FLYCAPTURE2
+#ifdef RTABMAP_FLYCAPTURE2
 struct ImageContainer
 {
 	FlyCapture2::Image tmp[2];
@@ -607,7 +608,7 @@ struct ImageContainer
 SensorData CameraStereoFlyCapture2::captureImage()
 {
 	SensorData data;
-#ifdef WITH_FLYCAPTURE2
+#ifdef RTABMAP_FLYCAPTURE2
 	if(camera_ && triclopsCtx_ && camera_->IsConnected())
 	{
 		// grab image from camera.

corelib/src/Features2d.cpp

@@ -419,28 +419,27 @@ Feature2D * Feature2D::create(const ParametersMap & parameters)
 }
 Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parameters)
 {
-	if(RTABMAP_NONFREE == 0)
+#ifndef RTABMAP_NONFREE
+	if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift)
 	{
-		if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift)
-		{
 #if CV_MAJOR_VERSION < 3
-			UWARN("SURF/SIFT features cannot be used because OpenCV was not built with nonfree module. ORB is used instead.");
+		UWARN("SURF/SIFT features cannot be used because OpenCV was not built with nonfree module. ORB is used instead.");
 #else
-			UWARN("SURF/SIFT features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead.");
-#endif
-			type = Feature2D::kFeatureOrb;
-		}
-#if CV_MAJOR_VERSION == 3
-		if(type == Feature2D::kFeatureFastBrief ||
-		   type == Feature2D::kFeatureFastFreak ||
-		   type == Feature2D::kFeatureGfttBrief ||
-		   type == Feature2D::kFeatureGfttFreak)
-		{
-			UWARN("BRIEF/FREAK features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead.");
-			type = Feature2D::kFeatureOrb;
-		}
+		UWARN("SURF/SIFT features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead.");
 #endif
+		type = Feature2D::kFeatureOrb;
 	}
+#if CV_MAJOR_VERSION == 3
+	if(type == Feature2D::kFeatureFastBrief ||
+	   type == Feature2D::kFeatureFastFreak ||
+	   type == Feature2D::kFeatureGfttBrief ||
+	   type == Feature2D::kFeatureGfttFreak)
+	{
+		UWARN("BRIEF/FREAK features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead.");
+		type = Feature2D::kFeatureOrb;
+	}
+#endif
+#endif
 
 	Feature2D * feature2D = 0;
 	switch(type)
@@ -475,7 +474,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	case Feature2D::kFeatureBrisk:
 		feature2D = new BRISK(parameters);
 		break;
-#if RTABMAP_NONFREE == 1
+#ifdef RTABMAP_NONFREE
 	default:
 		feature2D = new SURF(parameters);
 		type = Feature2D::kFeatureSurf;
@@ -697,7 +696,7 @@ void SURF::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kSURFGpuKeypointsRatio(), gpuKeypointsRatio_);
 	Parameters::parse(parameters, Parameters::kSURFGpuVersion(), gpuVersion_);
 
-#if RTABMAP_NONFREE == 1
+#ifdef RTABMAP_NONFREE
 #if CV_MAJOR_VERSION < 3
 	if(gpuVersion_ && cv::gpu::getCudaEnabledDeviceCount() == 0)
 	{
@@ -733,7 +732,7 @@ std::vector<cv::KeyPoint> SURF::generateKeypointsImpl(const cv::Mat & image, con
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
 
-#if RTABMAP_NONFREE == 1
+#ifdef RTABMAP_NONFREE
 	cv::Mat imgRoi(image, roi);
 	cv::Mat maskRoi;
 	if(!mask.empty())
@@ -766,7 +765,7 @@ cv::Mat SURF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
-#if RTABMAP_NONFREE == 1
+#ifdef RTABMAP_NONFREE
 	if(gpuVersion_)
 	{
 #if CV_MAJOR_VERSION < 3
@@ -825,7 +824,7 @@ void SIFT::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), nOctaveLayers_);
 	Parameters::parse(parameters, Parameters::kSIFTSigma(), sigma_);
 
-#if RTABMAP_NONFREE == 1
+#ifdef RTABMAP_NONFREE
 #if CV_MAJOR_VERSION < 3
 	_sift = cv::Ptr<CV_SIFT>(new CV_SIFT(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_));
 #else
@@ -840,7 +839,7 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
-#if RTABMAP_NONFREE == 1
+#ifdef RTABMAP_NONFREE
 	cv::Mat imgRoi(image, roi);
 	cv::Mat maskRoi;
 	if(!mask.empty())
@@ -858,7 +857,7 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
-#if RTABMAP_NONFREE == 1
+#ifdef RTABMAP_NONFREE
 	_sift->compute(image, keypoints, descriptors);
 #else
 	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");

corelib/src/Optimizer.cpp

@@ -57,7 +57,7 @@ bool Optimizer::isAvailable(Optimizer::Type type)
 	}
 	else if(type == Optimizer::kTypeTORO)
 	{
-		return true;
+		return OptimizerTORO::available();
 	}
 	return false;
 }
@@ -72,20 +72,65 @@ Optimizer * Optimizer::create(const ParametersMap & parameters)
 
 Optimizer * Optimizer::create(Optimizer::Type & type, const ParametersMap & parameters)
 {
+	UASSERT_MSG(OptimizerG2O::available() || OptimizerGTSAM::available() || OptimizerTORO::available(),
+			"RTAB-Map is not built with any graph optimization approach!");
+
+	if(!OptimizerTORO::available() && type == Optimizer::kTypeTORO)
+	{
+		if(OptimizerGTSAM::available())
+		{
+			UWARN("TORO optimizer not available. GTSAM will be used instead.");
+					type = Optimizer::kTypeGTSAM;
+		}
+		else if(OptimizerG2O::available())
+		{
+			UWARN("TORO optimizer not available. g2o will be used instead.");
+					type = Optimizer::kTypeG2O;
+		}
+	}
 	if(!OptimizerG2O::available() && type == Optimizer::kTypeG2O)
 	{
-		UWARN("g2o optimizer not available. TORO will be used instead.");
-		type = Optimizer::kTypeTORO;
+		if(OptimizerTORO::available())
+		{
+			UWARN("g2o optimizer not available. TORO will be used instead.");
+					type = Optimizer::kTypeTORO;
+		}
+		else if(OptimizerGTSAM::available())
+		{
+			UWARN("g2o optimizer not available. GTSAM will be used instead.");
+					type = Optimizer::kTypeGTSAM;
+		}
 	}
 	if(!OptimizerGTSAM::available() && type == Optimizer::kTypeGTSAM)
 	{
-		UWARN("GTSAM optimizer not available. TORO will be used instead.");
-		type = Optimizer::kTypeTORO;
+		if(OptimizerTORO::available())
+		{
+			UWARN("GTSAM optimizer not available. TORO will be used instead.");
+					type = Optimizer::kTypeTORO;
+		}
+		else if(OptimizerG2O::available())
+		{
+			UWARN("GTSAM optimizer not available. g2o will be used instead.");
+					type = Optimizer::kTypeG2O;
+		}
 	}
 	if(!OptimizerCVSBA::available() && type == Optimizer::kTypeCVSBA)
 	{
-		UWARN("CVSBA optimizer not available. TORO will be used instead.");
-		type = Optimizer::kTypeTORO;
+		if(OptimizerTORO::available())
+		{
+			UWARN("CVSBA optimizer not available. TORO will be used instead.");
+					type = Optimizer::kTypeTORO;
+		}
+		else if(OptimizerGTSAM::available())
+		{
+			UWARN("CVSBA optimizer not available. GTSAM will be used instead.");
+					type = Optimizer::kTypeGTSAM;
+		}
+		else if(OptimizerG2O::available())
+		{
+			UWARN("CVSBA optimizer not available. g2o will be used instead.");
+					type = Optimizer::kTypeG2O;
+		}
 	}
 	Optimizer * optimizer = 0;
 	switch(type)

corelib/src/OptimizerCVSBA.cpp

@@ -35,7 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/OptimizerCVSBA.h>
 
-#ifdef WITH_CVSBA
+#ifdef RTABMAP_CVSBA
 #include <cvsba/cvsba.h>
 #include "rtabmap/core/util3d_motion_estimation.h"
 #include "rtabmap/core/util3d_transforms.h"
@@ -46,7 +46,7 @@ namespace rtabmap {
 
 bool OptimizerCVSBA::available()
 {
-#ifdef WITH_CVSBA
+#ifdef RTABMAP_CVSBA
 	return true;
 #else
 	return false;
@@ -59,7 +59,7 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
 		const std::multimap<int, Link> & links,
 		const std::map<int, Signature> & signatures)
 {
-#ifdef WITH_CVSBA
+#ifdef RTABMAP_CVSBA
 	// run sba optimization
 	cvsba::Sba sba;
 

corelib/src/OptimizerG2O.cpp

@@ -34,7 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/OptimizerG2O.h>
 
-#ifdef WITH_G2O
+#ifdef RTABMAP_G2O
 #include "g2o/config.h"
 #include "g2o/core/sparse_optimizer.h"
 #include "g2o/core/block_solver.h"
@@ -63,18 +63,20 @@ typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSpa
 typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> SlamLinearCholmodSolver;
 #endif
 
+#ifdef RTABMAP_VERTIGO
 #include "vertigo/g2o/edge_switchPrior.h"
 #include "vertigo/g2o/edge_se2Switchable.h"
 #include "vertigo/g2o/edge_se3Switchable.h"
 #include "vertigo/g2o/vertex_switchLinear.h"
+#endif
 
-#endif // end WITH_G2O
+#endif // end RTABMAP_G2O
 
 namespace rtabmap {
 
 bool OptimizerG2O::available()
 {
-#ifdef WITH_G2O
+#ifdef RTABMAP_G2O
 	return true;
 #else
 	return false;
@@ -132,8 +134,17 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		int * iterationsDone)
 {
 	std::map<int, Transform> optimizedPoses;
-#ifdef WITH_G2O
+#ifdef RTABMAP_G2O
 	UDEBUG("Optimizing graph...");
+
+#ifndef RTABMAP_VERTIGO
+	if(this->isRobust())
+	{
+		UWARN("Vertigo robust optimization is not available! Robust optimization is now disabled.");
+		setRobust(false);
+	}
+#endif
+
 	optimizedPoses.clear();
 	if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
 	{
@@ -226,6 +237,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 
 			g2o::HyperGraph::Edge * edge = 0;
 
+#ifdef RTABMAP_VERTIGO
 			VertexSwitchLinear * v = 0;
 			if(this->isRobust() &&
 			   iter->second.type() != Link::kNeighbor &&
@@ -253,6 +265,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 				prior->setVertex(0, v);
 				UASSERT_MSG(optimizer.addEdge(prior), uFormat("cannot insert switchable prior edge %d!?", v->id()).c_str());
 			}
+#endif
 
 			if(isSlam2d())
 			{
@@ -270,6 +283,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 					information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
 				}
 
+#ifdef RTABMAP_VERTIGO
 				if(this->isRobust() &&
 				   iter->second.type() != Link::kNeighbor  &&
 				   iter->second.type() != Link::kNeighborMerged)
@@ -287,6 +301,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 					edge = e;
 				}
 				else
+#endif
 				{
 					g2o::EdgeSE2 * e = new g2o::EdgeSE2();
 					g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
@@ -313,6 +328,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 				constraint = a.rotation();
 				constraint.translation() = a.translation();
 
+#ifdef RTABMAP_VERTIGO
 				if(this->isRobust() &&
 				   iter->second.type() != Link::kNeighbor &&
 				   iter->second.type() != Link::kNeighborMerged)
@@ -330,6 +346,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 					edge = e;
 				}
 				else
+#endif
 				{
 					g2o::EdgeSE3 * e = new g2o::EdgeSE3();
 					g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);

corelib/src/OptimizerGTSAM.cpp

@@ -35,7 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/OptimizerGTSAM.h>
 
-#ifdef WITH_GTSAM
+#ifdef RTABMAP_GTSAM
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/inference/Key.h>
@@ -50,17 +50,19 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/nonlinear/Values.h>
 
+#ifdef RTABMAP_VERTIGO
 #include "vertigo/gtsam/betweenFactorMaxMix.h"
 #include "vertigo/gtsam/betweenFactorSwitchable.h"
 #include "vertigo/gtsam/switchVariableLinear.h"
 #include "vertigo/gtsam/switchVariableSigmoid.h"
-#endif // end WITH_GTSAM
+#endif
+#endif // end RTABMAP_GTSAM
 
 namespace rtabmap {
 
 bool OptimizerGTSAM::available()
 {
-#ifdef WITH_GTSAM
+#ifdef RTABMAP_GTSAM
 	return true;
 #else
 	return false;
@@ -76,7 +78,16 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		int * iterationsDone)
 {
 	std::map<int, Transform> optimizedPoses;
-#ifdef WITH_GTSAM
+#ifdef RTABMAP_GTSAM
+
+#ifndef RTABMAP_VERTIGO
+	if(this->isRobust())
+	{
+		UWARN("Vertigo robust optimization is not available! Robust optimization is now disabled.");
+		setRobust(false);
+	}
+#endif
+
 	UDEBUG("Optimizing graph...");
 	if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
 	{
@@ -120,6 +131,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 
 			UASSERT(!iter->second.transform().isNull());
 
+#ifdef RTABMAP_VERTIGO
 			if(this->isRobust() &&
 			   iter->second.type()!=Link::kNeighbor &&
 			   iter->second.type() != Link::kNeighborMerged)
@@ -140,6 +152,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0));
 				graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel));
 			}
+#endif
 
 			if(isSlam2d())
 			{
@@ -159,6 +172,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				}
 				gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
 
+#ifdef RTABMAP_VERTIGO
 				if(this->isRobust() &&
 				   iter->second.type()!=Link::kNeighbor &&
 				   iter->second.type() != Link::kNeighborMerged)
@@ -167,6 +181,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose2>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
 				}
 				else
+#endif
 				{
 					graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
 				}
@@ -183,6 +198,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 
 				gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
 
+#ifdef RTABMAP_VERTIGO
 				if(this->isRobust() &&
 				   iter->second.type()!=Link::kNeighbor &&
 				   iter->second.type() != Link::kNeighborMerged)
@@ -191,6 +207,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));
 				}
 				else
+#endif
 				{
 					graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
 				}

corelib/src/OptimizerTORO.cpp

@@ -35,11 +35,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/OptimizerTORO.h>
 
+#ifdef RTABMAP_TORO
 #include "toro3d/treeoptimizer3.hh"
 #include "toro3d/treeoptimizer2.hh"
+#endif
 
 namespace rtabmap {
 
+bool OptimizerTORO::available()
+{
+#ifdef RTABMAP_TORO
+	return true;
+#else
+	return false;
+#endif
+}
+
 std::map<int, Transform> OptimizerTORO::optimize(
 		int rootId,
 		const std::map<int, Transform> & poses,
@@ -49,6 +60,7 @@ std::map<int, Transform> OptimizerTORO::optimize(
 		int * iterationsDone)
 {
 	std::map<int, Transform> optimizedPoses;
+#ifdef RTABMAP_TORO
 	UDEBUG("Optimizing graph (pose=%d constraints=%d)...", (int)poses.size(), (int)edgeConstraints.size());
 	if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
 	{
@@ -302,6 +314,9 @@ std::map<int, Transform> OptimizerTORO::optimize(
 		UWARN("This method should be called at least with 1 pose!");
 	}
 	UDEBUG("Optimizing graph...end!");
+#else
+	UERROR("Not built with TORO support!");
+#endif
 	return optimizedPoses;
 }
 

corelib/src/toro3d/treeoptimizer2.cpp

@@ -266,8 +266,8 @@ void TreeOptimizer2::propagateErrors(){
       updatePoseChain(v1,top);
       updatePoseChain(v2,top);
 
-      Pose pf1=v1->pose;
-      Pose pf2=v2->pose;
+      //Pose pf1=v1->pose;
+      //Pose pf2=v2->pose;
       
       //DEBUG(2) << " pf1=" << pf1.x() << " " << pf1.y() << " " << pf1.theta() << endl;
       //DEBUG(2) << " pf2=" << pf2.x() << " " << pf2.y() << " " << pf2.theta() << endl;