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

Updated default RGBD/LocalLoopDetectionPathFilteringRadius to 0.25

Browse Source
This commit is contained in:
Mathieu Labbe
2015-04-27 18:16:04 -04:00
parent 242c3e5f1b
commit a4f11b7a3b
5 changed files with 42 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -298,7 +298,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, LocalLoopDetectionTime, bool, false, "Detection over all locations in STM."); RTABMAP_PARAM(RGBD, LocalLoopDetectionTime, bool, false, "Detection over all locations in STM.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace, bool, false, "Detection over locations (in Working Memory or STM) near in space."); RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace, bool, false, "Detection over locations (in Working Memory or STM) near in space.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID, int, 50, "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore."); RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID, int, 50, "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionPathFilteringRadius, float, 1.0, "Path filtering radius."); RTABMAP_PARAM(RGBD, LocalLoopDetectionPathFilteringRadius, float, 0.25, "Path filtering radius.");
// Graph optimization // Graph optimization
RTABMAP_PARAM(RGBD, OptimizeStrategy, int, 0, "Graph optimization strategy: 0=TORO and 1=g2o."); RTABMAP_PARAM(RGBD, OptimizeStrategy, int, 0, "Graph optimization strategy: 0=TORO and 1=g2o.");

39
corelib/src/Memory.cpp
View File

