Skip to content

Commit cc17ebe

Browse files
matlabbematlabbe
authored
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
1 parent bfeb487 commit cc17ebe

10 files changed

Lines changed: 450 additions & 84 deletions

File tree

‎corelib/include/rtabmap/core/Parameters.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -436,6 +436,9 @@ class RTABMAP_CORE_EXPORT Parameters
436436
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.");
437437

438438
RTABMAP_PARAM(GTSAM, Optimizer, int, 1, "0=Levenberg 1=GaussNewton 2=Dogleg");
439+
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()));
440+
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.");
441+
RTABMAP_PARAM(GTSAM, IncRelinearizeSkip, int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info.");
439442

440443
// Odometry
441444
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‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -157,6 +157,7 @@ class RTABMAP_CORE_EXPORT Statistics
157157
RTABMAP_STATS(Timing, Memory_update, ms);
158158
RTABMAP_STATS(Timing, Neighbor_link_refining, ms);
159159
RTABMAP_STATS(Timing, Proximity_by_time, ms);
160+
RTABMAP_STATS(Timing, Proximity_by_space_search, ms);
160161
RTABMAP_STATS(Timing, Proximity_by_space_visual, ms);
161162
RTABMAP_STATS(Timing, Proximity_by_space, ms);
162163
RTABMAP_STATS(Timing, Cleaning_neighbors, ms);

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

Lines changed: 25 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
3030

3131
#include <rtabmap/core/Optimizer.h>
3232

33+
namespace gtsam {
34+
class ISAM2;
35+
}
36+
3337
namespace rtabmap {
3438

3539
class RTABMAP_CORE_EXPORT OptimizerGTSAM : public Optimizer
@@ -38,13 +42,8 @@ class RTABMAP_CORE_EXPORT OptimizerGTSAM : public Optimizer
3842
static bool available();
3943

4044
public:
41-
OptimizerGTSAM(const ParametersMap & parameters = ParametersMap()) :
42-
Optimizer(parameters),
43-
optimizer_(Parameters::defaultGTSAMOptimizer())
44-
{
45-
parseParameters(parameters);
46-
}
47-
virtual ~OptimizerGTSAM() {}
45+
OptimizerGTSAM(const ParametersMap & parameters = ParametersMap());
46+
virtual ~OptimizerGTSAM();
4847

4948
virtual Type type() const {return kTypeGTSAM;}
5049

@@ -60,7 +59,25 @@ class RTABMAP_CORE_EXPORT OptimizerGTSAM : public Optimizer
6059
int * iterationsDone = 0);
6160

6261
private:
63-
int optimizer_;
62+
int internalOptimizerType_;
63+
64+
gtsam::ISAM2 * isam2_;
65+
struct ConstraintToFactor {
66+
ConstraintToFactor(int _from, int _to, std::uint64_t _factorIndice)
67+
{
68+
from = _from;
69+
to = _to;
70+
factorIndice = _factorIndice;
71+
}
72+
int from;
73+
int to;
74+
std::uint64_t factorIndice;
75+
};
76+
77+
std::vector<ConstraintToFactor> lastAddedConstraints_;
78+
int lastSwitchId_;
79+
std::set<int> addedPoses_;
80+
std::pair<int, std::uint64_t> lastRootFactorIndex_;
6481
};
6582

6683
} /* namespace rtabmap */

‎corelib/src/CameraThread.cpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -292,6 +292,11 @@ bool CameraThread::odomProvided() const
292292
void CameraThread::mainLoopBegin()
293293
{
294294
ULogger::registerCurrentThread("Camera");
295+
if(_imuFilter)
296+
{
297+
// In case we paused the camera and moved somewhere else, restart filtering.
298+
_imuFilter->reset();
299+
}
295300
_camera->resetTimer();
296301
}
297302

‎corelib/src/GlobalMap.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -121,7 +121,7 @@ bool GlobalMap::update(const std::map<int, Transform> & poses)
121121
}
122122
else
123123
{
124-
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);
124+
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);
125125
}
126126
}
127127

‎corelib/src/Rtabmap.cpp‎

