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

Added ViewPlane XY, XZ and YZ options to GraphViewer

Browse Source 
Parameter RGBD/SavedLocalizationIgnored now called RGBD/StartAtOrigin (updated description, used only in localization mode)
IcpReg: if Force4DoF, set lower covariance values for roll and pitch
DbViewer: updated detectMoreLoopClosures with new minRadius option and update optimized poses between each new accepted loop closures.
MainWindow: suppressed warning if depth image is not found in current node data (when rgb is).
3D Map view: changed default map point size to 1 (was 2)
This commit is contained in:
matlabbe
2021-03-12 17:47:36 -05:00
parent da2e2f810c
commit f6e17be2b4
13 changed files with 360 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -356,7 +356,7 @@ class RTABMAP_EXP 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, SavedLocalizationIgnored, bool, false, uFormat("Ignore last saved localization pose from previous session. If true, RTAB-Map won't assume it is restarting from the same place than where it shut down previously. In localization mode (%s=true), the robot will assume that it is starting from the origin of the map.", kMemIncrementalMemory().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.");
RTABMAP_PARAM(RGBD, PlanLinearVelocity, float, 0, "Linear velocity (m/sec) used to compute path weights.");

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

@@ -301,7 +301,7 @@ private:
int _pathStuckIterations;
float _pathLinearVelocity;
float _pathAngularVelocity;
bool _savedLocalizationIgnored;
bool _restartAtOrigin;
bool _loopCovLimited;
bool _loopGPS;
int _maxOdomCacheSize;

2
corelib/src/Parameters.cpp
View File

@@ -242,6 +242,8 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("OdomORBSLAM2/MaxFeatures", std::make_pair(true, Parameters::kOdomORBSLAMMaxFeatures())));
removedParameters_.insert(std::make_pair("OdomORBSLAM2/MapSize", std::make_pair(true, Parameters::kOdomORBSLAMMapSize())));
removedParameters_.insert(std::make_pair("RGBD/SavedLocalizationIgnored", std::make_pair(true, Parameters::kRGBDStartAtOrigin())));
// 0.20.
removedParameters_.insert(std::make_pair("SuperGlue/Path", std::make_pair(true, Parameters::kPyMatcherPath())));
removedParameters_.insert(std::make_pair("SuperGlue/Iterations", std::make_pair(true, Parameters::kPyMatcherIterations())));

9
corelib/src/RegistrationIcp.cpp
View File

