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

0.15.3: util3d::computeVarianceAndCorrespondences(): added max angle parameters for normal type. util3d::downsample(): added Normal types support. OccupancyGrid: fixed 2d scan with 5 channels not handled correctly on update.

Browse Source
This commit is contained in:
matlabbe
2017-12-13 18:12:40 -05:00
parent 7091406abc
commit 398ca1f8e4
7 changed files with 122 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/RegistrationIcp.cpp
+25 -9
View File
@@ -523,6 +523,7 @@ Transform RegistrationIcp::computeTransformationImpl(
fromCloudNormalsRegistered,
toCloudNormals,
_maxCorrespondenceDistance,
_maxRotation,
variance,
correspondences);
}
@@ -717,6 +718,7 @@ Transform RegistrationIcp::computeTransformationImpl(
fromCloudNormalsRegistered,
toCloudNormals,
_maxCorrespondenceDistance,
_maxRotation,
variance,
correspondences);
}
@@ -855,14 +857,28 @@ Transform RegistrationIcp::computeTransformationImpl(
t.getEulerAngles(roll, pitch, yaw);
t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
icpT = guess * t.inverse() * guessInv;
}
util3d::computeVarianceAndCorrespondences(
fromCloudRegistered,
toCloudFiltered,
_maxCorrespondenceDistance,
variance,
correspondences);
// we were using normals, so compute correspondences using normals
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromLocalTransform);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform);
util3d::computeVarianceAndCorrespondences(
fromCloudNormalsRegistered,
toCloudNormals,
_maxCorrespondenceDistance,
_maxRotation,
variance,
correspondences);
}
else
{
util3d::computeVarianceAndCorrespondences(
fromCloudRegistered,
toCloudFiltered,
_maxCorrespondenceDistance,
variance,
correspondences);
}
}
}
}
@@ -926,8 +942,8 @@ Transform RegistrationIcp::computeTransformationImpl(
if(correspondencesRatio < _correspondenceRatio)
{
msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f)",
correspondences, correspondencesRatio, _correspondenceRatio);
msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f maxLaserScans=%d)",
correspondences, correspondencesRatio, _correspondenceRatio, maxLaserScans);
UINFO(msg.c_str());
}
else