iSAM2 integration (#1249)

* isam2 integration

* fonctional iSAM2, added related parameters

* updated default params after testing large-scale enviroment

* proximity search optimization

* boost<1.68 fix

* boost version

* Fixed map not showing in graphview

* iSAM2: supporting MM/localization/multi-session modes

corelib/include/rtabmap/core/Parameters.h

@@ -436,6 +436,9 @@ class RTABMAP_CORE_EXPORT Parameters
  RTABMAP_PARAM(g2o, Baseline, double, 0.075, "When doing bundle adjustment with RGB-D data, we can set a fake baseline (m) to do stereo bundle adjustment (if 0, mono bundle adjustment is done). For stereo data, the baseline in the calibration is used directly.");
 
  RTABMAP_PARAM(GTSAM, Optimizer, int, 1, "0=Levenberg 1=GaussNewton 2=Dogleg");
+ RTABMAP_PARAM(GTSAM, Incremental, bool, false, uFormat("Do graph optimization incrementally (iSAM2) to increase optimization speed on loop closures. Note that only GaussNewton and Dogleg optimization algorithms are supported (%s) in this mode.", kGTSAMOptimizer().c_str()));
+ RTABMAP_PARAM(GTSAM, IncRelinearizeThreshold, double, 0.01, "Only relinearize variables whose linear delta magnitude is greater than this threshold. See GTSAM::ISAM2 doc for more info.");
+ RTABMAP_PARAM(GTSAM, IncRelinearizeSkip, int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info.");
 
  // Odometry
  RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D");

corelib/include/rtabmap/core/Statistics.h

@@ -157,6 +157,7 @@ class RTABMAP_CORE_EXPORT Statistics
  RTABMAP_STATS(Timing, Memory_update, ms);
  RTABMAP_STATS(Timing, Neighbor_link_refining, ms);
  RTABMAP_STATS(Timing, Proximity_by_time, ms);
+ RTABMAP_STATS(Timing, Proximity_by_space_search, ms);
  RTABMAP_STATS(Timing, Proximity_by_space_visual, ms);
  RTABMAP_STATS(Timing, Proximity_by_space, ms);
  RTABMAP_STATS(Timing, Cleaning_neighbors, ms);

corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h

@@ -30,6 +30,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/Optimizer.h>
 
+namespace gtsam {
+ class ISAM2;
+}
+
 namespace rtabmap {
 
 class RTABMAP_CORE_EXPORT OptimizerGTSAM : public Optimizer
@@ -38,13 +42,8 @@ class RTABMAP_CORE_EXPORT OptimizerGTSAM : public Optimizer
  static bool available();
 
 public:
- OptimizerGTSAM(const ParametersMap & parameters = ParametersMap()) :
- Optimizer(parameters),
- optimizer_(Parameters::defaultGTSAMOptimizer())
- {
- parseParameters(parameters);
- }
- virtual ~OptimizerGTSAM() {}
+ OptimizerGTSAM(const ParametersMap & parameters = ParametersMap());
+ virtual ~OptimizerGTSAM();
 
  virtual Type type() const {return kTypeGTSAM;}
 
@@ -60,7 +59,25 @@ class RTABMAP_CORE_EXPORT OptimizerGTSAM : public Optimizer
  int * iterationsDone = 0);
 
 private:
- int optimizer_;
+ int internalOptimizerType_;
+
+ gtsam::ISAM2 * isam2_;
+ struct ConstraintToFactor {
+ ConstraintToFactor(int _from, int _to, std::uint64_t _factorIndice)
+ {
+ from = _from;
+ to = _to;
+ factorIndice = _factorIndice;
+ }
+ int from;
+ int to;
+ std::uint64_t factorIndice;
+ };
+
+ std::vector<ConstraintToFactor> lastAddedConstraints_;
+ int lastSwitchId_;
+ std::set<int> addedPoses_;
+ std::pair<int, std::uint64_t> lastRootFactorIndex_;
 };
 
 } /* namespace rtabmap */

corelib/src/CameraThread.cpp

@@ -292,6 +292,11 @@ bool CameraThread::odomProvided() const
 void CameraThread::mainLoopBegin()
 {
  ULogger::registerCurrentThread("Camera");
+ if(_imuFilter)
+ {
+ // In case we paused the camera and moved somewhere else, restart filtering.
+ _imuFilter->reset();
+ }
  _camera->resetTimer();
 }
 

