Added support for latest g2o code (with c++11 interface)

CMakeLists.txt

@@ -320,6 +320,7 @@ IF(WITH_POINTMATCHER)
     ENDIF(libpointmatcher_FOUND)
 ENDIF(WITH_POINTMATCHER)
 
+SET(ZED_FOUND FALSE)
 IF(WITH_ZED)
     IF(WIN32) # Windows
         SET(ZED_INCLUDE_DIRS $ENV{ZED_INCLUDE_DIRS})
@@ -472,6 +473,9 @@ IF(NOT G2O_FOUND)
    SET(G2O "//")
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${G2O_LIBRARIES})
+   IF(NOT G2O_CPP11)
+   SET(G2O_CPP_CONF "//")
+   ENDIF(NOT G2O_CPP11)
 ENDIF()
 IF(NOT GTSAM_FOUND)
    SET(GTSAM "//")

Version.h.in

@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @NONFREE@#define RTABMAP_NONFREE
 @TORO@#define RTABMAP_TORO
 @G2O@#define RTABMAP_G2O
+@G2O_CPP_CONF@#define RTABMAP_G2O_CPP11
 @GTSAM@#define RTABMAP_GTSAM
 @VERTIGO@#define RTABMAP_VERTIGO
 @OPENCV3@#define RTABMAP_OPENCV3

cmake_modules/FindG2O.cmake

@@ -17,6 +17,14 @@ FIND_LIBRARY(CHOLMOD_LIB cholmod)
 FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h 
   PATHS "C:\\Program Files\\g2o\\include")
 
+FIND_FILE(G2O_CONFIG_FILE g2o/config.h 
+  PATHS ${G2O_INCLUDE_DIR}
+  NO_DEFAULT_PATH)
+
+#ifdef G2O_NUMBER_FORMAT_STR
+#define G2O_CPP11 // we assume that if G2O_NUMBER_FORMAT_STR is defined, this is the new g2o code with c++11 interface
+#endif
+
 # Macro to unify finding both the debug and release versions of the
 # libraries; this is adapted from the rtabmap config
 
@@ -75,7 +83,7 @@ ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER
 
 # G2O itself declared found if we found the core libraries and at least one solver
 SET(G2O_FOUND "NO")
-IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
+IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_SOLVERS_FOUND)
   SET(G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR})
   SET(G2O_LIBRARIES 
 	${G2O_CORE_LIBRARY}
@@ -105,5 +113,15 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FO
 	  ${CHOLMOD_LIB})
   ENDIF(G2O_SOLVER_CHOLMOD)
 
+  FILE(READ ${G2O_CONFIG_FILE} TMPTXT)
+  STRING(FIND "${TMPTXT}" "G2O_NUMBER_FORMAT_STR" matchres)
+  IF(${matchres} EQUAL -1)
+    MESSAGE(STATUS "Old g2o version detected with c++03 interface (config file: ${G2O_CONFIG_FILE}).")
+    SET(G2O_CPP11 0)
+  ELSE()
+    MESSAGE(WARNING "Latest g2o version detected with c++11 interface (config file: ${G2O_CONFIG_FILE}). Make sure g2o is built with \"-DBUILD_WITH_MARCH_NATIVE=OFF\" to avoid segmentation faults caused by Eigen.")
+    SET(G2O_CPP11 1)
+  ENDIF()
+
   SET(G2O_FOUND "YES")
-ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
+ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_SOLVERS_FOUND)

corelib/src/OptimizerG2O.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <set>
 
+#include <rtabmap/core/Version.h>
 #include <rtabmap/core/OptimizerG2O.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d_motion_estimation.h>
@@ -82,7 +83,7 @@ typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSpa
 typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> SlamLinearCholmodSolver;
 #endif
 
-#ifdef RTABMAP_VERTIGO
+#if defined(RTABMAP_VERTIGO)
 #include "vertigo/g2o/edge_switchPrior.h"
 #include "vertigo/g2o/edge_se2Switchable.h"
 #include "vertigo/g2o/edge_se3Switchable.h"
