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