corelib/src/GlobalMap.cpp

@@ -121,7 +121,7 @@ bool GlobalMap::update(const std::map<int, Transform> & poses)
  }
  else
  {
- UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", jter->first);
+ UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", iter->first);
  }
  }
 

corelib/src/Rtabmap.cpp

@@ -1094,6 +1094,13 @@ void Rtabmap::resetMemory()
  {
  UERROR("RTAB-Map is not initialized. No memory to reset...");
  }
+
+ if(_graphOptimizer)
+ {
+ delete _graphOptimizer;
+ _graphOptimizer = Optimizer::create(_parameters);
+ }
+
  this->setupLogFiles(true);
 }
 
@@ -1177,6 +1184,7 @@ bool Rtabmap::process(
  double timeMemoryUpdate = 0;
  double timeNeighborLinkRefining = 0;
  double timeProximityByTimeDetection = 0;
+ double timeProximityBySpaceSearch = 0;
  double timeProximityBySpaceVisualDetection = 0;
  double timeProximityBySpaceDetection = 0;
  double timeCleaningNeighbors = 0;
@@ -2598,22 +2606,39 @@ bool Rtabmap::process(
  // 1) compare visually with nearest locations
  //
  UDEBUG("Proximity detection (local loop closure in SPACE using matching images, local radius=%fm)", _localRadius);
- std::map<int, float> nearestIds;
- if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
- {
- nearestIds = _memory->getNeighborsIdRadius(signature->id(), _localRadius, _optimizedPoses, _proximityMaxGraphDepth);
- }
- else
- {
- nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
- }
+ std::map<int, float> nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
  UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size());
  std::map<int, Transform> nearestPoses;
+ std::multimap<int, int> links;
+ if(_memory->isIncremental() && _proximityMaxGraphDepth>0)
+ {
+ // get bidirectional links
+ for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter)
+ {
+ if(uContains(_optimizedPoses, iter->second.from()) && uContains(_optimizedPoses, iter->second.to()))
+ {
+ links.insert(std::make_pair(iter->second.from(), iter->second.to()));
+ links.insert(std::make_pair(iter->second.to(), iter->second.from())); // <->
+ }
+ }
+ }
  for(std::map<int, float>::iterator iter=nearestIds.lower_bound(1); iter!=nearestIds.end(); ++iter)
  {
  if(_memory->getStMem().find(iter->first) == _memory->getStMem().end())
  {
- nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
+ if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
+ {
+ std::list<std::pair<int, Transform> > path = graph::computePath(_optimizedPoses, links, signature->id(), iter->first);
+ UDEBUG("Graph depth to %d = %ld", iter->first, path.size());
+ if(!path.empty() && (int)path.size() <= _proximityMaxGraphDepth)
+ {
+ nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
+ }
+ }
+ else
+ {
+ nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
+ }
  }
  }
  UDEBUG("nearestPoses=%d", (int)nearestPoses.size());
@@ -2645,6 +2670,9 @@ bool Rtabmap::process(
  }
  UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths);
 
+ timeProximityBySpaceSearch = timer.ticks();
+ ULOGGER_INFO("timeProximityBySpaceSearch=%fs", timeProximityBySpaceSearch);
+
  float proximityFilteringRadius = _proximityFilteringRadius;
  if(_maxLoopClosureDistance>0.0f && (proximityFilteringRadius <= 0.0f || _maxLoopClosureDistance<proximityFilteringRadius))
  {
@@ -4072,6 +4100,7 @@ bool Rtabmap::process(
  statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
  statistics_.addStatistic(Statistics::kTimingNeighbor_link_refining(), timeNeighborLinkRefining*1000);
  statistics_.addStatistic(Statistics::kTimingProximity_by_time(), timeProximityByTimeDetection*1000);
+ statistics_.addStatistic(Statistics::kTimingProximity_by_space_search(), timeProximityBySpaceSearch*1000);
  statistics_.addStatistic(Statistics::kTimingProximity_by_space_visual(), timeProximityBySpaceVisualDetection*1000);
  statistics_.addStatistic(Statistics::kTimingProximity_by_space(), timeProximityBySpaceDetection*1000);
  statistics_.addStatistic(Statistics::kTimingReactivation(), timeReactivations*1000);