From 4e9ba66555475a4f7148b2ba1b7d6c9618ea882d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 1 Dec 2015 11:51:59 -0500 Subject: [PATCH 1/3] Ubuntu 12.04 an g2o, fixed -std=c++11 error --- CMakeLists.txt | 16 ++++++++++++++++ cmake_modules/FindG2O.cmake | 4 +++- corelib/src/CMakeLists.txt | 2 -- 3 files changed, 19 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 57c87316..664f5395 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/cmake_modules/FindG2O.cmake b/cmake_modules/FindG2O.cmake index 892c0933..a16cd204 100644 --- a/cmake_modules/FindG2O.cmake +++ b/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) diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 9afae0de..1b3d0ed2 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -153,8 +153,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} From 4d97ba80d6397dcc71d2de032c4ef5afbc0abab4 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 1 Dec 2015 12:34:34 -0500 Subject: [PATCH 2/3] Don't trigger new map in localization mode when identity odom is detected --- corelib/src/Rtabmap.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index eea33ad3..1a9920cd 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -904,7 +904,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()) From 60f34fe994b741e7fdecf66622ac240bcadf55ac Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 1 Dec 2015 14:51:26 -0500 Subject: [PATCH 3/3] Rejecting last loop closures if optimization fails. Fixed _lastLocalizationNodeId not found in optimized poses in Localization mode. --- corelib/src/Graph.cpp | 16 ++++++++++++++++ corelib/src/Rtabmap.cpp | 30 ++++++++++++++++++++++++++---- 2 files changed, 42 insertions(+), 4 deletions(-) diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index 4e95e58a..b57e636b 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -958,6 +958,8 @@ std::map G2OOptimizer::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; @@ -1017,6 +1019,14 @@ std::map G2OOptimizer::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()) { @@ -1054,6 +1064,12 @@ std::map G2OOptimizer::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::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 1a9920cd..f4289f00 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -2213,12 +2213,24 @@ bool Rtabmap::process( std::map poses = _optimizedPoses; std::multimap 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 >::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()) { @@ -2498,7 +2510,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); } @@ -2512,6 +2524,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 ids = _memory->getNeighborsId(id, 0, 0, true); for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end();) @@ -2519,6 +2535,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 @@ -2957,7 +2974,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(); + } } } }