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

OdometryF2M: updated how local scan map is updated

Browse Source
This commit is contained in:
matlabbe
2016-06-25 13:18:09 -04:00
parent ca95c9de97
commit 517bfa5272
7 changed files with 138 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/RegistrationIcp.cpp
+25 -5
View File
@@ -113,14 +113,16 @@ Transform RegistrationIcp::computeTransformationImpl(
if(!guess.isNull() && !dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
{
// ICP with guess transform
int maxLaserScans = dataTo.laserScanMaxPts()?dataTo.laserScanMaxPts():dataFrom.laserScanMaxPts();
int maxLaserScansTo = dataTo.laserScanMaxPts();
int maxLaserScansFrom = dataFrom.laserScanMaxPts();
cv::Mat fromScan = dataFrom.laserScanRaw();
cv::Mat toScan = dataTo.laserScanRaw();
if(_downsamplingStep>1)
{
fromScan = util3d::downsample(fromScan, _downsamplingStep);
toScan = util3d::downsample(toScan, _downsamplingStep);
maxLaserScans/=_downsamplingStep;
maxLaserScansTo/=_downsamplingStep;
maxLaserScansFrom/=_downsamplingStep;
UDEBUG("Downsampling time (step=%d) = %f s", _downsamplingStep, timer.ticks());
}
@@ -140,6 +142,7 @@ Transform RegistrationIcp::computeTransformationImpl(
//special case if we have already normals computed and there is no filtering
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, Transform());
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess);
UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
icpT = util3d::icpPointToPlane(
@@ -186,14 +189,19 @@ Transform RegistrationIcp::computeTransformationImpl(
bool filtered = false;
if(_voxelSize > 0.0f)
{
int pointsBeforeFiltering = fromCloudFiltered->size();
fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize);
int pointsBeforeFiltering = toCloudFiltered->size();
maxLaserScansFrom = maxLaserScansFrom * fromCloudFiltered->size() / pointsBeforeFiltering;
pointsBeforeFiltering = toCloudFiltered->size();
toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize);
maxLaserScansTo = maxLaserScansTo * toCloudFiltered->size() / pointsBeforeFiltering;
filtered = true;
UDEBUG("Voxel filtering time (voxel=%f m) = %f s", _voxelSize, timer.ticks());
//Adjust maxLaserScans
maxLaserScans = maxLaserScans * toCloudFiltered->size() / pointsBeforeFiltering;
}
bool correspondencesComputed = false;
@@ -214,6 +222,10 @@ Transform RegistrationIcp::computeTransformationImpl(
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
// update output scans
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals), maxLaserScansFrom, fromSignature.sensorData().laserScanMaxRange());
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, guess.inverse()), maxLaserScansTo, toSignature.sensorData().laserScanMaxRange());
UDEBUG("Compute normals time = %f s", timer.ticks());
if(toCloudNormals->size() && fromCloudNormals->size())
@@ -244,6 +256,13 @@ Transform RegistrationIcp::computeTransformationImpl(
}
else // ICP Point to Point
{
if(_voxelSize > 0.0f)
{
// update output scans
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered), maxLaserScansFrom, fromSignature.sensorData().laserScanMaxRange());
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, guess.inverse()), maxLaserScansTo, toSignature.sensorData().laserScanMaxRange());
}
icpT = util3d::icp(
fromCloudFiltered,
toCloudFiltered,
@@ -310,6 +329,7 @@ Transform RegistrationIcp::computeTransformationImpl(
else
{
// verify if there are enough correspondences
int maxLaserScans = maxLaserScansTo?maxLaserScansTo:maxLaserScansFrom;
if(maxLaserScans)
{
correspondencesRatio = float(correspondences)/float(maxLaserScans);
@@ -331,7 +351,7 @@ Transform RegistrationIcp::computeTransformationImpl(
hasConverged?"true":"false",
variance,
correspondences,
maxLaserScans>0?maxLaserScans:dataTo.laserScanMaxPts()?dataTo.laserScanMaxPts():(int)(toScan.cols>fromScan.cols?toScan.cols:fromScan.cols),
maxLaserScans>0?maxLaserScans:(int)(toScan.cols>fromScan.cols?toScan.cols:fromScan.cols),
correspondencesRatio*100.0f);
info.variance = variance>0.0f?variance:0.0001; // epsilon if exact transform