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

Added RGBD/ProximityMergedScanCovFactor parameter. Localization: fixed output height when Reg/Force3DoF=true but input poses are 6DoF. Transform: added is3DoF() and is4DoF() functions. GTSAM: when Reg/Force3DoF=true, copy input roll,pitch,z values for output poses. RegIcp: fixed working memory dir '~' conversion. RGBD/ProximityGlobalScanMap: fixed map::at error when some nodes don't have scans.

Browse Source
This commit is contained in:
matlabbe
2022-01-12 16:14:42 -05:00
parent a4ec95963e
commit fcec98105d
14 changed files with 122 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Optimizer.h
+2 -1
View File
@@ -75,7 +75,8 @@ public:
const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & linksIn,
std::map<int, Transform> & posesOut,
std::multimap<int, Link> & linksOut) const;
std::multimap<int, Link> & linksOut,
bool adjustPosesWithConstraints = true) const;
public:
virtual ~Optimizer() {}
corelib/include/rtabmap/core/Parameters.h
+1
View File
@@ -386,6 +386,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, ProximityAngle, float, 45, "Maximum angle (degrees) for one-to-one proximity detection.");
RTABMAP_PARAM(RGBD, ProximityOdomGuess, bool, false, "Use odometry as motion guess for one-to-one proximity detection.");
RTABMAP_PARAM(RGBD, ProximityGlobalScanMap, bool, false, uFormat("Create a global assembled map from laser scans for one-to-many proximity detection, replacing the original one-to-many proximity detection (i.e., detection against local paths). Only used in localization mode (%s=false), otherwise original one-to-many proximity detection is done. Note also that if graph is modified (i.e., memory management is enabled or robot jumps from one disjoint session to another in same database), the global scan map is cleared and one-to-many proximity detection is reverted to original approach.", kMemIncrementalMemory().c_str()));
RTABMAP_PARAM(RGBD, ProximityMergedScanCovFactor, double, 100.0, uFormat("Covariance factor for one-to-many proximity detection (when %s>0 and scans are used).", kRGBDProximityPathMaxNeighbors().c_str()));
// Graph optimization
#ifdef RTABMAP_GTSAM
corelib/include/rtabmap/core/Rtabmap.h
+1
View File
@@ -307,6 +307,7 @@ private:
bool _proximityRawPosesUsed;
float _proximityAngle;
bool _proximityOdomGuess;
double _proximityMergedScanCovFactor;
std::string _databasePath;
bool _optimizeFromGraphEnd;
float _optimizationMaxError;
corelib/include/rtabmap/core/Transform.h
+2
View File
@@ -104,6 +104,8 @@ public:
Transform translation() const;
Transform to3DoF() const;
Transform to4DoF() const;
bool is3DoF() const;
bool is4DoF() const;
cv::Mat rotationMatrix() const;
cv::Mat translationMatrix() const;
corelib/src/Graph.cpp
+1 -1
View File
@@ -1336,7 +1336,7 @@ std::map<int, Transform> radiusPosesFiltering(
//pcl::IndicesPtr indicesOut(new std::vector<int>);
//indicesOut->insert(indicesOut->end(), indicesKept.begin(), indicesKept.end());
UINFO("Cloud filtered In = %d, Out = %d", cloud->size(), indicesKept.size());
UINFO("Cloud filtered In = %d, Out = %d (radius=%f angle=%f keepLatest=%d)", cloud->size(), indicesKept.size(), radius, angle, keepLatest?1:0);
//pcl::io::savePCDFile("duplicateIn.pcd", *cloud);
//pcl::io::savePCDFile("duplicateOut.pcd", *cloud, *indicesOut);
corelib/src/Optimizer.cpp
+17 -9
View File
@@ -190,7 +190,8 @@ void Optimizer::getConnectedGraph(
const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & linksIn,
std::map<int, Transform> & posesOut,
std::multimap<int, Link> & linksOut) const
std::multimap<int, Link> & linksOut,
bool adjustPosesWithConstraints) const
{
UDEBUG("IN: fromId=%d poses=%d links=%d priorsIgnored=%d landmarksIgnored=%d", fromId, (int)posesIn.size(), (int)linksIn.size(), priorsIgnored()?1:0, landmarksIgnored()?1:0);
UASSERT(fromId>0);
@@ -243,23 +244,30 @@ void Optimizer::getConnectedGraph(
{
if(!uContains(posesOut, toId))
{
if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0)
if(adjustPosesWithConstraints)
{
Transform t;
if(kter->second.from()==currentId)
if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0)
{
t = kter->second.transform();
Transform t;
if(kter->second.from()==currentId)
{
t = kter->second.transform();
}
else
{
t = kter->second.transform().inverse();
}
posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
}
else
{
t = kter->second.transform().inverse();
Transform t = posesOut.at(currentId) * (kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse());
posesOut.insert(std::make_pair(toId, t));
}
posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
}
else
{
Transform t = posesOut.at(currentId) * (kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse());
posesOut.insert(std::make_pair(toId, t));
posesOut.insert(*posesIn.find(toId));
}
// add prior links
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)
corelib/src/RegistrationIcp.cpp
+1 -1
View File
@@ -136,7 +136,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
{
_workingDir = iter->second;
_workingDir = uReplaceChar(iter->second, '~', UDirectory::homeDir());
}
bool pointToPlane = _pointToPlane;
corelib/src/Rtabmap.cpp
+29 -15
View File
@@ -121,6 +121,7 @@ Rtabmap::Rtabmap() :
_proximityRawPosesUsed(Parameters::defaultRGBDProximityPathRawPosesUsed()),
_proximityAngle(Parameters::defaultRGBDProximityAngle()*M_PI/180.0f),
_proximityOdomGuess(Parameters::defaultRGBDProximityOdomGuess()),
_proximityMergedScanCovFactor(Parameters::defaultRGBDProximityMergedScanCovFactor()),
_databasePath(""),
_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
_optimizationMaxError(Parameters::defaultRGBDOptimizeMaxError()),
@@ -573,6 +574,9 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
_proximityAngle *= M_PI/180.0f;
}
Parameters::parse(parameters, Parameters::kRGBDProximityOdomGuess(), _proximityOdomGuess);
Parameters::parse(parameters, Parameters::kRGBDProximityMergedScanCovFactor(), _proximityMergedScanCovFactor);
UASSERT(_proximityMergedScanCovFactor>0.0);
bool optimizeFromGraphEndPrevious = _optimizeFromGraphEnd;
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
if(optimizeFromGraphEndPrevious != _optimizeFromGraphEnd && !_optimizedPoses.empty())
@@ -2586,7 +2590,7 @@ bool Rtabmap::process(
// Assemble scans in the path and do ICP only
std::map<int, Transform> optimizedLocalPath;
if(_proximityRawPosesUsed)
if(_globalScanMap.empty() && _proximityRawPosesUsed)
{
//optimize the path's poses locally
cv::Mat covariance;
@@ -2610,7 +2614,7 @@ bool Rtabmap::process(
}
std::map<int, Transform> filteredPath;
if(optimizedLocalPath.size() > 2 && proximityFilteringRadius > 0.0f)
if(_globalScanMap.empty() && optimizedLocalPath.size() > 2 && proximityFilteringRadius > 0.0f)
{
// path filtering
filteredPath = graph::radiusPosesFiltering(optimizedLocalPath, proximityFilteringRadius, 0, true);
@@ -2639,6 +2643,7 @@ bool Rtabmap::process(
}
else
{
UASSERT_MSG(_globalScanMapPoses.find(nearestId) != _globalScanMapPoses.end(), uFormat("Pose of %d not found in global scan poses", nearestId).c_str());
icpMulti = false;
// use pre-assembled scan map
SensorData assembledData;
@@ -2684,7 +2689,7 @@ bool Rtabmap::process(
// set Identify covariance for laser scan matching only
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, getInformation(info.covariance)/100.0, scanMatchingIds));
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, getInformation(info.covariance)/_proximityMergedScanCovFactor, scanMatchingIds));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
if(icpMulti)
@@ -2925,7 +2930,8 @@ bool Rtabmap::process(
bool priorsIgnored = _graphOptimizer->priorsIgnored();
UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false");
_graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut);
// If slam2d: get connected graph while keeping original roll,pitch,z values.
_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut, !_graphOptimizer->isSlam2d());
std::map<int, Transform> optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut);
_graphOptimizer->setPriorsIgnored(priorsIgnored); // set back
for(std::map<int, Transform>::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter)
@@ -3067,12 +3073,7 @@ bool Rtabmap::process(
Transform oldPose = _optimizedPoses.at(localizationLinks.rbegin()->first);
Transform mapCorrectionInv = _mapCorrection.inverse();
Transform u = signature->getPose() * localizationLinks.rbegin()->second.transform();
if(_graphOptimizer->isSlam2d())
{
// in case of 3d landmarks, transform constraint to 2D
u = u.to3DoF();
}
else if(_graphOptimizer->gravitySigma() > 0)
if(_graphOptimizer->gravitySigma() > 0)
{
// Adjust transform with gravity
Transform transform = localizationLinks.rbegin()->second.transform();
@@ -3122,6 +3123,11 @@ bool Rtabmap::process(
}
}
Transform up = u * oldPose.inverse();
if(_graphOptimizer->isSlam2d())
{
// in case of 3d landmarks, transform constraint to 2D
up.to3DoF();
}
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{
iter->second = mapCorrectionInv * up * iter->second;
@@ -3132,7 +3138,7 @@ bool Rtabmap::process(
{
Transform newPose = _optimizedPoses.at(localizationLinks.rbegin()->first) * localizationLinks.rbegin()->second.transform().inverse();
UDEBUG("newPose=%s", newPose.prettyPrint().c_str());
if(_graphOptimizer->isSlam2d())
if(_graphOptimizer->isSlam2d() && signature->getPose().is3DoF())
{
// in case of 3d landmarks, transform constraint to 2D
newPose = newPose.to3DoF();
@@ -3607,10 +3613,8 @@ bool Rtabmap::process(
Signature lastSignatureData(signature->id());
Transform lastSignatureLocalizedPose;
if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end() &&
graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size())
if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end())
{
// only if localized set it
lastSignatureLocalizedPose = _optimizedPoses.at(signature->id());
}
if(_publishLastSignatureData)
@@ -3762,8 +3766,8 @@ bool Rtabmap::process(
if(signaturesRemoved.size() == 1 && signaturesRemoved.front() == lastSignatureData.id())
{
UDEBUG("Detected that only last signature has been removed");
int lastId = signaturesRemoved.front();
UDEBUG("Detected that only last signature has been removed (lastId=%d)", lastId);
_optimizedPoses.erase(lastId);
for(std::multimap<int, Link>::iterator iter=_constraints.find(lastId); iter!=_constraints.end() && iter->first==lastId;++iter)
{
@@ -6319,6 +6323,16 @@ void Rtabmap::createGlobalScanMap()
break;
}
}
else
{
UDEBUG("Ignored %d (scan is empty), pose still added.", iter->first);
_globalScanMapPoses.insert(*iter);
}
}
else
{
UDEBUG("Ignored %d (no scan), pose still added.", iter->first);
_globalScanMapPoses.insert(*iter);
}
}
if(_globalScanMap.size() > 3)
corelib/src/Transform.cpp
+14
View File
@@ -221,6 +221,20 @@ Transform Transform::to4DoF() const
return Transform(x,y,z, 0,0,yaw);
}
bool Transform::is3DoF() const
{
return is4DoF() && z() == 0.0;
}
bool Transform::is4DoF() const
{
return r13() == 0.0 &&
r23() == 0.0 &&
r31() == 0.0 &&
r32() == 0.0 &&
r33() == 0.0;
}
cv::Mat Transform::rotationMatrix() const
{
return data_.colRange(0, 3).clone();
corelib/src/optimizer/OptimizerG2O.cpp
+1 -1
View File
@@ -359,7 +359,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
int landmarkVertexOffset = poses.rbegin()->first+1;
std::map<int, bool> isLandmarkWithRotation;
UDEBUG("fill poses to g2o... (rootId=%d hasGravityConstraints=%d)", rootId, hasGravityConstraints?1:0);
UDEBUG("fill poses to g2o... (rootId=%d hasGravityConstraints=%d isSlam2d=%d)", rootId, hasGravityConstraints?1:0, isSlam2d()?1:0);
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
corelib/src/optimizer/OptimizerGTSAM.cpp
+2 -1
View File
@@ -625,8 +625,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
if(key > 0)
{
poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta())));
optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta())));
}
else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
{
guilib/src/MainWindow.cpp
+1
View File
@@ -2348,6 +2348,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
}
#endif
UDEBUG("%d %d %d", poses.size(), poses.size()?poses.rbegin()->first:0, stat.refImageId());
if(!_odometryReceived && poses.size() && poses.rbegin()->first == stat.refImageId())
{
if(poses.rbegin()->first == stat.getLastSignatureData().id())
guilib/src/PreferencesDialog.cpp
+1
View File
@@ -1121,6 +1121,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->localDetection_maxPaths->setObjectName(Parameters::kRGBDProximityMaxPaths().c_str());
_ui->localDetection_pathFilteringRadius->setObjectName(Parameters::kRGBDProximityPathFilteringRadius().c_str());
_ui->localDetection_angle->setObjectName(Parameters::kRGBDProximityAngle().c_str());
_ui->localDetection_mergedScanCovFactor->setObjectName(Parameters::kRGBDProximityMergedScanCovFactor().c_str());
_ui->checkBox_localSpaceOdomGuess->setObjectName(Parameters::kRGBDProximityOdomGuess().c_str());
_ui->checkBox_localSpacePathOdomPosesUsed->setObjectName(Parameters::kRGBDProximityPathRawPosesUsed().c_str());
_ui->rgdb_localImmunizationRatio->setObjectName(Parameters::kRGBDLocalImmunizationRatio().c_str());
guilib/src/ui/preferencesDialog.ui
+49 -14
View File
@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-650</y>
<y>0</y>
<width>756</width>
<height>3623</height>
</rect>
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>12</number>
<number>13</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -11396,7 +11396,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="4" column="1">
<item row="5" column="1">
<widget class="QLabel" name="label_scanMatching_13">
<property name="text">
<string>Use odometry as motion guess for one-to-one proximity detection.</string>
@@ -11419,7 +11419,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="3" column="0">
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="localDetection_angle">
<property name="suffix">
<string> deg</string>
@@ -11435,14 +11435,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="4" column="0">
<item row="5" column="0">
<widget class="QCheckBox" name="checkBox_localSpaceOdomGuess">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="1">
<item row="6" column="1">
<widget class="QLabel" name="label_space3_3">
<property name="text">
<string>Path filtering radius to reduce the number of nodes to compare in a path in one-to-many proximity detection. The nearest node in a path should be inside that radius to be considered for one-to-one proximity detection.</string>
@@ -11478,7 +11478,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="6" column="1">
<item row="7" column="1">
<widget class="QLabel" name="label_scanMatching_4">
<property name="text">
<string>When comparing to a local path for one-to-many proximity detection, merge the scans using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.</string>
@@ -11504,7 +11504,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="5" column="0">
<item row="6" column="0">
<widget class="QDoubleSpinBox" name="localDetection_pathFilteringRadius">
<property name="suffix">
<string> m</string>
@@ -11530,14 +11530,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="6" column="0">
<item row="7" column="0">
<widget class="QCheckBox" name="checkBox_localSpacePathOdomPosesUsed">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="8" column="1">
<item row="9" column="1">
<widget class="QLabel" name="label_scanMatching_6">
<property name="text">
<string>Save scan matching IDs from one-to-many proximity detection in link's user data.</string>
@@ -11550,7 +11550,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="3" column="1">
<item row="4" column="1">
<widget class="QLabel" name="label_scanMatching_8">
<property name="text">
<string>Maximum angle (degrees) for one-to-one proximity detection.</string>
@@ -11563,14 +11563,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="8" column="0">
<item row="9" column="0">
<widget class="QCheckBox" name="checkBox_localSpaceScanMatchingIDsSaved">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="9" column="1">
<item row="10" column="1">
<widget class="QLabel" name="label_scanMatching_15">
<property name="text">
<string>Create a global assembled map from laser scans for one-to-many proximity detection, replacing the original one-to-many proximity detection (i.e., detection against local paths). Only used in localization mode, otherwise original one-to-many proximity detection is done. Note also that if graph is modified (i.e., memory management is enabled or robot jumps from one disjoint session to another in same database), the global scan map is cleared and one-to-many proximity detection is reverted to original approach.</string>
@@ -11583,13 +11583,48 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="9" column="0">
<item row="10" column="0">
<widget class="QCheckBox" name="checkBox_localSpaceCreateGlobalScanMap">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_space3_9">
<property name="text">
<string>Covariance factor for one-to-many proximity detection (when Maximum neighbor nodes&gt;0 and scans are used).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="localDetection_mergedScanCovFactor">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="minimum">
<double>1.000000000000000</double>
</property>
<property name="maximum">
<double>10000.000000000000000</double>
</property>
<property name="singleStep">
<double>10.000000000000000</double>
</property>
<property name="value">
<double>100.000000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
</layout>