@@ -1432,6 +1432,15 @@ Transform RegistrationIcp::computeTransformationImpl(
{
info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
info.covariance(cv::Range(3,6),cv::Range(3,6))/=10.0; //orientation error
if(_libpointmatcher &&
_pointToPlane &&
!force3DoF() &&
_libpointmatcherForce4DoF &&
!tooLowComplexityForPlaneToPlane)
{
// Force4DoF: Assume roll and pitch more accurate (IMU)
info.covariance(cv::Range(3,5),cv::Range(3,5))/=10.0;
}
}
info.icpInliersRatio = correspondencesRatio;
info.icpCorrespondences = correspondences;

73
corelib/src/Rtabmap.cpp
View File

@@ -126,7 +126,7 @@ Rtabmap::Rtabmap() :
_pathStuckIterations(Parameters::defaultRGBDPlanStuckIterations()),
_pathLinearVelocity(Parameters::defaultRGBDPlanLinearVelocity()),
_pathAngularVelocity(Parameters::defaultRGBDPlanAngularVelocity()),
_savedLocalizationIgnored(Parameters::defaultRGBDSavedLocalizationIgnored()),
_restartAtOrigin(Parameters::defaultRGBDStartAtOrigin()),
_loopCovLimited(Parameters::defaultRGBDLoopCovLimited()),
_loopGPS(Parameters::defaultRtabmapLoopGPS()),
_maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()),
@@ -337,36 +337,43 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
this->parseParameters(allParameters);
Transform lastPose;
_optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
if(!_optimizedPoses.empty())
_optimizedPoses.clear();
if(!_memory->isIncremental())
{
if(_savedLocalizationIgnored)
_optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
if(!_optimizedPoses.empty())
{
UDEBUG("lastPose is ignored (%s=true), assuming we start at the origin of the map.", Parameters::kRGBDSavedLocalizationIgnored().c_str());
lastPose.setIdentity();
if(_restartAtOrigin)
{
UINFO("lastPose is ignored (%s=true), assuming we start at the origin of the map.", Parameters::kRGBDStartAtOrigin().c_str());
lastPose.setIdentity();
}
_lastLocalizationPose = lastPose;
UINFO("Loaded optimizedPoses=%d lastPose=%s", _optimizedPoses.size(), _lastLocalizationPose.prettyPrint().c_str());
std::map<int, Transform> tmp;
// Get just the links
_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false, true);
// Initialize Bayes' prediction matrix
UTimer time;
std::map<int, float> likelihood;
likelihood.insert(std::make_pair(Memory::kIdVirtual, 1));
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{
if(_memory->getSignature(iter->first))
{
likelihood.insert(std::make_pair(iter->first, 0));
}
}
_bayesFilter->computePosterior(_memory, likelihood);
UINFO("Time initializing Bayes' prediction with %ld nodes: %fs", _optimizedPoses.size(), time.ticks());
}
_lastLocalizationPose = lastPose;
UINFO("Loaded optimizedPoses=%d lastPose=%s", _optimizedPoses.size(), _lastLocalizationPose.prettyPrint().c_str());
std::map<int, Transform> tmp;
// Get just the links
_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false, true);
// Initialize Bayes' prediction matrix
UTimer time;
std::map<int, float> likelihood;
likelihood.insert(std::make_pair(Memory::kIdVirtual, 1));
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
else
{
likelihood.insert(std::make_pair(iter->first, 0));
UINFO("Loaded optimizedPoses=0, last localization pose is ignored!");
}
_bayesFilter->computePosterior(_memory, likelihood);
UINFO("Time initializing Bayes' prediction with %ld nodes: %fs", _optimizedPoses.size(), time.ticks());
}
else
{
UINFO("Loaded optimizedPoses=0, last localization pose is ignored!");
}
if(_databasePath.empty())
@@ -543,7 +550,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::kRGBDSavedLocalizationIgnored(), _savedLocalizationIgnored);
Parameters::parse(parameters, Parameters::kRGBDStartAtOrigin(), _restartAtOrigin);
Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited);
Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS);
Parameters::parse(parameters, Parameters::kRGBDMaxOdomCacheSize(), _maxOdomCacheSize);
@@ -750,7 +757,7 @@ int Rtabmap::triggerNewMap()
if(!_memory->isIncremental())
{
if(_savedLocalizationIgnored)
if(_restartAtOrigin)
{
_mapCorrection.setIdentity();
_lastLocalizationPose.setIdentity();
@@ -2268,7 +2275,15 @@ bool Rtabmap::process(
_rgbdSlamMode &&
signature->getWeight() >= 0) // not an intermediate node
{
if(_graphOptimizer->iterations() == 0)
if(_startNewMapOnLoopClosure &&
_memory->getWorkingMem().size()>=2 && // must have an old map (+1 virtual place)
graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0) // alone in new session)
{
UINFO("Proximity detection by space disabled as if we force to have a global loop "
"closure with previous map before doing proximity detections (%s=true).",
Parameters::kRtabmapStartNewMapOnLoopClosure().c_str());
}
else if(_graphOptimizer->iterations() == 0)
{
UWARN("Cannot do local loop closure detection in space if graph optimization is disabled!");
}

2
corelib/src/util3d_filtering.cpp
View File

@@ -226,7 +226,7 @@ LaserScan commonFiltering(
{
if(scan.hasNormals())
{
UWARN("Voxel filter i applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
UWARN("Voxel filter is applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
}
if(scan.is2d())
{