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

39
corelib/src/Memory.cpp
View File

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

16
corelib/src/Rtabmap.cpp
View File

@@ -964,7 +964,7 @@ bool Rtabmap::process(const SensorData & data)
}
else
{
UWARN("Scan matching rejected: %s", rejectedMsg.c_str());
UINFO("Scan matching rejected: %s", rejectedMsg.c_str());
}
}
timeScanMatching = timer.ticks();
@@ -1485,7 +1485,7 @@ bool Rtabmap::process(const SensorData & data)
rejectedHypothesis = transform.isNull();
if(rejectedHypothesis)
{
UWARN("Rejected loop closure %d -> %d: %s",
UINFO("Rejected loop closure %d -> %d: %s",
_loopClosureHypothesis.first, signature->id(), rejectedMsg.c_str());
}
}
@@ -1557,8 +1557,12 @@ bool Rtabmap::process(const SensorData & data)
// path filtering
if(_localPathFilteringRadius > 0.0f)
{
path = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true);
path.insert(*_optimizedPoses.find(nearestId)); // make sure the nearest pose is still here
std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true);
// 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
@@ -1567,7 +1571,7 @@ bool Rtabmap::process(const SensorData & data)
bool foundByVisual = false;
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
}
if(transform.isNull())
@@ -1868,7 +1872,7 @@ bool Rtabmap::process(const SensorData & data)
}
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);
// If there is a too small displacement, remove the node
signaturesRemoved.push_back(signature->id());