i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

iSAM2 integration (#1249)

Browse Source 
* 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
This commit is contained in:
matlabbe
2024-03-27 17:18:02 -07:00
committed by GitHub
parent bfeb487dba
commit cc17ebe92b
10 changed files with 450 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/include/rtabmap/core/Parameters.h
View File

@@ -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");

1
corelib/include/rtabmap/core/Statistics.h
View File

@@ -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);

33
corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h
View File

@@ -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 @@ public:
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 @@ public:
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 */