Explicitly show licenses of optional dependencies on CMake

2026-10-03 16:27:46 +08:00 · 2016-03-01 14:25:00 -05:00
parent e6e9c61c38
commit 1f999c4d28
20 changed files with 458 additions and 228 deletions
@@ -134,6 +134,8 @@ option(WITH_OPENNI2       "Include OpenNI2 support"              ON)
 option(WITH_DC1394        "Include dc1394 support"               ON)
 option(WITH_G2O           "Include g2o support"                  ON)
 option(WITH_GTSAM         "Include GTSAM support"                ON)
+option(WITH_TORO          "Include TORO support"                 ON)
+option(WITH_VERTIGO       "Include Vertigo support"              ON)
 option(WITH_CVSBA         "Include cvsba support"                ON)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)

@@ -280,22 +282,42 @@ ENDIF(APPLE AND BUILD_AS_BUNDLE)


 ####### SOURCES (Projects) #######
-SET(NONFREE 0)
-SET(G2O 0)
-SET(GTSAM 0)
-SET(OPENCV3 0)
-IF(OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND)
-   SET(NONFREE 1)
-ENDIF(OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND)
-IF(G2O_FOUND)
-   SET(G2O 1)
-ENDIF(G2O_FOUND)
-IF(GTSAM_FOUND)
-   SET(GTSAM 1)
-ENDIF(GTSAM_FOUND)
-IF(OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3)
-   SET(OPENCV3 1)
-ENDIF(OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3)
+IF(NOT (OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND))
+   SET(NONFREE "//")
+ENDIF(NOT (OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND))
+IF(NOT G2O_FOUND)
+   SET(G2O "//")
+ENDIF(NOT G2O_FOUND)
+IF(NOT GTSAM_FOUND)
+   SET(GTSAM "//")
+ENDIF(NOT GTSAM_FOUND)
+IF(NOT WITH_TORO)
+   SET(TORO "//")
+ENDIF(NOT WITH_TORO)
+IF(NOT WITH_VERTIGO)
+   SET(VERTIGO "//")
+ENDIF(NOT WITH_VERTIGO)
+IF(NOT cvsba_FOUND)
+   SET(CVSBA "//")
+ENDIF(NOT cvsba_FOUND)
+IF(NOT Freenect_FOUND)
+   SET(FREENECT "//")
+ENDIF(NOT Freenect_FOUND)
+IF(NOT freenect2_FOUND)
+   SET(FREENECT2 "//")
+ENDIF(NOT freenect2_FOUND)
+IF(NOT OpenNI2_FOUND)
+   SET(OPENNI2 "//")
+ENDIF(NOT OpenNI2_FOUND)
+IF(NOT DC1394_FOUND)
+   SET(DC1394 "//")
+ENDIF(NOT DC1394_FOUND)
+IF(NOT FlyCapture2_FOUND)
+   SET(FLYCAPTURE2 "//")
+ENDIF(NOT FlyCapture2_FOUND)
+IF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
+   SET(OPENCV3 "//")
+ENDIF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
 CONFIGURE_FILE(Version.h.in ${PROJECT_SOURCE_DIR}/corelib/include/${PROJECT_PREFIX}/core/Version.h)

 ADD_SUBDIRECTORY( utilite )
@@ -472,21 +494,21 @@ MESSAGE(STATUS "  CMAKE_CXX_FLAGS = ${CMAKE_CXX_FLAGS}")
 IF(OpenCV_FOUND)
  IF(OpenCV_VERSION_MAJOR EQUAL 2)
    IF(OPENCV_NONFREE_FOUND)
-      MESSAGE(STATUS "  With OpenCV 2 nonfree module (SIFT/SURF) = YES")
+      MESSAGE(STATUS "  With OpenCV 2 nonfree module (SIFT/SURF) = YES (License: Non commercial)")
    ELSE()