@@ -190,6 +191,55 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		odomOffset->setId(PARAM_OFFSET);
 		optimizer.addParameter(odomOffset);
 
+#ifdef RTABMAP_G2O_CPP11
+
+		std::unique_ptr<SlamBlockSolver> blockSolver;
+
+		if(solver_ == 3)
+		{
+			//eigen
+			auto linearSolver = g2o::make_unique<SlamLinearEigenSolver>();
+			linearSolver->setBlockOrdering(false);
+			blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
+		}
+#ifdef G2O_HAVE_CHOLMOD
+		else if(solver_ == 2)
+		{
+			//chmold
+			auto linearSolver = g2o::make_unique<SlamLinearCholmodSolver>();
+			linearSolver->setBlockOrdering(false);
+			blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
+		}
+#endif
+#ifdef G2O_HAVE_CSPARSE
+		else if(solver_ == 0)
+		{
+
+			//csparse
+			auto linearSolver = g2o::make_unique<SlamLinearCSparseSolver>();
+			linearSolver->setBlockOrdering(false);
+			blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
+		}
+#endif
+		else
+		{
+			//pcg
+			auto linearSolver = g2o::make_unique<SlamLinearPCGSolver>();
+			blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
+		}
+
+		if(optimizer_ == 1)
+		{
+
+			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(std::move(blockSolver)));
+		}
+		else
+		{
+			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver)));
+		}
+
+#else
+
 		SlamBlockSolver * blockSolver = 0;
 
 		if(solver_ == 3)
@@ -199,26 +249,25 @@ std::map<int, Transform> OptimizerG2O::optimize(
 			linearSolver->setBlockOrdering(false);
 			blockSolver = new SlamBlockSolver(linearSolver);
 		}
+#ifdef G2O_HAVE_CHOLMOD
 		else if(solver_ == 2)
 		{
-#ifdef G2O_HAVE_CHOLMOD
 			//chmold
 			SlamLinearCholmodSolver * linearSolver = new SlamLinearCholmodSolver();
 			linearSolver->setBlockOrdering(false);
 			blockSolver = new SlamBlockSolver(linearSolver);
-#endif
 		}
+#endif
+#ifdef G2O_HAVE_CSPARSE
 		else if(solver_ == 0)
 		{
-#ifdef G2O_HAVE_CSPARSE
 			//csparse
 			SlamLinearCSparseSolver* linearSolver = new SlamLinearCSparseSolver();
 			linearSolver->setBlockOrdering(false);
 			blockSolver = new SlamBlockSolver(linearSolver);
-#endif
 		}
-
+#endif
-		if(blockSolver == 0)
+		else
 		{
 			//pcg
 			SlamLinearPCGSolver * linearSolver = new SlamLinearPCGSolver();
@@ -233,7 +282,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		{
 			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(blockSolver));
 		}
-
+#endif
 		// detect if there is a global pose prior set, if so remove rootId
 		if(!priorsIgnored())
 		{
@@ -282,7 +331,9 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		}
 
 		UDEBUG("fill edges to g2o...");
+#if defined(RTABMAP_VERTIGO)
 		int vertigoVertexId = poses.rbegin()->first+1;
