merged master to 0.11.0

CMakeLists.txt

@@ -202,6 +202,22 @@ IF(WITH_GTSAM)
     FIND_PACKAGE(GTSAM QUIET)
 ENDIF(WITH_GTSAM)
 
+IF(G2O_FOUND OR GTSAM_FOUND)
+   #Newest versions require std11
+   IF(NOT MSVC)
+     include(CheckCXXCompilerFlag)
+	  CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+	  CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+	  IF(COMPILER_SUPPORTS_CXX11)
+		set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+	  ELSEIF(COMPILER_SUPPORTS_CXX0X)
+		set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+	  ELSE()
+		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler if you want to use g2o or gtsam (set \"-DWITH_G2O=OFF -DWITH_GTSAM=OFF\" to build without g2o and gtsam).")
+	  ENDIF()
+   ENDIF()
+ENDIF(G2O_FOUND OR GTSAM_FOUND)
+
 IF(WITH_FLYCAPTURE2)
     FIND_PACKAGE(FlyCapture2 QUIET)
     IF(FlyCapture2_FOUND)

cmake_modules/FindG2O.cmake

@@ -79,4 +79,6 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FO
 	${CSPARSE_LIBRARY}
         cholmod)
   SET(G2O_FOUND "YES")
-ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND AND CSPARSE_FOUND)
+ELSEIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR)
+  MESSAGE(STATUS "g2o core libraries found but some solvers are missing. Make sure to install \"libsuitesparse-dev\" before building/installing g2o.")
+ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR)

corelib/src/CMakeLists.txt

@@ -162,8 +162,6 @@ IF(G2O_FOUND)
 		${LIBRARIES}
 		${G2O_LIBRARIES}
 	)
-	#Newest versions require std11
-	SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 	
 	SET(SRC_FILES
     	${SRC_FILES}

corelib/src/OptimizerG2O.cpp

@@ -302,6 +302,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		UDEBUG("Initial optimization...");
 		optimizer.initializeOptimization();
 
+		UASSERT(optimizer.verifyInformationMatrices());
+
 		UINFO("g2o optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
 		int it = 0;
 		UTimer timer;
@@ -361,6 +363,13 @@ std::map<int, Transform> OptimizerG2O::optimize(
 				optimizer.computeActiveErrors();
 				double chi2 = optimizer.activeRobustChi2();
 				UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
+
+				if(i>0 && optimizer.activeRobustChi2() > 1000000000000.0)
+				{
+					UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!");
+					return optimizedPoses;
+				}
+
 				double errorDelta = lastError - chi2;
 				if(i>0 && errorDelta < this->epsilon())
 				{
@@ -398,6 +407,12 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		}
 		UINFO("g2o optimizing end (%d iterations done, error=%f, time = %f s)", it, optimizer.activeRobustChi2(), timer.ticks());
 
+		if(optimizer.activeRobustChi2() > 1000000000000.0)
+		{
+			UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!");
+			return optimizedPoses;
+		}
+
 		if(isSlam2d())
 		{
 			for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)

corelib/src/Rtabmap.cpp

@@ -874,7 +874,7 @@ bool Rtabmap::process(
 				   "Image %d is ignored!", data.id());
 			return false;
 		}
-		else
+		else if(_memory->isIncremental()) // only in mapping mode
 		{
 			// Detect if the odometry is reset. If yes, trigger a new map.
 			if(_memory->getLastWorkingSignature())
@@ -2080,12 +2080,24 @@ bool Rtabmap::process(
 			std::map<int, Transform> poses = _optimizedPoses;
 			std::multimap<int, Link> constraints;
 			optimizeCurrentMap(signature->id(), false, poses, &constraints, &optimizationError, &optimizationIterations);
-			UASSERT(poses.find(signature->id()) != poses.end());
 
 			// Check added loop closures have broken the graph
 			// (in case of wrong loop closures).
 			bool updateConstraints = true;
-			if(_memory->isIncremental() && // FIXME: not tested in localization mode, so do it only in mapping mode
+			if(poses.empty())
+			{
+				UWARN("Graph optimization failed! Rejecting last loop closures added.");
+				for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
+                              	{
+                                	_memory->removeLink(iter->first, iter->second);
+                                        UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
+                                 }
+                                updateConstraints = false;
+                                _loopClosureHypothesis.first = 0;
+                                lastLocalSpaceClosureId = 0;
+                                rejectedHypothesis = true;
+			}
+			else if(_memory->isIncremental() && // FIXME: not tested in localization mode, so do it only in mapping mode
 			  _optimizationMaxLinearError > 0.0f &&
 			  loopClosureLinksAdded.size())
 			{
@@ -2365,7 +2377,7 @@ bool Rtabmap::process(
 			}
 			else
 			{
-				UASSERT_MSG(uContains(_optimizedPoses, _lastLocalizationNodeId), uFormat("id=%d", _lastLocalizationNodeId).c_str());
+				UASSERT_MSG(uContains(_optimizedPoses, _lastLocalizationNodeId), uFormat("id=%d isInWM?=%d", _lastLocalizationNodeId, _memory->isInWM(_lastLocalizationNodeId)?1:0).c_str());
 				id = _lastLocalizationNodeId;
 				UDEBUG("Refresh local map from %d", id);
 			}
@@ -2379,6 +2391,10 @@ bool Rtabmap::process(
 		}
 		if(id > 0)
 		{
+			if(_lastLocalizationNodeId != 0)
+			{
+				_lastLocalizationNodeId = id;
+			}
 			UASSERT_MSG(_memory->getSignature(id) != 0, uFormat("id=%d", id).c_str());
 			std::map<int, int> ids = _memory->getNeighborsId(id, 0, 0, true);
 			for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end();)
@@ -2386,6 +2402,7 @@ bool Rtabmap::process(
 				if(!uContains(ids, iter->first))
 				{
 					UDEBUG("Removed %d from local map", iter->first);
+					UASSERT(iter->first != _lastLocalizationNodeId);
 					_optimizedPoses.erase(iter++);
 				}
 				else
@@ -2824,7 +2841,12 @@ void Rtabmap::optimizeCurrentMap(
 		}
 		else
 		{
-			UERROR("Failed to optimize the graph! Keeping the graph without optimization...");
+			UERROR("Failed to optimize the graph! returning empty optimized poses...");
+			optimizedPoses.clear();
+			if(constraints)
+			{
+				constraints->clear();
+			}
 		}
 	}
 }