-      MESSAGE(STATUS "  With OpenCV 2 nonfree module (SIFT/SURF) = NO (not found)")
+      MESSAGE(STATUS "  With OpenCV 2 nonfree module (SIFT/SURF) = NO (not found, License: BSD)")
    ENDIF()
  ELSE()
    IF(OPENCV_XFEATURES2D_FOUND)
-      MESSAGE(STATUS "  With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = YES")
+      MESSAGE(STATUS "  With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = YES (License: Non commercial)")
    ELSE()
-      MESSAGE(STATUS "  With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = NO (not found)")
+      MESSAGE(STATUS "  With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = NO (not found, License: BSD)")
    ENDIF()
  ENDIF()
 ENDIF(OpenCV_FOUND)

 IF(Freenect_FOUND)
-MESSAGE(STATUS "  With Freenect             = YES")
+MESSAGE(STATUS "  With Freenect             = YES (License: Apache v2 and/or GPLv2)")
 ELSEIF(NOT WITH_FREENECT)
 MESSAGE(STATUS "  With Freenect             = NO (WITH_FREENECT=OFF)")
 ELSE()
@@ -494,7 +516,7 @@ MESSAGE(STATUS "  With Freenect             = NO (libfreenect not found)")
 ENDIF()

 IF(OpenNI2_FOUND)
-MESSAGE(STATUS "  With OpenNI2              = YES")
+MESSAGE(STATUS "  With OpenNI2              = YES (License: Apache v2)")
 ELSEIF(NOT WITH_OPENNI2)
 MESSAGE(STATUS "  With OpenNI2              = NO (WITH_OPENNI2=OFF)")
 ELSE()
@@ -502,7 +524,7 @@ MESSAGE(STATUS "  With OpenNI2              = NO (OpenNI2 not found)")
 ENDIF()

 IF(freenect2_FOUND)
-MESSAGE(STATUS "  With Freenect2            = YES")
+MESSAGE(STATUS "  With Freenect2            = YES (License: Apache v2 and/or GPLv2)")
 ELSEIF(NOT WITH_FREENECT2)
 MESSAGE(STATUS "  With Freenect2            = NO (WITH_FREENECT2=OFF)")
 ELSE()
@@ -510,7 +532,7 @@ MESSAGE(STATUS "  With Freenect2            = NO (libfreenect2 not found)")
 ENDIF()

 IF(DC1394_FOUND)
-MESSAGE(STATUS "  With dc1394               = YES")
+MESSAGE(STATUS "  With dc1394               = YES (License: LGPL)")
 ELSEIF(NOT WITH_DC1394)
 MESSAGE(STATUS "  With dc1394               = NO (WITH_DC1394=OFF)")
 ELSE()
@@ -525,8 +547,14 @@ ELSE()
 MESSAGE(STATUS "  With FlyCapture2/Triclops = NO (Point Grey SDK not found)")
 ENDIF()

+IF(WITH_TORO)
+MESSAGE(STATUS "  With TORO                 = YES (License: Creative Commons [Attribution-NonCommercial-ShareAlike])")
+ELSE()
+MESSAGE(STATUS "  With TORO                 = NO (WITH_TORO=OFF)")
+ENDIF()
+
 IF(G2O_FOUND)
-MESSAGE(STATUS "  With g2o                  = YES")
+MESSAGE(STATUS "  With g2o                  = YES (License: BSD)")
 ELSEIF(NOT WITH_G2O)
 MESSAGE(STATUS "  With g2o                  = NO (WITH_G2O=OFF)")
 ELSE()
@@ -534,15 +562,21 @@ MESSAGE(STATUS "  With g2o                  = NO (g2o not found)")
 ENDIF()

 IF(GTSAM_FOUND)
-MESSAGE(STATUS "  With GTSAM                = YES")
+MESSAGE(STATUS "  With GTSAM                = YES (License: BSD)")
 ELSEIF(NOT WITH_GTSAM)
 MESSAGE(STATUS "  With GTSAM                = NO (WITH_GTSAM=OFF)")
 ELSE()
 MESSAGE(STATUS "  With GTSAM                = NO (GTSAM not found)")
 ENDIF()