+#endif
 		for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 		{
 			int id1 = iter->second.from();
@@ -342,7 +393,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 			}
 			else
 			{
-#ifdef RTABMAP_VERTIGO
+#if defined(RTABMAP_VERTIGO)
 				VertexSwitchLinear * v = 0;
 				if(this->isRobust() &&
 				   iter->second.type() != Link::kNeighbor &&
@@ -388,7 +439,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 						information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
 					}
 
-#ifdef RTABMAP_VERTIGO
+#if defined(RTABMAP_VERTIGO)
 					if(this->isRobust() &&
 					   iter->second.type() != Link::kNeighbor  &&
 					   iter->second.type() != Link::kNeighborMerged)
@@ -433,7 +484,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 					constraint = a.linear();
 					constraint.translation() = a.translation();
 
-#ifdef RTABMAP_VERTIGO
+#if defined(RTABMAP_VERTIGO)
 					if(this->isRobust() &&
 					   iter->second.type() != Link::kNeighbor &&
 					   iter->second.type() != Link::kNeighborMerged)
@@ -739,7 +790,11 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 	{
 		g2o::SparseOptimizer optimizer;
 		optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
+#ifdef RTABMAP_G2O_CPP11
+		std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;
+#else
 		g2o::BlockSolver_6_3::LinearSolverType * linearSolver = 0;
+#endif
 
 #ifdef RTABMAP_ORB_SLAM2
 		linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
@@ -747,43 +802,67 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		if(solver_ == 3)
 		{
 			//eigen
+#ifdef RTABMAP_G2O_CPP11
+			linearSolver = g2o::make_unique<g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType> >();
+#else
 			linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
+#endif
 		}
+#ifdef G2O_HAVE_CHOLMOD
 		else if(solver_ == 2)
 		{
-#ifdef G2O_HAVE_CHOLMOD
 			//chmold
+#ifdef RTABMAP_G2O_CPP11
+			linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> >();
+#else
 			linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
 #endif
 		}
+#endif
+#ifdef G2O_HAVE_CSPARSE
 		else if(solver_ == 0)
 		{
-#ifdef G2O_HAVE_CSPARSE
 			//csparse
+#ifdef RTABMAP_G2O_CPP11
+			linearSolver = g2o::make_unique<g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType> >();
+#else
 			linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>();
 #endif
 		}
-
+#endif
-		if(linearSolver == 0)
+		else
 		{
 			//pcg
+#ifdef RTABMAP_G2O_CPP11
+			linearSolver = g2o::make_unique<g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType> >();
+#else
 			linearSolver = new g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType>();
-		}
 #endif
-
+		}
-		g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
+#endif // RTABMAP_ORB_SLAM2
 
 #ifndef RTABMAP_ORB_SLAM2
 		if(optimizer_ == 1)
 		{
-			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(solver_ptr));
+#ifdef RTABMAP_G2O_CPP11
+			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(
+					g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver))));
+#else
+			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(new g2o::BlockSolver_6_3(linearSolver)));
+#endif
 		}
 		else
 #endif
 		{
-			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(solver_ptr));
+#ifdef RTABMAP_G2O_CPP11
+			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(
+					g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver))));
+#else
+			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(new g2o::BlockSolver_6_3(linearSolver)));
+#endif
 		}
 
+
 		UDEBUG("fill poses to g2o...");
 		for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); )
 		{

corelib/src/vertigo/g2o/edge_se3Switchable.cpp

@@ -31,7 +31,7 @@ EdgeSE3Switchable::EdgeSE3Switchable() : g2o::BaseMultiEdge<6, Eigen::Isometry3d
 // ================================================
 bool EdgeSE3Switchable::read(std::istream& is)
   {    
-    g2o::Vector7d meas;
+ /*   g2o::Vector7d meas;
     for (int i=0; i<7; i++) 
       is >> meas[i];
     // normalize the quaternion to recover numerical precision lost by storing as human readable text
@@ -44,18 +44,20 @@ bool EdgeSE3Switchable::read(std::istream& is)
         if (i!=j)
           information()(j,i) = information()(i,j);
       }
-    return true;
+    return true;*/
+	return false;
 
   }
 // ================================================
 bool EdgeSE3Switchable::write(std::ostream& os) const
 {
-    g2o::Vector7d meas = g2o::internal::toVectorQT(measurement());
+   /* g2o::Vector7d meas = g2o::internal::toVectorQT(measurement());
     for (int i=0; i<7; i++) os  << meas[i] << " ";
     for (int i = 0; i < 6; ++i)
       for (int j = i; j < 6; ++j)
         os << " " << information()(i, j);
-    return os.good();
+    return os.good();*/
+	return false;
 }
 
 // ================================================