diff --git a/CMakeLists.txt b/CMakeLists.txt index c1ce19f6..2a7d18fa 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 7e5a06e7..5f8f5a5b 100644 --- a/corelib/src/CMakeLists.txt +++ b/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} diff --git a/corelib/src/OptimizerG2O.cpp b/corelib/src/OptimizerG2O.cpp index 101f11f7..a2b51262 100644 --- a/corelib/src/OptimizerG2O.cpp +++ b/corelib/src/OptimizerG2O.cpp @@ -302,6 +302,8 @@ std::map 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 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 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::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 7fa3a1be..b9988a4f 100644 --- a/corelib/src/Rtabmap.cpp +++ b/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 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()) { @@ -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 ids = _memory->getNeighborsId(id, 0, 0, true); for(std::map::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(); + } } } }