+IF(WITH_VERTIGO)
+MESSAGE(STATUS "  With VERTIGO              = YES (License: GPLv3)")
+ELSE()
+MESSAGE(STATUS "  With VERTIGO              = NO (WITH_VERTIGO=OFF)")
+ENDIF()
+
 IF(cvsba_FOUND)
-MESSAGE(STATUS "  With cvsba                = YES")
+MESSAGE(STATUS "  With cvsba                = YES (License: GPLv2)")
 ELSEIF(NOT WITH_CVSBA)
 MESSAGE(STATUS "  With cvsba                = NO (WITH_CVSBA=OFF)")
 ELSE()
@@ -550,9 +584,9 @@ MESSAGE(STATUS "  With cvsba                = NO (cvsba not found)")
 ENDIF()

 IF(QT4_FOUND)
-MESSAGE(STATUS "  With Qt                   = YES (version 4)")
+MESSAGE(STATUS "  With Qt4                  = YES (License: Open Source or Commercial)")
 ELSEIF(Qt5_FOUND)
-MESSAGE(STATUS "  With Qt                   = YES (version 5)")
+MESSAGE(STATUS "  With Qt5                  = YES (License: Open Source or Commercial)")
 ELSEIF(NOT WITH_QT)
 MESSAGE(STATUS "  With Qt                   = NO (WITH_QT=OFF)")
 ELSE()
@@ -560,3 +594,10 @@ MESSAGE(STATUS "  With Qt                   = NO (Qt not found, to use Qt5 you s
 ENDIF()

 MESSAGE(STATUS "--------------------------------------------")
+
+IF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO)
+MESSAGE(SEND_ERROR "No graph optimizer found! You should have at least one of these options:
+       g2o (https://github.com/RainerKuemmerle/g2o)
+       GTSAM (https://collab.cc.gatech.edu/borg/gtsam)
+       set -DWITH_TORO=ON")
+ENDIF(NOT GTSAM_FOUND AND NOT G2O_FOUND AND NOT WITH_TORO)
@@ -37,10 +37,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #define RTABMAP_VERSION_COMPARE(major, minor, patch)  (major>=@PROJECT_VERSION_MAJOR@ || (major==@PROJECT_VERSION_MAJOR@ && minor>=@PROJECT_VERSION_MINOR@) || (major==@PROJECT_VERSION_MAJOR@ && minor==@PROJECT_VERSION_MINOR@ && patch >=@PROJECT_VERSION_PATCH@))

-#define RTABMAP_NONFREE @NONFREE@
-#define RTABMAP_G2O @G2O@
-#define RTABMAP_GTSAM @GTSAM@
-#define RTABMAP_OPENCV3 @OPENCV3@
+@NONFREE@#define RTABMAP_NONFREE
+@TORO@#define RTABMAP_TORO
+@G2O@#define RTABMAP_G2O
+@GTSAM@#define RTABMAP_GTSAM
+@VERTIGO@#define RTABMAP_VERTIGO
+@OPENCV3@#define RTABMAP_OPENCV3
+@OPENNI2@#define RTABMAP_OPENNI2
+@FREENECT@#define RTABMAP_FREENECT
+@FREENECT2@#define RTABMAP_FREENECT2
+@CVSBA@#define RTABMAP_CVSBA
+@DC1394@#define RTABMAP_DC1394
+@FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2

 #endif /* VERSION_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,
@@ -36,6 +36,9 @@ namespace rtabmap {

 class RTABMAP_EXP OptimizerTORO : public Optimizer
 {
+public:
+	static bool available();
+
 public:
 	static bool saveGraph(
 			const std::string & fileName,
@@ -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.");
@@ -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}
@@ -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;
@@ -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.
@@ -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!");
@@ -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)
@@ -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;

@@ -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);
@@ -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));
 				}
@@ -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;
 }

@@ -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;
@@ -46,7 +46,7 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	version.append(" [DEMO]");
 #endif
 	QString cv_version = CV_VERSION;
