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

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

@@ -358,6 +358,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 3.0, uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
RTABMAP_PARAM(RGBD, MaxLoopClosureDistance, float, 0.0, "Reject loop closures/localizations if the distance from the map is over this distance (0=disabled).");
RTABMAP_PARAM(RGBD, ForceOdom3DoF, bool, true, uFormat("Force odometry pose to be 3DoF if %s=true.", kRegForce3DoF().c_str()));
RTABMAP_PARAM(RGBD, StartAtOrigin, bool, false, uFormat("If true, rtabmap will assume the robot is starting from origin of the map. If false, rtabmap will assume the robot is restarting from the last saved localization pose from previous session (the place where it shut down previously). Used only in localization mode (%s=false).", kMemIncrementalMemory().c_str()));
RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m).");
RTABMAP_PARAM(RGBD, PlanStuckIterations, int, 0, "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");

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

@@ -322,6 +322,7 @@ private:
int _pathStuckIterations;
float _pathLinearVelocity;
float _pathAngularVelocity;
bool _forceOdom3doF;
bool _restartAtOrigin;
bool _loopCovLimited;
bool _loopGPS;

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())
{