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

fixed https://github.com/introlab/rtabmap_ros/issues/1134

Browse Source
This commit is contained in:
matlabbe
2024-03-31 13:06:52 -07:00
parent a0476af896
commit 8612ebba93
5 changed files with 433 additions and 402 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
corelib/src/Rtabmap.cpp
View File

@@ -143,6 +143,7 @@ Rtabmap::Rtabmap() :
_pathStuckIterations(Parameters::defaultRGBDPlanStuckIterations()),
_pathLinearVelocity(Parameters::defaultRGBDPlanLinearVelocity()),
_pathAngularVelocity(Parameters::defaultRGBDPlanAngularVelocity()),
_forceOdom3doF(Parameters::defaultRGBDForceOdom3DoF()),
_restartAtOrigin(Parameters::defaultRGBDStartAtOrigin()),
_loopCovLimited(Parameters::defaultRGBDLoopCovLimited()),
_loopGPS(Parameters::defaultRtabmapLoopGPS()),
@@ -616,6 +617,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations);
Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity);
Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
Parameters::parse(parameters, Parameters::kRGBDForceOdom3DoF(), _forceOdom3doF);
Parameters::parse(parameters, Parameters::kRGBDStartAtOrigin(), _restartAtOrigin);
Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited);
Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS);
@@ -1252,6 +1254,12 @@ bool Rtabmap::process(
{
if(!odomPose.isNull())
{
// If we are doing 2D mapping, make sure the pose is 3DoF so that landmark logic works.
if(_forceOdom3doF && _graphOptimizer->isSlam2d() && !odomPose.is3DoF())
{
odomPose = odomPose.to3DoF();
}
// this will make sure that all inverse operations will work!
if(!odomPose.isInvertible())
{