-#if RTABMAP_NONFREE == 1
+#ifdef RTABMAP_NONFREE
 	cv_version.append(" [With nonfree]");
 #else
 	cv_version.append(" [Without nonfree]");
@@ -405,34 +405,58 @@ void ParametersToolBox::addParameter(QVBoxLayout * layout,

 	if(key.compare(Parameters::kVisFeatureType().c_str()) == 0)
 	{
-		if(RTABMAP_NONFREE == 0)
+#ifndef RTABMAP_NONFREE
+		if(value <= 1)
 		{
-			if(value <= 1)
-			{
-				UWARN("SURF/SIFT not available, setting feature default to FAST/BRIEF.");
-				widget->setValue(4);
-			}
+			UWARN("SURF/SIFT not available, setting feature default to FAST/BRIEF.");
+			widget->setValue(4);
 		}
+#endif
 	}
 	if(key.compare(Parameters::kOptimizerStrategy().c_str()) == 0)
 	{
-		if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
+		if(value == 0 && !Optimizer::isAvailable(Optimizer::kTypeTORO))
 		{
-			if(value == 1)
+			if(Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+			{
+				UWARN("TORO is not available, setting optimization default to GTSAM.");
+				widget->setValue(2);
+			}
+			else if(Optimizer::isAvailable(Optimizer::kTypeG2O))
+			{
+				UWARN("TORO is not available, setting optimization default to g2o.");
+				widget->setValue(1);
+			}
+		}
+		if(value == 1 && !Optimizer::isAvailable(Optimizer::kTypeG2O))
+		{
+			if(Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+			{
+				UWARN("g2o is not available, setting optimization default to GTSAM.");
+				widget->setValue(2);
+			}
+			else if(Optimizer::isAvailable(Optimizer::kTypeTORO))
 			{
 				UWARN("g2o is not available, setting optimization default to TORO.");
 				widget->setValue(0);
 			}
 		}
-		if(!Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+		if(value == 2 && !Optimizer::isAvailable(Optimizer::kTypeGTSAM))
 		{
-			if(value == 2)
+			if(Optimizer::isAvailable(Optimizer::kTypeG2O))
+			{
+				UWARN("GTSAM is not available, setting optimization default to g2o.");
+				widget->setValue(2);
+			}
+			else if(Optimizer::isAvailable(Optimizer::kTypeTORO))
 			{
 				UWARN("GTSAM is not available, setting optimization default to TORO.");
-				widget->setValue(0);
+				widget->setValue(1);
 			}
 		}
-		if(!Optimizer::isAvailable(Optimizer::kTypeG2O) && !Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+		if(!Optimizer::isAvailable(Optimizer::kTypeG2O) &&
+			!Optimizer::isAvailable(Optimizer::kTypeGTSAM) &&
+			!Optimizer::isAvailable(Optimizer::kTypeTORO))
 		{
 			widget->setEnabled(false);
 		}
@@ -146,8 +146,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->label_map_shown->setText(_ui->label_map_shown->text() + " (Disabled, PCL >=1.7.2 required)");
 #endif

-	if(RTABMAP_NONFREE == 0)
-	{
+#ifndef RTABMAP_NONFREE
 		_ui->comboBox_detector_strategy->setItemData(0, 0, Qt::UserRole - 1);
 		_ui->comboBox_detector_strategy->setItemData(1, 0, Qt::UserRole - 1);
 		_ui->reextract_type->setItemData(0, 0, Qt::UserRole - 1);
@@ -170,7 +169,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		_ui->reextract_type->setItemData(5, 0, Qt::UserRole - 1);
 		_ui->reextract_type->setItemData(6, 0, Qt::UserRole - 1);
 #endif
-	}
+#endif

 #if CV_MAJOR_VERSION == 3
 	_ui->groupBox_fast_opencv2->setEnabled(false);
@@ -189,11 +188,17 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	{
 		_ui->comboBox_g2o_solver->setItemData(2, 0, Qt::UserRole - 1);
 	}
+	if(!Optimizer::isAvailable(Optimizer::kTypeTORO))
+	{
+		_ui->graphOptimization_type->setItemData(0, 0, Qt::UserRole - 1);
+	}
 	if(!Optimizer::isAvailable(Optimizer::kTypeGTSAM))
 	{
 		_ui->graphOptimization_type->setItemData(2, 0, Qt::UserRole - 1);
 	}
+#ifdef RTABMAP_VERTIGO
 	if(!Optimizer::isAvailable(Optimizer::kTypeG2O) && !Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+#endif
 	{
 		_ui->graphOptimization_robust->setEnabled(false);
 	}
@@ -2059,32 +2064,55 @@ void PreferencesDialog::writeCoreSettings(const QString & filePath) const

 bool PreferencesDialog::validateForm()
 {
-	if(RTABMAP_NONFREE == 0)
+#ifndef RTABMAP_NONFREE
+	// verify that SURF/SIFT cannot be selected if not built with OpenCV nonfree module
+	// BOW dictionary type
+	if(_ui->comboBox_detector_strategy->currentIndex() <= 1)
 	{
-		// verify that SURF/SIFT cannot be selected if not built with OpenCV nonfree module
-		// BOW dictionary type
-		if(_ui->comboBox_detector_strategy->currentIndex() <= 1)
-		{
-			QMessageBox::warning(this, tr("Parameter warning"),
-					tr("Selected feature type (SURF/SIFT) is not available. RTAB-Map is not built "
-					   "with the nonfree module from OpenCV. ORB is set instead for the bag-of-words dictionary."));
-			_ui->comboBox_detector_strategy->setCurrentIndex(Feature2D::kFeatureOrb);
-		}
-		// BOW Reextract features type
-		if(_ui->reextract_type->currentIndex() <= 1)
-		{
-			QMessageBox::warning(this, tr("Parameter warning"),
-					tr("Selected feature type (SURF/SIFT) is not available. RTAB-Map is not built "
-					   "with the nonfree module from OpenCV. Fast/Brief is set instead for the re-extraction "
-					   "of features on loop closure."));
-			_ui->reextract_type->setCurrentIndex(Feature2D::kFeatureFastBrief);
-		}
+		QMessageBox::warning(this, tr("Parameter warning"),
+				tr("Selected feature type (SURF/SIFT) is not available. RTAB-Map is not built "
+				   "with the nonfree module from OpenCV. ORB is set instead for the bag-of-words dictionary."));
+		_ui->comboBox_detector_strategy->setCurrentIndex(Feature2D::kFeatureOrb);
 	}
+	// BOW Reextract features type
+	if(_ui->reextract_type->currentIndex() <= 1)
+	{
+		QMessageBox::warning(this, tr("Parameter warning"),
+				tr("Selected feature type (SURF/SIFT) is not available. RTAB-Map is not built "
+				   "with the nonfree module from OpenCV. Fast/Brief is set instead for the re-extraction "
+				   "of features on loop closure."));
+		_ui->reextract_type->setCurrentIndex(Feature2D::kFeatureFastBrief);
+	}
+#endif

 	// optimization strategy
-	if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
+	if(_ui->graphOptimization_type->currentIndex() == 0 && !Optimizer::isAvailable(Optimizer::kTypeTORO))
 	{
-		if(_ui->graphOptimization_type->currentIndex() == 1)
+		if(Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+		{
+			QMessageBox::warning(this, tr("Parameter warning"),
+					tr("Selected graph optimization strategy (TORO) is not available. RTAB-Map is not built "
+					   "with TORO. GTSAM is set instead for graph optimization strategy."));
+			_ui->graphOptimization_type->setCurrentIndex(Optimizer::kTypeGTSAM);
+		}
+		else if(Optimizer::isAvailable(Optimizer::kTypeG2O))
+		{
+			QMessageBox::warning(this, tr("Parameter warning"),
+					tr("Selected graph optimization strategy (TORO) is not available. RTAB-Map is not built "
+					   "with TORO. g2o is set instead for graph optimization strategy."));
+			_ui->graphOptimization_type->setCurrentIndex(Optimizer::kTypeG2O);
+		}
+	}
+	if(_ui->graphOptimization_type->currentIndex() == 1 && !Optimizer::isAvailable(Optimizer::kTypeG2O))
+	{
+		if(Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+		{
+			QMessageBox::warning(this, tr("Parameter warning"),
+					tr("Selected graph optimization strategy (g2o) is not available. RTAB-Map is not built "
+					   "with g2o. GTSAM is set instead for graph optimization strategy."));
+			_ui->graphOptimization_type->setCurrentIndex(Optimizer::kTypeGTSAM);
+		}
+		else if(Optimizer::isAvailable(Optimizer::kTypeTORO))
 		{
 			QMessageBox::warning(this, tr("Parameter warning"),
 					tr("Selected graph optimization strategy (g2o) is not available. RTAB-Map is not built "
@@ -2092,9 +2120,16 @@ bool PreferencesDialog::validateForm()
 			_ui->graphOptimization_type->setCurrentIndex(Optimizer::kTypeTORO);
 		}
 	}
-	if(!Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+	if(_ui->graphOptimization_type->currentIndex() == 2 && !Optimizer::isAvailable(Optimizer::kTypeGTSAM))
 	{
-		if(_ui->graphOptimization_type->currentIndex() == 2)
+		if(Optimizer::isAvailable(Optimizer::kTypeG2O))
+		{
+			QMessageBox::warning(this, tr("Parameter warning"),
+					tr("Selected graph optimization strategy (GTSAM) is not available. RTAB-Map is not built "
+					   "with GTSAM. g2o is set instead for graph optimization strategy."));
+			_ui->graphOptimization_type->setCurrentIndex(Optimizer::kTypeG2O);
+		}
+		else if(Optimizer::isAvailable(Optimizer::kTypeTORO))
 		{
 			QMessageBox::warning(this, tr("Parameter warning"),
 					tr("Selected graph optimization strategy (GTSAM) is not available. RTAB-Map is not built "
@@ -2103,6 +2138,15 @@ bool PreferencesDialog::validateForm()
 		}
 	}

+	// verify that Robust and Reject threshold are not set at the same time
+	if(_ui->graphOptimization_robust->isEnabled() && _ui->graphOptimization_maxError->value()>0.0)
+	{
+		QMessageBox::warning(this, tr("Parameter warning"),
+				tr("Robust graph optimization and maximum optimization error threshold cannot be "
+				   "both used at the same time. Disabling robust optimization."));
+		_ui->graphOptimization_robust->setChecked(false);
+	}
+
 	//verify binary features and nearest neighbor
 	// BOW dictionary type
 	if(_ui->comboBox_dictionary_strategy->currentIndex() == VWDictionary::kNNFlannLSH && _ui->comboBox_detector_strategy->currentIndex() <= 1)
@@ -2771,31 +2815,30 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 			}
 			else
 			{
-				if(RTABMAP_NONFREE == 0)
+#ifndef RTABMAP_NONFREE
+				if(valueInt <= 1 &&
+						(combo->objectName().toStdString().compare(Parameters::kKpDetectorStrategy()) == 0 ||
+						 combo->objectName().toStdString().compare(Parameters::kVisFeatureType()) == 0))
 				{
-					if(valueInt <= 1 &&
-							(combo->objectName().toStdString().compare(Parameters::kKpDetectorStrategy()) == 0 ||
-							 combo->objectName().toStdString().compare(Parameters::kVisFeatureType()) == 0))
-					{
-						UWARN("Trying to set \"%s\" to SIFT/SURF but RTAB-Map isn't built "
-							  "with the nonfree module from OpenCV. Keeping default combo value: %s.",
-							  combo->objectName().toStdString().c_str(),
-							  combo->currentText().toStdString().c_str());
-						ok = false;
-					}
-					else if(valueInt==1 &&
-							(combo->objectName().toStdString().compare(Parameters::kKpNNStrategy()) == 0 ||
-							 combo->objectName().toStdString().compare(Parameters::kVisCorNNType()) == 0))
-
-					{
-						UWARN("Trying to set \"%s\" to KdTree but RTAB-Map isn't built "
-							  "with the nonfree module from OpenCV and kdTree cannot be used "
-							  "with binary descriptors. Keeping default combo value: %s.",
-							  combo->objectName().toStdString().c_str(),
-							  combo->currentText().toStdString().c_str());
-						ok = false;
-					}
+					UWARN("Trying to set \"%s\" to SIFT/SURF but RTAB-Map isn't built "
+						  "with the nonfree module from OpenCV. Keeping default combo value: %s.",
+						  combo->objectName().toStdString().c_str(),
+						  combo->currentText().toStdString().c_str());
+					ok = false;
 				}
+				else if(valueInt==1 &&
+						(combo->objectName().toStdString().compare(Parameters::kKpNNStrategy()) == 0 ||
+						 combo->objectName().toStdString().compare(Parameters::kVisCorNNType()) == 0))
+
+				{
+					UWARN("Trying to set \"%s\" to KdTree but RTAB-Map isn't built "
+						  "with the nonfree module from OpenCV and kdTree cannot be used "
+						  "with binary descriptors. Keeping default combo value: %s.",
+						  combo->objectName().toStdString().c_str(),
+						  combo->currentText().toStdString().c_str());
+					ok = false;
+				}
+#endif
 				if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
 				{
 					if(valueInt==1 && combo->objectName().toStdString().compare(Parameters::kOptimizerStrategy()) == 0)
@@ -65,7 +65,7 @@
            <x>0</x>
            <y>0</y>
            <width>681</width>
-            <height>2010</height>
+            <height>2026</height>
           </rect>
          </property>
          <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                <enum>QFrame::Raised</enum>
               </property>
               <property name="currentIndex">
-                <number>18</number>
+                <number>12</number>
               </property>
               <widget class="QWidget" name="page_22">
                <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -202,7 +202,7 @@ unsigned char UVariant::toUChar(bool * ok) const
 	else if(type_ == kChar)
 	{
 		char tmp = toChar();
-		if(tmp >= std::numeric_limits<unsigned char>::min() && tmp <= std::numeric_limits<unsigned char>::max())
+		if(tmp >= std::numeric_limits<unsigned char>::min())
 		{
 			v = (unsigned char)tmp;
 			if(ok)
@@ -250,7 +250,7 @@ unsigned char UVariant::toUChar(bool * ok) const
 	else if(type_ == kUInt)
 	{
 		unsigned int tmp = toUInt();
-		if(tmp >= std::numeric_limits<unsigned char>::min() && tmp <= std::numeric_limits<unsigned char>::max())
+		if(tmp <= std::numeric_limits<unsigned char>::max())
 		{
 			v = (unsigned char)tmp;
 			if(ok)
@@ -368,7 +368,7 @@ unsigned short UVariant::toUShort(bool * ok) const
 	else if(type_ == kShort)
 	{
 		short tmp = toShort();
-		if(tmp >= std::numeric_limits<unsigned short>::min() && tmp <= std::numeric_limits<unsigned short>::max())
+		if(tmp >= std::numeric_limits<unsigned short>::min())
 		{
 			v = (unsigned short)tmp;
 			if(ok)
@@ -392,7 +392,7 @@ unsigned short UVariant::toUShort(bool * ok) const
 	else if(type_ == kUInt)
 	{
 		unsigned int tmp = toUInt();
-		if(tmp >= std::numeric_limits<unsigned short>::min() && tmp <= std::numeric_limits<unsigned short>::max())
+		if(tmp <= std::numeric_limits<unsigned short>::max())
 		{
 			v = (unsigned short)tmp;
 			if(ok)