@@ -2179,11 +2179,11 @@ Transform Memory::computeIcpTransform(
// verify if there are enough correspondences // verify if there are enough correspondences
correspondencesRatio = float(correspondences)/float(oldCloudXYZ->size()>newCloudXYZ->size()?oldCloudXYZ->size():newCloudXYZ->size()); correspondencesRatio = float(correspondences)/float(oldCloudXYZ->size()>newCloudXYZ->size()?oldCloudXYZ->size():newCloudXYZ->size());
UDEBUG("hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)",
hasConverged?"true":"false", hasConverged?"true":"false",
variance?*variance:-1, variance?*variance:-1,
correspondences, correspondences,
(int)oldCloudXYZ->size(), (int)(oldCloudXYZ->size()>newCloudXYZ->size()?oldCloudXYZ->size():newCloudXYZ->size()),
correspondencesRatio*100.0f); correspondencesRatio*100.0f);
if(inliers) if(inliers)
@@ -2197,14 +2197,14 @@ Transform Memory::computeIcpTransform(
float x,y,z, roll,pitch,yaw; float x,y,z, roll,pitch,yaw;
icpT.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); icpT.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
if((_icpMaxTranslation>0.0f && if((_icpMaxTranslation>0.0f &&
(x > _icpMaxTranslation || (fabs(x) > _icpMaxTranslation ||
y > _icpMaxTranslation || fabs(y) > _icpMaxTranslation ||
z > _icpMaxTranslation)) fabs(z) > _icpMaxTranslation))
|| ||
(_icpMaxRotation>0.0f && (_icpMaxRotation>0.0f &&
(roll > _icpMaxRotation || (fabs(roll) > _icpMaxRotation ||
pitch > _icpMaxRotation || fabs(pitch) > _icpMaxRotation ||
yaw > _icpMaxRotation))) fabs(yaw) > _icpMaxRotation)))
{ {
msg = uFormat("Cannot compute transform (ICP correction too large)"); msg = uFormat("Cannot compute transform (ICP correction too large)");
UINFO(msg.c_str()); UINFO(msg.c_str());
@@ -2278,11 +2278,12 @@ Transform Memory::computeIcpTransform(
// verify if there are enough correspondences // verify if there are enough correspondences
correspondencesRatio = float(correspondences)/float(oldCloud->size()>newCloud->size()?oldCloud->size():newCloud->size()); correspondencesRatio = float(correspondences)/float(oldCloud->size()>newCloud->size()?oldCloud->size():newCloud->size());
UDEBUG("hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)",
newS.id(), oldS.id(),
hasConverged?"true":"false", hasConverged?"true":"false",
variance?*variance:-1, variance?*variance:-1,
correspondences, correspondences,
(int)oldCloud->size(), (int)(oldCloud->size()>newCloud->size()?oldCloud->size():newCloud->size()),
correspondencesRatio*100.0f); correspondencesRatio*100.0f);
//pcl::io::savePCDFile("oldCloud.pcd", *oldCloud); //pcl::io::savePCDFile("oldCloud.pcd", *oldCloud);
@@ -2302,17 +2303,17 @@ Transform Memory::computeIcpTransform(
if(!icpT.isNull() && hasConverged && correspondencesRatio >= _icp2CorrespondenceRatio) if(!icpT.isNull() && hasConverged && correspondencesRatio >= _icp2CorrespondenceRatio)
{ {
float x,y,z, roll,pitch,yaw; float ix,iy,iz, iroll,ipitch,iyaw;
icpT.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); icpT.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
if((_icpMaxTranslation>0.0f && if((_icpMaxTranslation>0.0f &&
(x > _icpMaxTranslation || (fabs(ix) > _icpMaxTranslation ||
y > _icpMaxTranslation || fabs(iy) > _icpMaxTranslation ||
z > _icpMaxTranslation)) fabs(iz) > _icpMaxTranslation))
|| ||
(_icpMaxRotation>0.0f && (_icpMaxRotation>0.0f &&
(roll > _icpMaxRotation || (fabs(iroll) > _icpMaxRotation ||
pitch > _icpMaxRotation || fabs(ipitch) > _icpMaxRotation ||
yaw > _icpMaxRotation))) fabs(iyaw) > _icpMaxRotation)))
{ {
msg = uFormat("Cannot compute transform (ICP correction too large)"); msg = uFormat("Cannot compute transform (ICP correction too large)");
UINFO(msg.c_str()); UINFO(msg.c_str());
@@ -2439,7 +2440,7 @@ Transform Memory::computeScanMatchingTransform(
variance?*variance:-1, variance?*variance:-1,
correspondences, correspondences,
(int)newCloud->size(), (int)newCloud->size(),
correspondencesRatio); correspondencesRatio*100.0f);
if(inliers) if(inliers)
{ {

16
corelib/src/Rtabmap.cpp
View File

@@ -964,7 +964,7 @@ bool Rtabmap::process(const SensorData & data)
} }
else else
{ {
UWARN("Scan matching rejected: %s", rejectedMsg.c_str()); UINFO("Scan matching rejected: %s", rejectedMsg.c_str());
} }
} }
timeScanMatching = timer.ticks(); timeScanMatching = timer.ticks();
@@ -1485,7 +1485,7 @@ bool Rtabmap::process(const SensorData & data)
rejectedHypothesis = transform.isNull(); rejectedHypothesis = transform.isNull();
if(rejectedHypothesis) if(rejectedHypothesis)
{ {
UWARN("Rejected loop closure %d -> %d: %s", UINFO("Rejected loop closure %d -> %d: %s",
_loopClosureHypothesis.first, signature->id(), rejectedMsg.c_str()); _loopClosureHypothesis.first, signature->id(), rejectedMsg.c_str());
} }
} }
@@ -1557,8 +1557,12 @@ bool Rtabmap::process(const SensorData & data)
// path filtering // path filtering
if(_localPathFilteringRadius > 0.0f) if(_localPathFilteringRadius > 0.0f)
{ {
path = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true); std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true);
path.insert(*_optimizedPoses.find(nearestId)); // make sure the nearest pose is still here // make sure the nearest and farthest poses are still here
filteredPath.insert(*_optimizedPoses.find(nearestId));
filteredPath.insert(*path.begin());
filteredPath.insert(*path.rbegin());
path = filteredPath;
} }
// 1) look for loop closures based on visual correspondences // 1) look for loop closures based on visual correspondences
@@ -1567,7 +1571,7 @@ bool Rtabmap::process(const SensorData & data)
bool foundByVisual = false; bool foundByVisual = false;
if(!transform.isNull() && _globalLoopClosureIcpType > 0) if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{ {
transform = _memory->computeIcpTransform(_loopClosureHypothesis.first, signature->id(), transform, _globalLoopClosureIcpType == 1, 0, 0, &variance); transform = _memory->computeIcpTransform(nearestId, signature->id(), transform, _globalLoopClosureIcpType == 1, 0, 0, &variance);
variance = 1.0f; // ICP, set variance to 1 variance = 1.0f; // ICP, set variance to 1
} }
if(transform.isNull()) if(transform.isNull())
@@ -1868,7 +1872,7 @@ bool Rtabmap::process(const SensorData & data)
} }
else if(smallDisplacement) else if(smallDisplacement)
{ {
UWARN("Ignoring location %d because the displacement is too small! (d=%f a=%f)", UINFO("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate); signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate);
// If there is a too small displacement, remove the node // If there is a too small displacement, remove the node
signaturesRemoved.push_back(signature->id()); signaturesRemoved.push_back(signature->id());

8
guilib/src/GraphViewer.cpp
View File

@@ -87,7 +87,6 @@ protected:
virtual void hoverLeaveEvent ( QGraphicsSceneHoverEvent * event ) virtual void hoverLeaveEvent ( QGraphicsSceneHoverEvent * event )
{ {
qDeleteAll(this->childItems());
this->setScale(1); this->setScale(1);
QGraphicsEllipseItem::hoverEnterEvent(event); QGraphicsEllipseItem::hoverEnterEvent(event);
} }
@@ -133,11 +132,18 @@ public:
protected: protected:
virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event ) virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
{ {
this->setToolTip(QString("%1->%2 %3 m").arg(_from).arg(_to).arg(_poseA.getDistance(_poseB)));
QPen pen = this->pen();
pen.setWidthF(pen.widthF()+0.02);
this->setPen(pen);
QGraphicsLineItem::hoverEnterEvent(event); QGraphicsLineItem::hoverEnterEvent(event);
} }
virtual void hoverLeaveEvent ( QGraphicsSceneHoverEvent * event ) virtual void hoverLeaveEvent ( QGraphicsSceneHoverEvent * event )
{ {
QPen pen = this->pen();
pen.setWidthF(pen.widthF()-0.02);
this->setPen(pen);
QGraphicsLineItem::hoverEnterEvent(event); QGraphicsLineItem::hoverEnterEvent(event);
} }

8
guilib/src/ui/preferencesDialog.ui
View File

@@ -63,9 +63,9 @@
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>-806</y>
<width>760</width> <width>760</width>
<height>1002</height> <height>1462</height>
</rect> </rect>
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_16"> <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>21</number> <number>19</number>
</property> </property>
<widget class="QWidget" name="page_22"> <widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29"> <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -5214,7 +5214,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<string> m</string> <string> m</string>
</property> </property>
<property name="decimals"> <property name="decimals">
<number>1</number> <number>2</number>
</property> </property>
<property name="singleStep"> <property name="singleStep">
<double>0.100000000000000</double> <double>0.100000000000000</double>