00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include <rtabmap/core/RegistrationIcp.h>
00030 #include <rtabmap/core/util3d_registration.h>
00031 #include <rtabmap/core/util3d_surface.h>
00032 #include <rtabmap/core/util3d.h>
00033 #include <rtabmap/core/util3d_transforms.h>
00034 #include <rtabmap/core/util3d_filtering.h>
00035 #include <rtabmap/utilite/ULogger.h>
00036 #include <rtabmap/utilite/UConversion.h>
00037 #include <rtabmap/utilite/UMath.h>
00038 #include <rtabmap/utilite/UTimer.h>
00039 #include <pcl/io/pcd_io.h>
00040
00041 namespace rtabmap {
00042
00043 RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration * child) :
00044 Registration(parameters, child),
00045 _maxTranslation(Parameters::defaultIcpMaxTranslation()),
00046 _maxRotation(Parameters::defaultIcpMaxRotation()),
00047 _voxelSize(Parameters::defaultIcpVoxelSize()),
00048 _downsamplingStep(Parameters::defaultIcpDownsamplingStep()),
00049 _maxCorrespondenceDistance(Parameters::defaultIcpMaxCorrespondenceDistance()),
00050 _maxIterations(Parameters::defaultIcpIterations()),
00051 _epsilon(Parameters::defaultIcpEpsilon()),
00052 _correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()),
00053 _pointToPlane(Parameters::defaultIcpPointToPlane()),
00054 _pointToPlaneNormalNeighbors(Parameters::defaultIcpPointToPlaneNormalNeighbors())
00055 {
00056 this->parseParameters(parameters);
00057 }
00058
00059 void RegistrationIcp::parseParameters(const ParametersMap & parameters)
00060 {
00061 Registration::parseParameters(parameters);
00062
00063 Parameters::parse(parameters, Parameters::kIcpMaxTranslation(), _maxTranslation);
00064 Parameters::parse(parameters, Parameters::kIcpMaxRotation(), _maxRotation);
00065 Parameters::parse(parameters, Parameters::kIcpVoxelSize(), _voxelSize);
00066 Parameters::parse(parameters, Parameters::kIcpDownsamplingStep(), _downsamplingStep);
00067 Parameters::parse(parameters, Parameters::kIcpMaxCorrespondenceDistance(), _maxCorrespondenceDistance);
00068 Parameters::parse(parameters, Parameters::kIcpIterations(), _maxIterations);
00069 Parameters::parse(parameters, Parameters::kIcpEpsilon(), _epsilon);
00070 Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _correspondenceRatio);
00071 Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane);
00072 Parameters::parse(parameters, Parameters::kIcpPointToPlaneNormalNeighbors(), _pointToPlaneNormalNeighbors);
00073
00074 UASSERT_MSG(_voxelSize >= 0, uFormat("value=%d", _voxelSize).c_str());
00075 UASSERT_MSG(_downsamplingStep >= 0, uFormat("value=%d", _downsamplingStep).c_str());
00076 UASSERT_MSG(_maxCorrespondenceDistance > 0.0f, uFormat("value=%f", _maxCorrespondenceDistance).c_str());
00077 UASSERT_MSG(_maxIterations > 0, uFormat("value=%d", _maxIterations).c_str());
00078 UASSERT(_epsilon >= 0.0f);
00079 UASSERT_MSG(_correspondenceRatio >=0.0f && _correspondenceRatio <=1.0f, uFormat("value=%f", _correspondenceRatio).c_str());
00080 UASSERT_MSG(_pointToPlaneNormalNeighbors > 0, uFormat("value=%d", _pointToPlaneNormalNeighbors).c_str());
00081 }
00082
00083 Transform RegistrationIcp::computeTransformationImpl(
00084 Signature & fromSignature,
00085 Signature & toSignature,
00086 Transform guess,
00087 RegistrationInfo & info) const
00088 {
00089 UDEBUG("Guess transform = %s", guess.prettyPrint().c_str());
00090 UDEBUG("Voxel size=%f", _voxelSize);
00091 UDEBUG("PointToPlane=%d", _pointToPlane?1:0);
00092 UDEBUG("Normal neighborhood=%d", _pointToPlaneNormalNeighbors);
00093 UDEBUG("Max corrrespondence distance=%f", _maxCorrespondenceDistance);
00094 UDEBUG("Max Iterations=%d", _maxIterations);
00095 UDEBUG("Correspondence Ratio=%f", _correspondenceRatio);
00096 UDEBUG("Max translation=%f", _maxTranslation);
00097 UDEBUG("Max rotation=%f", _maxRotation);
00098 UDEBUG("Downsampling step=%d", _downsamplingStep);
00099
00100 UTimer timer;
00101 std::string msg;
00102 Transform transform;
00103
00104 SensorData & dataFrom = fromSignature.sensorData();
00105 SensorData & dataTo = toSignature.sensorData();
00106
00107 UDEBUG("size from=%d (channels=%d) to=%d (channels=%d)",
00108 dataFrom.laserScanRaw().cols,
00109 dataFrom.laserScanRaw().channels(),
00110 dataTo.laserScanRaw().cols,
00111 dataTo.laserScanRaw().channels());
00112
00113 if(!guess.isNull() && !dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
00114 {
00115
00116 int maxLaserScansTo = dataTo.laserScanMaxPts();
00117 int maxLaserScansFrom = dataFrom.laserScanMaxPts();
00118 cv::Mat fromScan = dataFrom.laserScanRaw();
00119 cv::Mat toScan = dataTo.laserScanRaw();
00120 if(_downsamplingStep>1)
00121 {
00122 fromScan = util3d::downsample(fromScan, _downsamplingStep);
00123 toScan = util3d::downsample(toScan, _downsamplingStep);
00124 maxLaserScansTo/=_downsamplingStep;
00125 maxLaserScansFrom/=_downsamplingStep;
00126 UDEBUG("Downsampling time (step=%d) = %f s", _downsamplingStep, timer.ticks());
00127 }
00128
00129 if(fromScan.cols && toScan.cols)
00130 {
00131 Transform icpT;
00132 bool hasConverged = false;
00133 float correspondencesRatio = 0.0f;
00134 int correspondences = 0;
00135 double variance = 1.0;
00136
00137 if( _pointToPlane &&
00138 _voxelSize == 0.0f &&
00139 fromScan.channels() == 6 &&
00140 toScan.channels() == 6)
00141 {
00142
00143 pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, Transform());
00144 pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess);
00145
00146 UDEBUG("Conversion time = %f s", timer.ticks());
00147 pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
00148 icpT = util3d::icpPointToPlane(
00149 fromCloudNormals,
00150 toCloudNormals,
00151 _maxCorrespondenceDistance,
00152 _maxIterations,
00153 hasConverged,
00154 *fromCloudNormalsRegistered,
00155 _epsilon,
00156 this->force3DoF());
00157 if(!icpT.isNull() && hasConverged)
00158 {
00159 util3d::computeVarianceAndCorrespondences(
00160 fromCloudNormalsRegistered,
00161 toCloudNormals,
00162 _maxCorrespondenceDistance,
00163 variance,
00164 correspondences);
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179 }
00180 }
00181 else
00182 {
00183 pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, Transform());
00184 pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess);
00185 UDEBUG("Conversion time = %f s", timer.ticks());
00186
00187 pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
00188 pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
00189 bool filtered = false;
00190 if(_voxelSize > 0.0f)
00191 {
00192 int pointsBeforeFiltering = fromCloudFiltered->size();
00193 fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize);
00194 maxLaserScansFrom = maxLaserScansFrom * fromCloudFiltered->size() / pointsBeforeFiltering;
00195
00196 pointsBeforeFiltering = toCloudFiltered->size();
00197 toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize);
00198 maxLaserScansTo = maxLaserScansTo * toCloudFiltered->size() / pointsBeforeFiltering;
00199
00200 filtered = true;
00201 UDEBUG("Voxel filtering time (voxel=%f m) = %f s", _voxelSize, timer.ticks());
00202
00203
00204
00205 }
00206
00207 bool correspondencesComputed = false;
00208 pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
00209 if(_pointToPlane)
00210 {
00211 pcl::PointCloud<pcl::Normal>::Ptr normals;
00212
00213 normals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneNormalNeighbors);
00214 pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
00215 pcl::concatenateFields(*fromCloudFiltered, *normals, *fromCloudNormals);
00216
00217 normals = util3d::computeNormals(toCloudFiltered, _pointToPlaneNormalNeighbors);
00218 pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
00219 pcl::concatenateFields(*toCloudFiltered, *normals, *toCloudNormals);
00220
00221 std::vector<int> indices;
00222 toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
00223 fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
00224
00225
00226 fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals), maxLaserScansFrom, fromSignature.sensorData().laserScanMaxRange());
00227 toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, guess.inverse()), maxLaserScansTo, toSignature.sensorData().laserScanMaxRange());
00228
00229 UDEBUG("Compute normals time = %f s", timer.ticks());
00230
00231 if(toCloudNormals->size() && fromCloudNormals->size())
00232 {
00233 pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
00234 icpT = util3d::icpPointToPlane(
00235 fromCloudNormals,
00236 toCloudNormals,
00237 _maxCorrespondenceDistance,
00238 _maxIterations,
00239 hasConverged,
00240 *fromCloudNormalsRegistered,
00241 _epsilon,
00242 this->force3DoF());
00243 if(!filtered &&
00244 !icpT.isNull() &&
00245 hasConverged)
00246 {
00247 util3d::computeVarianceAndCorrespondences(
00248 fromCloudNormalsRegistered,
00249 toCloudNormals,
00250 _maxCorrespondenceDistance,
00251 variance,
00252 correspondences);
00253 correspondencesComputed = true;
00254 }
00255 }
00256 }
00257 else
00258 {
00259 if(_voxelSize > 0.0f)
00260 {
00261
00262 fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered), maxLaserScansFrom, fromSignature.sensorData().laserScanMaxRange());
00263 toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, guess.inverse()), maxLaserScansTo, toSignature.sensorData().laserScanMaxRange());
00264 }
00265
00266 icpT = util3d::icp(
00267 fromCloudFiltered,
00268 toCloudFiltered,
00269 _maxCorrespondenceDistance,
00270 _maxIterations,
00271 hasConverged,
00272 *fromCloudRegistered,
00273 _epsilon,
00274 this->force3DoF());
00275 }
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287 if(!icpT.isNull() &&
00288 hasConverged &&
00289 !correspondencesComputed)
00290 {
00291 if(filtered)
00292 {
00293 fromCloud = util3d::transformPointCloud(fromCloud, icpT);
00294 }
00295 else
00296 {
00297 fromCloud = fromCloudRegistered;
00298 }
00299
00300 util3d::computeVarianceAndCorrespondences(
00301 fromCloud,
00302 toCloud,
00303 _maxCorrespondenceDistance,
00304 variance,
00305 correspondences);
00306 }
00307 }
00308 UDEBUG("ICP (iterations=%d) time = %f s", _maxIterations, timer.ticks());
00309
00310 if(!icpT.isNull() &&
00311 hasConverged)
00312 {
00313 float ix,iy,iz, iroll,ipitch,iyaw;
00314 Transform icpInTargetReferential = guess.inverse() * icpT.inverse() * guess;
00315 icpInTargetReferential.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
00316 info.icpTranslation = uMax3(fabs(ix), fabs(iy), fabs(iz));
00317 info.icpRotation = uMax3(fabs(iroll), fabs(ipitch), fabs(iyaw));
00318 if((_maxTranslation>0.0f &&
00319 info.icpTranslation > _maxTranslation)
00320 ||
00321 (_maxRotation>0.0f &&
00322 info.icpRotation > _maxRotation))
00323 {
00324 msg = uFormat("Cannot compute transform (ICP correction too large -> %f m %f rad, limits=%f m, %f rad)",
00325 info.icpTranslation,
00326 info.icpRotation,
00327 _maxTranslation,
00328 _maxRotation);
00329 UINFO(msg.c_str());
00330 }
00331 else
00332 {
00333
00334 int maxLaserScans = maxLaserScansTo?maxLaserScansTo:maxLaserScansFrom;
00335 if(maxLaserScans)
00336 {
00337 correspondencesRatio = float(correspondences)/float(maxLaserScans);
00338 }
00339 else
00340 {
00341 static bool warningShown = false;
00342 if(!warningShown)
00343 {
00344 UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute! This message will only appear once.",
00345 dataTo.id());
00346 warningShown = true;
00347 }
00348 correspondencesRatio = float(correspondences)/float(toScan.cols>fromScan.cols?toScan.cols:fromScan.cols);
00349 }
00350
00351 UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)",
00352 dataTo.id(), dataFrom.id(),
00353 hasConverged?"true":"false",
00354 variance,
00355 correspondences,
00356 maxLaserScans>0?maxLaserScans:(int)(toScan.cols>fromScan.cols?toScan.cols:fromScan.cols),
00357 correspondencesRatio*100.0f);
00358
00359 info.variance = variance>0.0f?variance:0.0001;
00360 info.icpInliersRatio = correspondencesRatio;
00361
00362 if(correspondencesRatio < _correspondenceRatio)
00363 {
00364 msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f)",
00365 correspondences, correspondencesRatio, _correspondenceRatio);
00366 UINFO(msg.c_str());
00367 }
00368 else
00369 {
00370 transform = icpT.inverse()*guess;
00371 }
00372 }
00373 }
00374 else
00375 {
00376 msg = uFormat("Cannot compute transform (converged=%s var=%f)",
00377 hasConverged?"true":"false", variance);
00378 UINFO(msg.c_str());
00379 }
00380 }
00381 else
00382 {
00383 msg = "Laser scans empty ?!?";
00384 UWARN(msg.c_str());
00385 }
00386 }
00387 else if(dataTo.isValid())
00388 {
00389 if(guess.isNull())
00390 {
00391 msg = "RegistrationIcp cannot do registration with a null guess.";
00392 }
00393 else
00394 {
00395 msg = uFormat("Laser scans empty?!? (new[%d]=%d old[%d]=%d)",
00396 dataTo.id(), dataTo.laserScanRaw().total(),
00397 dataFrom.id(), dataFrom.laserScanRaw().total());
00398 }
00399 UERROR(msg.c_str());
00400 }
00401
00402
00403 info.rejectedMsg = msg;
00404
00405 UDEBUG("New transform = %s", transform.prettyPrint().c_str());
00406 return transform;
00407 }
00408
00409 }