Lines changed: 39 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1094,6 +1094,13 @@ void Rtabmap::resetMemory()
10941094
{
10951095
UERROR("RTAB-Map is not initialized. No memory to reset...");
10961096
}
1097+
1098+
if(_graphOptimizer)
1099+
{
1100+
delete _graphOptimizer;
1101+
_graphOptimizer = Optimizer::create(_parameters);
1102+
}
1103+
10971104
this->setupLogFiles(true);
10981105
}
10991106

@@ -1177,6 +1184,7 @@ bool Rtabmap::process(
11771184
double timeMemoryUpdate = 0;
11781185
double timeNeighborLinkRefining = 0;
11791186
double timeProximityByTimeDetection = 0;
1187+
double timeProximityBySpaceSearch = 0;
11801188
double timeProximityBySpaceVisualDetection = 0;
11811189
double timeProximityBySpaceDetection = 0;
11821190
double timeCleaningNeighbors = 0;
@@ -2598,22 +2606,39 @@ bool Rtabmap::process(
25982606
// 1) compare visually with nearest locations
25992607
//
26002608
UDEBUG("Proximity detection (local loop closure in SPACE using matching images, local radius=%fm)", _localRadius);
2601-
std::map<int, float> nearestIds;
2602-
if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
2603-
{
2604-
nearestIds = _memory->getNeighborsIdRadius(signature->id(), _localRadius, _optimizedPoses, _proximityMaxGraphDepth);
2605-
}
2606-
else
2607-
{
2608-
nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
2609-
}
2609+
std::map<int, float> nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
26102610
UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size());
26112611
std::map<int, Transform> nearestPoses;
2612+
std::multimap<int, int> links;
2613+
if(_memory->isIncremental() && _proximityMaxGraphDepth>0)
2614+
{
2615+
// get bidirectional links
2616+
for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter)
2617+
{
2618+
if(uContains(_optimizedPoses, iter->second.from()) && uContains(_optimizedPoses, iter->second.to()))
2619+
{
2620+
links.insert(std::make_pair(iter->second.from(), iter->second.to()));
2621+
links.insert(std::make_pair(iter->second.to(), iter->second.from())); // <->
2622+
}
2623+
}
2624+
}
26122625
for(std::map<int, float>::iterator iter=nearestIds.lower_bound(1); iter!=nearestIds.end(); ++iter)
26132626
{
26142627
if(_memory->getStMem().find(iter->first) == _memory->getStMem().end())
26152628
{
2616-
nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
2629+
if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
2630+
{
2631+
std::list<std::pair<int, Transform> > path = graph::computePath(_optimizedPoses, links, signature->id(), iter->first);
2632+
UDEBUG("Graph depth to %d = %ld", iter->first, path.size());
2633+
if(!path.empty() && (int)path.size() <= _proximityMaxGraphDepth)
2634+
{
2635+
nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
2636+
}
2637+
}
2638+
else
2639+
{
2640+
nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
2641+
}
26172642
}
26182643
}
26192644
UDEBUG("nearestPoses=%d", (int)nearestPoses.size());
@@ -2645,6 +2670,9 @@ bool Rtabmap::process(
26452670
}
26462671
UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths);
26472672

2673+
timeProximityBySpaceSearch = timer.ticks();
2674+
ULOGGER_INFO("timeProximityBySpaceSearch=%fs", timeProximityBySpaceSearch);
2675+
26482676
float proximityFilteringRadius = _proximityFilteringRadius;
26492677
if(_maxLoopClosureDistance>0.0f && (proximityFilteringRadius <= 0.0f || _maxLoopClosureDistance<proximityFilteringRadius))
26502678
{
@@ -4072,6 +4100,7 @@ bool Rtabmap::process(
40724100
statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
40734101
statistics_.addStatistic(Statistics::kTimingNeighbor_link_refining(), timeNeighborLinkRefining*1000);
40744102
statistics_.addStatistic(Statistics::kTimingProximity_by_time(), timeProximityByTimeDetection*1000);
4103+
statistics_.addStatistic(Statistics::kTimingProximity_by_space_search(), timeProximityBySpaceSearch*1000);
40754104
statistics_.addStatistic(Statistics::kTimingProximity_by_space_visual(), timeProximityBySpaceVisualDetection*1000);
40764105
statistics_.addStatistic(Statistics::kTimingProximity_by_space(), timeProximityBySpaceDetection*1000);
40774106
statistics_.addStatistic(Statistics::kTimingReactivation(), timeReactivations*1000);

0 commit comments

Comments
 (0)