i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Local scan matching: set larger scan points for max scan points when it is not set. Fixed missing scan Ids in links' user data to correctly visualize proximity links by space in DatabaseViewer. CloudViewer: using line instead of arrow between the referential and the frustum. removed parameter "RGBD/ProximityPathScansMerged" as visual proximity by space already does that.

Browse Source
This commit is contained in:
matlabbe
2016-05-20 11:44:50 -04:00
parent b29ce28877
commit 9f6af75f79
11 changed files with 109 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Parameters.h
-1
View File
@@ -323,7 +323,6 @@ class RTABMAP_EXP Parameters
// Local/Proximity loop closure detection
RTABMAP_PARAM(RGBD, ProximityByTime, bool, false, "Detection over all locations in STM.");
RTABMAP_PARAM(RGBD, ProximityBySpace, bool, true, "Detection over locations (in Working Memory or STM) near in space.");
RTABMAP_PARAM(RGBD, ProximityPathScansMerged, bool, true, "Merge close laser scans on each path. If false, only the nearest laser scan on the path is used for ICP.");
RTABMAP_PARAM(RGBD, ProximityMaxGraphDepth, int, 50, "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 0.5, "Path filtering radius.");
RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed, bool, true, "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
corelib/include/rtabmap/core/Rtabmap.h
-1
View File
@@ -196,7 +196,6 @@ private:
int _proximityMaxGraphDepth;
float _proximityFilteringRadius;
bool _proximityRawPosesUsed;
bool _proximityScansMerged;
float _proximityAngle;
std::string _databasePath;
bool _optimizeFromGraphEnd;
corelib/src/Memory.cpp
+6 -1
View File
@@ -2290,6 +2290,7 @@ Transform Memory::computeIcpTransformMulti(
SensorData assembledData;
Transform toPose = poses.at(toId);
std::string msg;
int maxPoints = fromScan.cols;
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
@@ -2301,6 +2302,10 @@ Transform Memory::computeIcpTransformMulti(
cv::Mat scan;
s->sensorData().uncompressData(0, 0, &scan);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, toPose.inverse() * iter->second);
if(scan.cols > maxPoints)
{
maxPoints = scan.cols;
}
*assembledToClouds += *cloud;
}
else
@@ -2311,7 +2316,7 @@ Transform Memory::computeIcpTransformMulti(
}
if(assembledToClouds->size())
{
assembledData.setLaserScanRaw(util3d::laserScanFromPointCloud(*assembledToClouds, Transform()), fromS->sensorData().laserScanMaxPts(), fromS->sensorData().laserScanMaxRange());
assembledData.setLaserScanRaw(util3d::laserScanFromPointCloud(*assembledToClouds, Transform()), fromS->sensorData().laserScanMaxPts()?fromS->sensorData().laserScanMaxPts():maxPoints, fromS->sensorData().laserScanMaxRange());
}
Transform guess = poses.at(fromId).inverse() * poses.at(toId);
corelib/src/Parameters.cpp
+6 -3
View File
@@ -155,14 +155,17 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
{
// removed parameters
// 0.11.6
removedParameters_.insert(std::make_pair("RGBD/ProximityPathScansMerged", std::make_pair(false, "")));
// 0.11.3
removedParameters_.insert(std::make_pair("Mem/ImageDecimation", std::make_pair(true, Parameters::kMemImagePostDecimation())));
removedParameters_.insert(std::make_pair("Mem/ImageDecimation", std::make_pair(true, Parameters::kMemImagePostDecimation())));
// 0.11.2
removedParameters_.insert(std::make_pair("OdomLocalMap/HistorySize", std::make_pair(true, Parameters::kOdomF2MMaxSize())));
removedParameters_.insert(std::make_pair("OdomLocalMap/FixedMapPath", std::make_pair(true, Parameters::kOdomF2MFixedMapPath())));
removedParameters_.insert(std::make_pair("OdomF2F/GuessMotion", std::make_pair(true, Parameters::kOdomGuessMotion())));
removedParameters_.insert(std::make_pair("OdomF2F/KeyFrameThr", std::make_pair(false, Parameters::kOdomKeyFrameThr())));
removedParameters_.insert(std::make_pair("OdomF2F/KeyFrameThr", std::make_pair(false, Parameters::kOdomKeyFrameThr())));
// 0.11.0
removedParameters_.insert(std::make_pair("OdomBow/LocalHistorySize", std::make_pair(true, Parameters::kOdomF2MMaxSize())));
@@ -244,7 +247,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionBySpace", std::make_pair(true, Parameters::kRGBDProximityBySpace())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionTime", std::make_pair(true, Parameters::kRGBDProximityByTime())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionSpace", std::make_pair(true, Parameters::kRGBDProximityBySpace())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathScansMerged", std::make_pair(true, Parameters::kRGBDProximityPathScansMerged())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathScansMerged", std::make_pair(false, "")));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionMaxGraphDepth", std::make_pair(true, Parameters::kRGBDProximityMaxGraphDepth())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathFilteringRadius", std::make_pair(true, Parameters::kRGBDProximityPathFilteringRadius())));
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathRawPosesUsed", std::make_pair(true, Parameters::kRGBDProximityPathRawPosesUsed())));
corelib/src/RegistrationIcp.cpp
+8 -3
View File
@@ -113,7 +113,7 @@ Transform RegistrationIcp::computeTransformationImpl(
if(!guess.isNull() && !dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
{
// ICP with guess transform
int maxLaserScans = dataTo.laserScanMaxPts();
int maxLaserScans = dataTo.laserScanMaxPts()?dataTo.laserScanMaxPts():dataFrom.laserScanMaxPts();
cv::Mat fromScan = dataFrom.laserScanRaw();
cv::Mat toScan = dataTo.laserScanRaw();
if(_downsamplingStep>1)
@@ -309,8 +309,13 @@ Transform RegistrationIcp::computeTransformationImpl(
}
else
{
UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute!",
dataTo.id());
static bool warningShown = false;
if(!warningShown)
{
UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute! This message will only appear once.",
dataTo.id());
warningShown = true;
}
correspondencesRatio = float(correspondences)/float(toScan.cols>fromScan.cols?toScan.cols:fromScan.cols);
}
corelib/src/Rtabmap.cpp
+23 -38
View File
@@ -100,7 +100,6 @@ Rtabmap::Rtabmap() :
_proximityMaxGraphDepth(Parameters::defaultRGBDProximityMaxGraphDepth()),
_proximityFilteringRadius(Parameters::defaultRGBDProximityPathFilteringRadius()),
_proximityRawPosesUsed(Parameters::defaultRGBDProximityPathRawPosesUsed()),
_proximityScansMerged(Parameters::defaultRGBDProximityPathScansMerged()),
_proximityAngle(Parameters::defaultRGBDProximityAngle()*M_PI/180.0f),
_databasePath(""),
_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
@@ -411,7 +410,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDProximityMaxGraphDepth(), _proximityMaxGraphDepth);
Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), _proximityFilteringRadius);
Parameters::parse(parameters, Parameters::kRGBDProximityPathRawPosesUsed(), _proximityRawPosesUsed);
Parameters::parse(parameters, Parameters::kRGBDProximityPathScansMerged(), _proximityScansMerged);
Parameters::parse(parameters, Parameters::kRGBDProximityAngle(), _proximityAngle);
_proximityAngle *= M_PI/180.0f;
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
@@ -1901,47 +1899,37 @@ bool Rtabmap::process(
(_proximityFilteringRadius <= 0.0f ||
_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
{
if(!_proximityScansMerged)
{
//only keep the nearest node
std::map<int, Transform> tmp;
tmp.insert(*path.find(nearestId));
path = tmp;
}
else
// Assemble scans in the path and do ICP only
if(_proximityRawPosesUsed)
{
// Assemble scans in the path and do ICP only
if(_proximityRawPosesUsed)
//optimize the path's poses locally
path = optimizeGraph(nearestId, uKeysSet(path), std::map<int, Transform>(), false);
// transform local poses in optimized graph referential
UASSERT(uContains(path, nearestId));
Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
{
//optimize the path's poses locally
path = optimizeGraph(nearestId, uKeysSet(path), std::map<int, Transform>(), false);
// transform local poses in optimized graph referential
UASSERT(uContains(path, nearestId));
Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
{
jter->second = t * jter->second;
}
}
if(path.size() > 2 && _proximityFilteringRadius > 0.0f)
{
// path filtering
std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _proximityFilteringRadius, 0, true);
// make sure the current pose is still here
filteredPath.insert(*path.find(nearestId));
path = filteredPath;
jter->second = t * jter->second;
}
}
std::map<int, Transform> filteredPath = path;
if(path.size() > 2 && _proximityFilteringRadius > 0.0f)
{
// path filtering
filteredPath = graph::radiusPosesFiltering(path, _proximityFilteringRadius, 0, true);
// make sure the current pose is still here
filteredPath.insert(*path.find(nearestId));
}
if(path.size() > 0)
if(filteredPath.size() > 0)
{
// add current node to poses
path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
filteredPath.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
//The nearest will be the reference for a loop closure transform
if(signature->getLinks().find(nearestId) == signature->getLinks().end())
{
RegistrationInfo info;
Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, path, &info);
Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, filteredPath, &info);
if(!transform.isNull())
{
if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
@@ -1958,14 +1946,11 @@ bool Rtabmap::process(
stream << "SCANS:";
for(std::map<int, Transform>::iterator iter=path.begin(); iter!=path.end(); ++iter)
{
if(iter->first!=signature->id())
if(iter != path.begin())
{
if(iter != path.begin())
{
stream << ";";
}
stream << uNumber2Str(iter->first);
stream << ";";
}
stream << uNumber2Str(iter->first);
}
std::string scansStr = stream.str();
scanMatchingIds = cv::Mat(1, int(scansStr.size()+1), CV_8SC1, (void *)scansStr.c_str());
guilib/include/rtabmap/gui/CloudViewer.h
+6 -5
View File
@@ -155,13 +155,14 @@ public:
void removeCoordinate(const std::string & id);
void removeAllCoordinates();
void addOrUpdateArrow(
void addOrUpdateLine(
const std::string & id,
const Transform & from,
const Transform & to,
const QColor & color);
void removeArrow(const std::string & id);
void removeAllArrows();
const QColor & color,
bool arrow = false);
void removeLine(const std::string & id);
void removeAllLines();
void addOrUpdateFrustum(
const std::string & id,
@@ -291,7 +292,7 @@ private:
std::set<std::string> _graphes;
std::set<std::string> _coordinates;
std::set<std::string> _texts;
std::set<std::string> _arrows;
std::set<std::string> _lines;
std::set<std::string> _frustums;
pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
unsigned int _maxTrajectorySize;
guilib/src/CloudViewer.cpp
+23 -15
View File
@@ -172,7 +172,7 @@ void CloudViewer::clear()
this->removeAllClouds();
this->removeAllGraphs();
this->removeAllCoordinates();
this->removeAllArrows();
this->removeAllLines();
this->removeAllFrustums();
this->removeAllTexts();
this->clearTrajectory();
@@ -793,11 +793,12 @@ void CloudViewer::removeAllCoordinates()
UASSERT(_coordinates.empty());
}
void CloudViewer::addOrUpdateArrow(
void CloudViewer::addOrUpdateLine(
const std::string & id,
const Transform & from,
const Transform & to,
const QColor & color)
const QColor & color,
bool arrow)
{
if(id.empty())
{
@@ -805,11 +806,11 @@ void CloudViewer::addOrUpdateArrow(
return;
}
removeArrow(id);
removeLine(id);
if(!from.isNull() && !to.isNull())
{
_arrows.insert(id);
_lines.insert(id);
QColor c = Qt::gray;
if(color.isValid())
@@ -820,11 +821,18 @@ void CloudViewer::addOrUpdateArrow(
pcl::PointXYZ pt1(from.x(), from.y(), from.z());
pcl::PointXYZ pt2(to.x(), to.y(), to.z());
_visualizer->addArrow(pt2, pt1, c.redF(), c.greenF(), c.blueF(), false, id);
if(arrow)
{
_visualizer->addArrow(pt2, pt1, c.redF(), c.greenF(), c.blueF(), false, id);
}
else
{
_visualizer->addLine(pt2, pt1, c.redF(), c.greenF(), c.blueF(), id);
}
}
}
void CloudViewer::removeArrow(const std::string & id)
void CloudViewer::removeLine(const std::string & id)
{
if(id.empty())
{
@@ -832,21 +840,21 @@ void CloudViewer::removeArrow(const std::string & id)
return;
}
if(_arrows.find(id) != _arrows.end())
if(_lines.find(id) != _lines.end())
{
_visualizer->removeShape(id);
_arrows.erase(id);
_lines.erase(id);
}
}
void CloudViewer::removeAllArrows()
void CloudViewer::removeAllLines()
{
std::set<std::string> arrows = _arrows;
std::set<std::string> arrows = _lines;
for(std::set<std::string>::iterator iter = arrows.begin(); iter!=arrows.end(); ++iter)
{
this->removeArrow(*iter);
this->removeLine(*iter);
}
UASSERT(_arrows.empty());
UASSERT(_lines.empty());
}
static const float frustum_vertices[] = {
@@ -1112,7 +1120,7 @@ void CloudViewer::setFrustumShown(bool shown)
if(!shown)
{
this->removeFrustum("reference_frustum");
this->removeArrow("reference_frustum_arrow");
this->removeLine("reference_frustum_line");
this->update();
}
_aShowFrustum->setChecked(shown);
@@ -1372,7 +1380,7 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose, const Trans
this->addOrUpdateFrustum("reference_frustum", pose * baseToCamera, _frustumScale, _frustumColor);
if(!baseToCamera.isIdentity())
{
this->addOrUpdateArrow("reference_frustum_arrow", pose, pose * baseToCamera, _frustumColor);
this->addOrUpdateLine("reference_frustum_line", pose, pose * baseToCamera, _frustumColor);
}
}
guilib/src/DatabaseViewer.cpp
+17
View File
@@ -3080,6 +3080,7 @@ void DatabaseViewer::updateConstraintView(
memcmp(userData.data, "SCANS:", 6) == 0)
{
std::string scansStr = (const char *)userData.data;
UINFO("Detected \"%s\" in links's user data", scansStr.c_str());
if(!scansStr.empty())
{
std::list<std::string> strs = uSplit(scansStr, ':');
@@ -3128,6 +3129,22 @@ void DatabaseViewer::updateConstraintView(
posesOut,
linksOut);
if(poses.size() != posesOut.size())
{
UWARN("Scan poses input and output are different! %d vs %d", (int)poses.size(), (int)posesOut.size());
UWARN("Input poses: ");
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
UWARN(" %d", iter->first);
}
UWARN("Input links: ");
std::multimap<int, Link> modifiedLinks = updateLinksWithModifications(links_);
for(std::multimap<int, Link>::iterator iter=modifiedLinks.begin(); iter!=modifiedLinks.end(); ++iter)
{
UWARN(" %d->%d", iter->second.from(), iter->second.to());
}
}
QTime time;
time.start();
std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(link.to(), posesOut, linksOut);
guilib/src/PreferencesDialog.cpp
-1
View File
@@ -684,7 +684,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->localDetection_pathFilteringRadius->setObjectName(Parameters::kRGBDProximityPathFilteringRadius().c_str());
_ui->localDetection_angle->setObjectName(Parameters::kRGBDProximityAngle().c_str());
_ui->checkBox_localSpacePathOdomPosesUsed->setObjectName(Parameters::kRGBDProximityPathRawPosesUsed().c_str());
_ui->checkBox_localSpaceAssembleScans->setObjectName(Parameters::kRGBDProximityPathScansMerged().c_str());
_ui->rgdb_localImmunizationRatio->setObjectName(Parameters::kRGBDLocalImmunizationRatio().c_str());
_ui->loopClosure_reextract->setObjectName(Parameters::kRGBDLoopClosureReextractFeatures().c_str());
guilib/src/ui/preferencesDialog.ui
+20 -40
View File
@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-466</y>
<y>0</y>
<width>686</width>
<height>2023</height>
</rect>
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>1</number>
<number>11</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -6683,7 +6683,20 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</item>
<item>
<layout class="QGridLayout" name="gridLayout_50" columnstretch="0,1">
<item row="3" column="0">
<item row="3" column="1">
<widget class="QLabel" name="label_scanMatching_4">
<property name="text">
<string>When comparing to a local path, merge the laser scans using the odometry poses instead of the ones in the optimized local graph.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="localDetection_pathFilteringRadius">
<property name="suffix">
<string> m</string>
@@ -6699,7 +6712,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="2" column="1">
<widget class="QLabel" name="label_space3_3">
<property name="text">
<string>Path filtering radius to avoid merging laser scans which are close. 0 to ignore.</string>
@@ -6712,20 +6725,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_scanMatching_4">
<property name="text">
<string>When comparing to a local path, merge the laser scans using the odometry poses instead of the ones in the optimized local graph.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="0">
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox_localSpacePathOdomPosesUsed">
<property name="text">
<string/>
@@ -6755,7 +6755,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="5" column="1">
<widget class="QLabel" name="label_scanMatching_6">
<property name="text">
<string>Save scan matching IDs in link's user data.</string>
@@ -6768,33 +6768,13 @@ 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="5" column="0">
<widget class="QCheckBox" name="checkBox_localSpaceScanMatchingIDsSaved">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_scanMatching_7">
<property name="text">
<string>Merge close laser scans on each path. If false, only the nearest laser scan on the path is used for ICP.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="checkBox_localSpaceAssembleScans">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_scanMatching_8">
<property name="text">