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/conversions.h>
00040
00041 #ifdef RTABMAP_POINTMATCHER
00042 #include <fstream>
00043 #include "pointmatcher/PointMatcher.h"
00044 typedef PointMatcher<float> PM;
00045 typedef PM::DataPoints DP;
00046
00047 DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, bool is2D)
00048 {
00049 UDEBUG("");
00050 typedef DP::Label Label;
00051 typedef DP::Labels Labels;
00052 typedef DP::View View;
00053
00054 if (pclCloud->empty())
00055 return DP();
00056
00057
00058
00059
00060 Labels featLabels;
00061 Labels descLabels;
00062 std::vector<bool> isFeature;
00063 featLabels.push_back(Label("x", 1));
00064 isFeature.push_back(true);
00065 featLabels.push_back(Label("y", 1));
00066 isFeature.push_back(true);
00067 if(!is2D)
00068 {
00069 featLabels.push_back(Label("z", 1));
00070 isFeature.push_back(true);
00071 }
00072 featLabels.push_back(Label("pad", 1));
00073
00074
00075 DP cloud(featLabels, descLabels, pclCloud->size());
00076 cloud.getFeatureViewByName("pad").setConstant(1);
00077
00078
00079 View view(cloud.getFeatureViewByName("x"));
00080 for(unsigned int i=0; i<pclCloud->size(); ++i)
00081 {
00082 view(0, i) = pclCloud->at(i).x;
00083 view(1, i) = pclCloud->at(i).y;
00084 if(!is2D)
00085 {
00086 view(2, i) = pclCloud->at(i).z;
00087 }
00088 }
00089
00090 return cloud;
00091 }
00092
00093 DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud, bool is2D)
00094 {
00095 UDEBUG("");
00096 typedef DP::Label Label;
00097 typedef DP::Labels Labels;
00098 typedef DP::View View;
00099
00100 if (pclCloud->empty())
00101 return DP();
00102
00103
00104
00105
00106 Labels featLabels;
00107 Labels descLabels;
00108 std::vector<bool> isFeature;
00109 featLabels.push_back(Label("x", 1));
00110 isFeature.push_back(true);
00111 featLabels.push_back(Label("y", 1));
00112 isFeature.push_back(true);
00113 if(!is2D)
00114 {
00115 featLabels.push_back(Label("z", 1));
00116 isFeature.push_back(true);
00117 }
00118
00119 descLabels.push_back(Label("normals", 3));
00120 isFeature.push_back(false);
00121 isFeature.push_back(false);
00122 isFeature.push_back(false);
00123
00124 featLabels.push_back(Label("pad", 1));
00125
00126
00127 DP cloud(featLabels, descLabels, pclCloud->size());
00128 cloud.getFeatureViewByName("pad").setConstant(1);
00129
00130
00131 View view(cloud.getFeatureViewByName("x"));
00132 View viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
00133 View viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
00134 View viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
00135 for(unsigned int i=0; i<pclCloud->size(); ++i)
00136 {
00137 view(0, i) = pclCloud->at(i).x;
00138 view(1, i) = pclCloud->at(i).y;
00139 if(!is2D)
00140 {
00141 view(2, i) = pclCloud->at(i).z;
00142 }
00143 viewNormalX(0, i) = pclCloud->at(i).normal_x;
00144 viewNormalY(0, i) = pclCloud->at(i).normal_y;
00145 viewNormalZ(0, i) = pclCloud->at(i).normal_z;
00146 }
00147
00148 return cloud;
00149 }
00150
00151 DP laserScanToDP(const rtabmap::LaserScan & scan)
00152 {
00153 UDEBUG("");
00154 typedef DP::Label Label;
00155 typedef DP::Labels Labels;
00156 typedef DP::View View;
00157
00158 if (scan.isEmpty())
00159 return DP();
00160
00161
00162
00163
00164 Labels featLabels;
00165 Labels descLabels;
00166 featLabels.push_back(Label("x", 1));
00167 featLabels.push_back(Label("y", 1));
00168 if(!scan.is2d())
00169 {
00170 featLabels.push_back(Label("z", 1));
00171 }
00172 featLabels.push_back(Label("pad", 1));
00173
00174 if(scan.hasNormals())
00175 {
00176 descLabels.push_back(Label("normals", 3));
00177 }
00178 if(scan.hasIntensity())
00179 {
00180 descLabels.push_back(Label("intensity", 1));
00181 }
00182
00183
00184 DP cloud(featLabels, descLabels, scan.size());
00185 cloud.getFeatureViewByName("pad").setConstant(1);
00186
00187
00188 int nx = scan.getNormalsOffset();
00189 int ny = nx+1;
00190 int nz = ny+1;
00191 int offsetI = scan.getIntensityOffset();
00192 bool hasLocalTransform = !scan.localTransform().isNull() && !scan.localTransform().isIdentity();
00193 View view(cloud.getFeatureViewByName("x"));
00194 View viewNormalX(nx!=-1?cloud.getDescriptorRowViewByName("normals",0):view);
00195 View viewNormalY(nx!=-1?cloud.getDescriptorRowViewByName("normals",1):view);
00196 View viewNormalZ(nx!=-1?cloud.getDescriptorRowViewByName("normals",2):view);
00197 View viewIntensity(offsetI!=-1?cloud.getDescriptorRowViewByName("intensity",0):view);
00198 int oi = 0;
00199 for(int i=0; i<scan.size(); ++i)
00200 {
00201 const float * ptr = scan.data().ptr<float>(0, i);
00202
00203 if(uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && (scan.is2d() || uIsFinite(ptr[2])))
00204 {
00205 if(hasLocalTransform)
00206 {
00207 if(nx == -1)
00208 {
00209 cv::Point3f pt(ptr[0], ptr[1], scan.is2d()?0:ptr[2]);
00210 pt = rtabmap::util3d::transformPoint(pt, scan.localTransform());
00211 view(0, oi) = pt.x;
00212 view(1, oi) = pt.y;
00213 if(!scan.is2d())
00214 {
00215 view(2, oi) = pt.z;
00216 }
00217 if(offsetI!=-1)
00218 {
00219 viewIntensity(0, oi) = ptr[offsetI];
00220 }
00221 ++oi;
00222 }
00223 else if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz]))
00224 {
00225 pcl::PointNormal pt;
00226 pt.x=ptr[0];
00227 pt.y=ptr[1];
00228 pt.z=scan.is2d()?0:ptr[2];
00229 pt.normal_x=ptr[nx];
00230 pt.normal_y=ptr[ny];
00231 pt.normal_z=ptr[nz];
00232 pt = rtabmap::util3d::transformPoint(pt, scan.localTransform());
00233 view(0, oi) = pt.x;
00234 view(1, oi) = pt.y;
00235 if(!scan.is2d())
00236 {
00237 view(2, oi) = pt.z;
00238 }
00239 viewNormalX(0, oi) = pt.normal_x;
00240 viewNormalY(0, oi) = pt.normal_y;
00241 viewNormalZ(0, oi) = pt.normal_z;
00242
00243 if(offsetI!=-1)
00244 {
00245 viewIntensity(0, oi) = ptr[offsetI];
00246 }
00247
00248 ++oi;
00249 }
00250 else
00251 {
00252 UWARN("Ignoring point %d with invalid data: pos=%f %f %f, normal=%f %f %f", i, ptr[0], ptr[1], scan.is2d()?0:ptr[3], ptr[nx], ptr[ny], ptr[nz]);
00253 }
00254 }
00255 else if(nx==-1 || (uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz])))
00256 {
00257 view(0, oi) = ptr[0];
00258 view(1, oi) = ptr[1];
00259 if(!scan.is2d())
00260 {
00261 view(2, oi) = ptr[2];
00262 }
00263 if(nx!=-1)
00264 {
00265 viewNormalX(0, oi) = ptr[nx];
00266 viewNormalY(0, oi) = ptr[ny];
00267 viewNormalZ(0, oi) = ptr[nz];
00268 }
00269 if(offsetI!=-1)
00270 {
00271 viewIntensity(0, oi) = ptr[offsetI];
00272 }
00273 ++oi;
00274 }
00275 else
00276 {
00277 UWARN("Ignoring point %d with invalid data: pos=%f %f %f, normal=%f %f %f", i, ptr[0], ptr[1], scan.is2d()?0:ptr[3], ptr[nx], ptr[ny], ptr[nz]);
00278 }
00279 }
00280 else
00281 {
00282 UWARN("Ignoring point %d with invalid data: pos=%f %f %f", i, ptr[0], ptr[1], scan.is2d()?0:ptr[3]);
00283 }
00284
00285 }
00286 if(oi != scan.size())
00287 {
00288 cloud.conservativeResize(oi);
00289 }
00290
00291 return cloud;
00292 }
00293
00294 void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointXYZ> & pclCloud)
00295 {
00296 UDEBUG("");
00297 typedef DP::ConstView ConstView;
00298
00299 if (cloud.features.cols() == 0)
00300 return;
00301
00302 pclCloud.resize(cloud.features.cols());
00303 pclCloud.is_dense = true;
00304
00305
00306 ConstView view(cloud.getFeatureViewByName("x"));
00307 bool is3D = cloud.featureExists("z");
00308 for(unsigned int i=0; i<pclCloud.size(); ++i)
00309 {
00310 pclCloud.at(i).x = view(0, i);
00311 pclCloud.at(i).y = view(1, i);
00312 pclCloud.at(i).z = is3D?view(2, i):0;
00313 }
00314 }
00315
00316 void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointNormal> & pclCloud)
00317 {
00318 UDEBUG("");
00319 typedef DP::ConstView ConstView;
00320
00321 if (cloud.features.cols() == 0)
00322 return;
00323
00324 pclCloud.resize(cloud.features.cols());
00325 pclCloud.is_dense = true;
00326
00327
00328 ConstView view(cloud.getFeatureViewByName("x"));
00329 bool is3D = cloud.featureExists("z");
00330 ConstView viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
00331 ConstView viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
00332 ConstView viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
00333 for(unsigned int i=0; i<pclCloud.size(); ++i)
00334 {
00335 pclCloud.at(i).x = view(0, i);
00336 pclCloud.at(i).y = view(1, i);
00337 pclCloud.at(i).z = is3D?view(2, i):0;
00338 pclCloud.at(i).normal_x = viewNormalX(0, i);
00339 pclCloud.at(i).normal_y = viewNormalY(0, i);
00340 pclCloud.at(i).normal_z = viewNormalZ(0, i);
00341 }
00342 }
00343
00344 template<typename T>
00345 typename PointMatcher<T>::TransformationParameters eigenMatrixToDim(const typename PointMatcher<T>::TransformationParameters& matrix, int dimp1)
00346 {
00347 typedef typename PointMatcher<T>::TransformationParameters M;
00348 assert(matrix.rows() == matrix.cols());
00349 assert((matrix.rows() == 3) || (matrix.rows() == 4));
00350 assert((dimp1 == 3) || (dimp1 == 4));
00351
00352 if (matrix.rows() == dimp1)
00353 return matrix;
00354
00355 M out(M::Identity(dimp1,dimp1));
00356 out.topLeftCorner(2,2) = matrix.topLeftCorner(2,2);
00357 out.topRightCorner(2,1) = matrix.topRightCorner(2,1);
00358 return out;
00359 }
00360
00361 #endif
00362
00363 namespace rtabmap {
00364
00365 RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration * child) :
00366 Registration(parameters, child),
00367 _maxTranslation(Parameters::defaultIcpMaxTranslation()),
00368 _maxRotation(Parameters::defaultIcpMaxRotation()),
00369 _voxelSize(Parameters::defaultIcpVoxelSize()),
00370 _downsamplingStep(Parameters::defaultIcpDownsamplingStep()),
00371 _maxCorrespondenceDistance(Parameters::defaultIcpMaxCorrespondenceDistance()),
00372 _maxIterations(Parameters::defaultIcpIterations()),
00373 _epsilon(Parameters::defaultIcpEpsilon()),
00374 _correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()),
00375 _pointToPlane(Parameters::defaultIcpPointToPlane()),
00376 _pointToPlaneK(Parameters::defaultIcpPointToPlaneK()),
00377 _pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()),
00378 _pointToPlaneMinComplexity(Parameters::defaultIcpPointToPlaneMinComplexity()),
00379 _libpointmatcher(Parameters::defaultIcpPM()),
00380 _libpointmatcherConfig(Parameters::defaultIcpPMConfig()),
00381 _libpointmatcherKnn(Parameters::defaultIcpPMMatcherKnn()),
00382 _libpointmatcherEpsilon(Parameters::defaultIcpPMMatcherEpsilon()),
00383 _libpointmatcherOutlierRatio(Parameters::defaultIcpPMOutlierRatio()),
00384 _libpointmatcherICP(0)
00385 {
00386 this->parseParameters(parameters);
00387 }
00388
00389 RegistrationIcp::~RegistrationIcp()
00390 {
00391 #ifdef RTABMAP_POINTMATCHER
00392 delete (PM::ICP*)_libpointmatcherICP;
00393 #endif
00394 }
00395
00396 void RegistrationIcp::parseParameters(const ParametersMap & parameters)
00397 {
00398 Registration::parseParameters(parameters);
00399
00400 Parameters::parse(parameters, Parameters::kIcpMaxTranslation(), _maxTranslation);
00401 Parameters::parse(parameters, Parameters::kIcpMaxRotation(), _maxRotation);
00402 Parameters::parse(parameters, Parameters::kIcpVoxelSize(), _voxelSize);
00403 Parameters::parse(parameters, Parameters::kIcpDownsamplingStep(), _downsamplingStep);
00404 Parameters::parse(parameters, Parameters::kIcpMaxCorrespondenceDistance(), _maxCorrespondenceDistance);
00405 Parameters::parse(parameters, Parameters::kIcpIterations(), _maxIterations);
00406 Parameters::parse(parameters, Parameters::kIcpEpsilon(), _epsilon);
00407 Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _correspondenceRatio);
00408 Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane);
00409 Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK);
00410 Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), _pointToPlaneRadius);
00411 Parameters::parse(parameters, Parameters::kIcpPointToPlaneMinComplexity(), _pointToPlaneMinComplexity);
00412 UASSERT(_pointToPlaneMinComplexity >= 0.0f && _pointToPlaneMinComplexity <= 1.0f);
00413
00414 Parameters::parse(parameters, Parameters::kIcpPM(), _libpointmatcher);
00415 Parameters::parse(parameters, Parameters::kIcpPMConfig(), _libpointmatcherConfig);
00416 Parameters::parse(parameters, Parameters::kIcpPMOutlierRatio(), _libpointmatcherOutlierRatio);
00417 Parameters::parse(parameters, Parameters::kIcpPMMatcherKnn(), _libpointmatcherKnn);
00418 Parameters::parse(parameters, Parameters::kIcpPMMatcherEpsilon(), _libpointmatcherEpsilon);
00419
00420 #ifndef RTABMAP_POINTMATCHER
00421 if(_libpointmatcher)
00422 {
00423 UWARN("Parameter %s is set to true but RTAB-Map has not been built with libpointmatcher support. Setting to false.", Parameters::kIcpPM().c_str());
00424 _libpointmatcher = false;
00425 }
00426 #else
00427 if(_libpointmatcher)
00428 {
00429 UINFO("libpointmatcher enabled! config=\"%s\"", _libpointmatcherConfig.c_str());
00430 if(_libpointmatcherICP!=0)
00431 {
00432 delete (PM::ICP*)_libpointmatcherICP;
00433 _libpointmatcherICP = 0;
00434 }
00435
00436 _libpointmatcherICP = new PM::ICP();
00437
00438 PM::ICP * icp = (PM::ICP*)_libpointmatcherICP;
00439
00440 bool useDefaults = true;
00441 if(!_libpointmatcherConfig.empty())
00442 {
00443
00444 std::ifstream ifs(_libpointmatcherConfig.c_str());
00445 if (ifs.good())
00446 {
00447 icp->loadFromYaml(ifs);
00448 useDefaults = false;
00449 }
00450 else
00451 {
00452 UERROR("Cannot open libpointmatcher config file \"%s\", using default values instead.", _libpointmatcherConfig.c_str());
00453 }
00454 }
00455 if(useDefaults)
00456 {
00457
00458
00459 icp->setDefault();
00460
00461 icp->readingDataPointsFilters.clear();
00462 icp->readingDataPointsFilters.push_back(PM::get().DataPointsFilterRegistrar.create("IdentityDataPointsFilter"));
00463
00464 icp->referenceDataPointsFilters.clear();
00465 icp->referenceDataPointsFilters.push_back(PM::get().DataPointsFilterRegistrar.create("IdentityDataPointsFilter"));
00466
00467 PM::Parameters params;
00468 params["maxDist"] = uNumber2Str(_maxCorrespondenceDistance);
00469 params["knn"] = uNumber2Str(_libpointmatcherKnn);
00470 params["epsilon"] = uNumber2Str(_libpointmatcherEpsilon);
00471 icp->matcher.reset(PM::get().MatcherRegistrar.create("KDTreeMatcher", params));
00472 params.clear();
00473
00474 params["ratio"] = uNumber2Str(_libpointmatcherOutlierRatio);
00475 icp->outlierFilters.clear();
00476 icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("TrimmedDistOutlierFilter", params));
00477 params.clear();
00478 if(_pointToPlane)
00479 {
00480 params["maxAngle"] = uNumber2Str(_maxRotation<=0.0f?M_PI:_maxRotation);
00481 icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("SurfaceNormalOutlierFilter", params));
00482 params.clear();
00483
00484 params["force2D"] = force3DoF()?"1":"0";
00485 icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPlaneErrorMinimizer", params));
00486 params.clear();
00487 }
00488 else
00489 {
00490 icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer"));
00491 }
00492
00493 icp->transformationCheckers.clear();
00494 params["maxIterationCount"] = uNumber2Str(_maxIterations);
00495 icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("CounterTransformationChecker", params));
00496 params.clear();
00497
00498 params["minDiffRotErr"] = uNumber2Str(_epsilon*_epsilon*100.0f);
00499 params["minDiffTransErr"] = uNumber2Str(_epsilon*_epsilon);
00500 params["smoothLength"] = uNumber2Str(4);
00501 icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("DifferentialTransformationChecker", params));
00502 params.clear();
00503
00504 params["maxRotationNorm"] = uNumber2Str(_maxRotation<=0.0f?M_PI:_maxRotation);
00505 params["maxTranslationNorm"] = uNumber2Str(_maxTranslation<=0.0f?std::numeric_limits<float>::max():_maxTranslation);
00506 icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("BoundTransformationChecker", params));
00507 params.clear();
00508 }
00509 }
00510 #endif
00511
00512 UASSERT_MSG(_voxelSize >= 0, uFormat("value=%d", _voxelSize).c_str());
00513 UASSERT_MSG(_downsamplingStep >= 0, uFormat("value=%d", _downsamplingStep).c_str());
00514 UASSERT_MSG(_maxCorrespondenceDistance > 0.0f, uFormat("value=%f", _maxCorrespondenceDistance).c_str());
00515 UASSERT_MSG(_maxIterations > 0, uFormat("value=%d", _maxIterations).c_str());
00516 UASSERT(_epsilon >= 0.0f);
00517 UASSERT_MSG(_correspondenceRatio >=0.0f && _correspondenceRatio <=1.0f, uFormat("value=%f", _correspondenceRatio).c_str());
00518 UASSERT_MSG(!_pointToPlane || (_pointToPlane && (_pointToPlaneK > 0 || _pointToPlaneRadius > 0.0f)), uFormat("_pointToPlaneK=%d _pointToPlaneRadius=%f", _pointToPlaneK, _pointToPlaneRadius).c_str());
00519 }
00520
00521 Transform RegistrationIcp::computeTransformationImpl(
00522 Signature & fromSignature,
00523 Signature & toSignature,
00524 Transform guess,
00525 RegistrationInfo & info) const
00526 {
00527 UDEBUG("Guess transform = %s", guess.prettyPrint().c_str());
00528 UDEBUG("Voxel size=%f", _voxelSize);
00529 UDEBUG("PointToPlane=%d", _pointToPlane?1:0);
00530 UDEBUG("Normal neighborhood=%d", _pointToPlaneK);
00531 UDEBUG("Normal radius=%f", _pointToPlaneRadius);
00532 UDEBUG("Max correspondence distance=%f", _maxCorrespondenceDistance);
00533 UDEBUG("Max Iterations=%d", _maxIterations);
00534 UDEBUG("Correspondence Ratio=%f", _correspondenceRatio);
00535 UDEBUG("Max translation=%f", _maxTranslation);
00536 UDEBUG("Max rotation=%f", _maxRotation);
00537 UDEBUG("Downsampling step=%d", _downsamplingStep);
00538 UDEBUG("libpointmatcher=%d (knn=%d, outlier ratio=%f)", _libpointmatcher?1:0, _libpointmatcherKnn, _libpointmatcherOutlierRatio);
00539
00540 UTimer timer;
00541 std::string msg;
00542 Transform transform;
00543
00544 SensorData & dataFrom = fromSignature.sensorData();
00545 SensorData & dataTo = toSignature.sensorData();
00546
00547 UDEBUG("size from=%d (format=%d, max pts=%d) to=%d (format=%d, max pts=%d)",
00548 dataFrom.laserScanRaw().size(),
00549 (int)dataFrom.laserScanRaw().format(),
00550 dataFrom.laserScanRaw().maxPoints(),
00551 dataTo.laserScanRaw().size(),
00552 (int)dataTo.laserScanRaw().format(),
00553 dataTo.laserScanRaw().maxPoints());
00554
00555 if(!guess.isNull() && !dataFrom.laserScanRaw().isEmpty() && !dataTo.laserScanRaw().isEmpty())
00556 {
00557
00558 LaserScan fromScan = dataFrom.laserScanRaw();
00559 LaserScan toScan = dataTo.laserScanRaw();
00560 if(_downsamplingStep>1)
00561 {
00562 fromScan = util3d::downsample(fromScan, _downsamplingStep);
00563 toScan = util3d::downsample(toScan, _downsamplingStep);
00564 UDEBUG("Downsampling time (step=%d) = %f s", _downsamplingStep, timer.ticks());
00565 }
00566
00567 if(fromScan.size() && toScan.size())
00568 {
00569 Transform icpT;
00570 bool hasConverged = false;
00571 float correspondencesRatio = 0.0f;
00572 int correspondences = 0;
00573 double variance = 1.0;
00574 bool transformComputed = false;
00575 bool tooLowComplexityForPlaneToPlane = false;
00576 cv::Mat complexityVectors;
00577
00578 if( _pointToPlane &&
00579 _voxelSize == 0.0f &&
00580 fromScan.hasNormals() &&
00581 toScan.hasNormals() &&
00582 !((fromScan.is2d() || toScan.is2d()) && !_libpointmatcher))
00583 {
00584
00585
00586 cv::Mat complexityVectorsFrom, complexityVectorsTo;
00587 double fromComplexity = util3d::computeNormalsComplexity(fromScan, &complexityVectorsFrom);
00588 double toComplexity = util3d::computeNormalsComplexity(toScan, &complexityVectorsTo);
00589 float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
00590 info.icpStructuralComplexity = complexity;
00591 if(complexity < _pointToPlaneMinComplexity)
00592 {
00593 tooLowComplexityForPlaneToPlane = true;
00594 complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
00595 UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): %f < %f (%s). PointToPoint is done instead.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
00596 }
00597 else
00598 {
00599 pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromScan.localTransform());
00600 pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
00601
00602 fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
00603 toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
00604
00605
00606 UDEBUG("Conversion time = %f s", timer.ticks());
00607 pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
00608 #ifdef RTABMAP_POINTMATCHER
00609 if(_libpointmatcher)
00610 {
00611
00612 DP data = laserScanToDP(fromScan);
00613 DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.maxRange(), toScan.format(), guess * toScan.localTransform()));
00614
00615
00616 PM::TransformationParameters T;
00617 try
00618 {
00619 UASSERT(_libpointmatcherICP != 0);
00620 PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
00621 UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
00622 T = icp(data, ref);
00623 icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
00624 UDEBUG("libpointmatcher icp...done! T=%s", icpT.prettyPrint().c_str());
00625
00626 float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
00627 UDEBUG("match ratio: %f", matchRatio);
00628
00629 if(!icpT.isNull())
00630 {
00631 fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
00632 hasConverged = true;
00633 }
00634 }
00635 catch(const std::exception & e)
00636 {
00637 msg = uFormat("libpointmatcher has failed: %s", e.what());
00638 }
00639 }
00640 else
00641 #endif
00642 {
00643 icpT = util3d::icpPointToPlane(
00644 fromCloudNormals,
00645 toCloudNormals,
00646 _maxCorrespondenceDistance,
00647 _maxIterations,
00648 hasConverged,
00649 *fromCloudNormalsRegistered,
00650 _epsilon,
00651 this->force3DoF());
00652 }
00653
00654 if(!icpT.isNull() && hasConverged)
00655 {
00656 util3d::computeVarianceAndCorrespondences(
00657 fromCloudNormalsRegistered,
00658 toCloudNormals,
00659 _maxCorrespondenceDistance,
00660 _maxRotation,
00661 variance,
00662 correspondences);
00663 }
00664 transformComputed = true;
00665 }
00666 }
00667
00668 int maxLaserScansFrom = fromScan.maxPoints();
00669 int maxLaserScansTo = toScan.maxPoints();
00670 if(!transformComputed)
00671 {
00672 pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromScan.localTransform());
00673 pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toScan.localTransform());
00674 UDEBUG("Conversion time = %f s", timer.ticks());
00675
00676 pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
00677 pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
00678 if(_voxelSize > 0.0f)
00679 {
00680 float pointsBeforeFiltering = (float)fromCloudFiltered->size();
00681 fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize);
00682 float ratioFrom = float(fromCloudFiltered->size()) / pointsBeforeFiltering;
00683 maxLaserScansFrom = int(float(maxLaserScansFrom) * ratioFrom);
00684
00685 pointsBeforeFiltering = (float)toCloudFiltered->size();
00686 toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize);
00687 float ratioTo = float(toCloudFiltered->size()) / pointsBeforeFiltering;
00688 maxLaserScansTo = int(float(maxLaserScansTo) * ratioTo);
00689
00690 UDEBUG("Voxel filtering time (voxel=%f m, ratioFrom=%f->%d/%d ratioTo=%f->%d/%d) = %f s",
00691 _voxelSize,
00692 ratioFrom,
00693 (int)fromCloudFiltered->size(),
00694 maxLaserScansFrom,
00695 ratioTo,
00696 (int)toCloudFiltered->size(),
00697 maxLaserScansTo,
00698 timer.ticks());
00699 }
00700
00701 pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
00702 if(_pointToPlane &&
00703 !tooLowComplexityForPlaneToPlane &&
00704 !((fromScan.is2d()|| toScan.is2d()) && !_libpointmatcher))
00705 {
00706 Eigen::Vector3f viewpointFrom(fromScan.localTransform().x(), fromScan.localTransform().y(), fromScan.localTransform().z());
00707 pcl::PointCloud<pcl::Normal>::Ptr normalsFrom;
00708 if(fromScan.is2d())
00709 {
00710 if(_voxelSize > 0.0f)
00711 {
00712 normalsFrom = util3d::computeNormals2D(
00713 fromCloudFiltered,
00714 _pointToPlaneK,
00715 _pointToPlaneRadius,
00716 viewpointFrom);
00717 }
00718 else
00719 {
00720 normalsFrom = util3d::computeFastOrganizedNormals2D(
00721 fromCloudFiltered,
00722 _pointToPlaneK,
00723 _pointToPlaneRadius,
00724 viewpointFrom);
00725 }
00726 }
00727 else
00728 {
00729 normalsFrom = util3d::computeNormals(fromCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointFrom);
00730 }
00731
00732 Transform toT = guess * toScan.localTransform();
00733 Eigen::Vector3f viewpointTo(toT.x(), toT.y(), toT.z());
00734 pcl::PointCloud<pcl::Normal>::Ptr normalsTo;
00735 if(toScan.is2d())
00736 {
00737 if(_voxelSize > 0.0f)
00738 {
00739 normalsTo = util3d::computeNormals2D(
00740 toCloudFiltered,
00741 _pointToPlaneK,
00742 _pointToPlaneRadius,
00743 viewpointTo);
00744 }
00745 else
00746 {
00747 normalsTo = util3d::computeFastOrganizedNormals2D(
00748 toCloudFiltered,
00749 _pointToPlaneK,
00750 _pointToPlaneRadius,
00751 viewpointTo);
00752 }
00753 }
00754 else
00755 {
00756 normalsTo = util3d::computeNormals(toCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointTo);
00757 }
00758
00759 cv::Mat complexityVectorsFrom, complexityVectorsTo;
00760 double fromComplexity = util3d::computeNormalsComplexity(*normalsFrom, fromScan.is2d(), &complexityVectorsFrom);
00761 double toComplexity = util3d::computeNormalsComplexity(*normalsTo, toScan.is2d(), &complexityVectorsTo);
00762 float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
00763 info.icpStructuralComplexity = complexity;
00764 if(complexity < _pointToPlaneMinComplexity)
00765 {
00766 tooLowComplexityForPlaneToPlane = true;
00767 complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
00768 UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): %f < %f (%s). PointToPoint is done instead.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
00769 }
00770 else
00771 {
00772 pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
00773 pcl::concatenateFields(*fromCloudFiltered, *normalsFrom, *fromCloudNormals);
00774
00775 pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
00776 pcl::concatenateFields(*toCloudFiltered, *normalsTo, *toCloudNormals);
00777
00778 std::vector<int> indices;
00779 toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
00780 fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
00781
00782
00783 if(fromScan.is2d())
00784 {
00785 fromSignature.sensorData().setLaserScanRaw(
00786 LaserScan(
00787 util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
00788 maxLaserScansFrom,
00789 fromScan.maxRange(),
00790 LaserScan::kXYNormal,
00791 fromScan.localTransform()));
00792 }
00793 else
00794 {
00795 fromSignature.sensorData().setLaserScanRaw(
00796 LaserScan(
00797 util3d::laserScanFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
00798 maxLaserScansFrom,
00799 fromScan.maxRange(),
00800 LaserScan::kXYZNormal,
00801 fromScan.localTransform()));
00802 }
00803 if(toScan.is2d())
00804 {
00805 toSignature.sensorData().setLaserScanRaw(
00806 LaserScan(
00807 util3d::laserScan2dFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
00808 maxLaserScansTo,
00809 toScan.maxRange(),
00810 LaserScan::kXYNormal,
00811 toScan.localTransform()));
00812 }
00813 else
00814 {
00815 toSignature.sensorData().setLaserScanRaw(
00816 LaserScan(
00817 util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
00818 maxLaserScansTo,
00819 toScan.maxRange(),
00820 LaserScan::kXYZNormal,
00821 toScan.localTransform()));
00822 }
00823 UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks());
00824 fromScan = fromSignature.sensorData().laserScanRaw();
00825 toScan = toSignature.sensorData().laserScanRaw();
00826
00827 if(toCloudNormals->size() && fromCloudNormals->size())
00828 {
00829 pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
00830
00831 #ifdef RTABMAP_POINTMATCHER
00832 if(_libpointmatcher)
00833 {
00834
00835 DP data = laserScanToDP(fromScan);
00836 DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.maxRange(), toScan.format(), guess*toScan.localTransform()));
00837
00838
00839 PM::TransformationParameters T;
00840 try
00841 {
00842 UASSERT(_libpointmatcherICP != 0);
00843 PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
00844 UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
00845 T = icp(data, ref);
00846 UDEBUG("libpointmatcher icp...done!");
00847 icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
00848
00849 float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
00850 UDEBUG("match ratio: %f", matchRatio);
00851
00852 if(!icpT.isNull())
00853 {
00854 fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
00855 hasConverged = true;
00856 }
00857 }
00858 catch(const std::exception & e)
00859 {
00860 msg = uFormat("libpointmatcher has failed: %s", e.what());
00861 }
00862 }
00863 else
00864 #endif
00865 {
00866 icpT = util3d::icpPointToPlane(
00867 fromCloudNormals,
00868 toCloudNormals,
00869 _maxCorrespondenceDistance,
00870 _maxIterations,
00871 hasConverged,
00872 *fromCloudNormalsRegistered,
00873 _epsilon,
00874 this->force3DoF());
00875 }
00876
00877 if(!icpT.isNull() && hasConverged)
00878 {
00879 util3d::computeVarianceAndCorrespondences(
00880 fromCloudNormalsRegistered,
00881 toCloudNormals,
00882 _maxCorrespondenceDistance,
00883 _maxRotation,
00884 variance,
00885 correspondences);
00886 }
00887 }
00888 transformComputed = true;
00889 }
00890 }
00891
00892 if(!transformComputed)
00893 {
00894 if(_pointToPlane && !tooLowComplexityForPlaneToPlane && ((fromScan.is2d() || toScan.is2d()) && !_libpointmatcher))
00895 {
00896 UWARN("ICP PointToPlane ignored for 2d scans with PCL registration (some crash issues). Use libpointmatcher (%s) or disable %s to avoid this warning.", Parameters::kIcpPM().c_str(), Parameters::kIcpPointToPlane().c_str());
00897 }
00898
00899 if(_voxelSize > 0.0f || !tooLowComplexityForPlaneToPlane)
00900 {
00901
00902 if(fromScan.is2d())
00903 {
00904 fromSignature.sensorData().setLaserScanRaw(
00905 LaserScan(
00906 util3d::laserScan2dFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
00907 maxLaserScansFrom,
00908 fromScan.maxRange(),
00909 LaserScan::kXY,
00910 fromScan.localTransform()));
00911 }
00912 else
00913 {
00914 fromSignature.sensorData().setLaserScanRaw(
00915 LaserScan(
00916 util3d::laserScanFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
00917 maxLaserScansFrom,
00918 fromScan.maxRange(),
00919 LaserScan::kXYZ,
00920 fromScan.localTransform()));
00921 }
00922 if(toScan.is2d())
00923 {
00924 toSignature.sensorData().setLaserScanRaw(
00925 LaserScan(
00926 util3d::laserScan2dFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
00927 maxLaserScansTo,
00928 toScan.maxRange(),
00929 LaserScan::kXY,
00930 toScan.localTransform()));
00931 }
00932 else
00933 {
00934 toSignature.sensorData().setLaserScanRaw(
00935 LaserScan(
00936 util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
00937 maxLaserScansTo,
00938 toScan.maxRange(),
00939 LaserScan::kXYZ,
00940 toScan.localTransform()));
00941 }
00942 fromScan = fromSignature.sensorData().laserScanRaw();
00943 toScan = toSignature.sensorData().laserScanRaw();
00944 }
00945
00946 #ifdef RTABMAP_POINTMATCHER
00947 if(_libpointmatcher)
00948 {
00949
00950 DP data = laserScanToDP(fromScan);
00951 DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.maxRange(), toScan.format(), guess*toScan.localTransform()));
00952
00953
00954 PM::TransformationParameters T;
00955 try
00956 {
00957 UASSERT(_libpointmatcherICP != 0);
00958 PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
00959 UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
00960 if(_pointToPlane)
00961 {
00962
00963 PM::ICP & icpTmp = icp;
00964 icpTmp.errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer"));
00965
00966 for(PM::OutlierFilters::iterator iter=icpTmp.outlierFilters.begin(); iter!=icpTmp.outlierFilters.end();)
00967 {
00968 if((*iter)->className.compare("SurfaceNormalOutlierFilter") == 0)
00969 {
00970 iter = icpTmp.outlierFilters.erase(iter);
00971 }
00972 else
00973 {
00974 ++iter;
00975 }
00976 }
00977
00978 T = icpTmp(data, ref);
00979 }
00980 else
00981 {
00982 T = icp(data, ref);
00983 }
00984 UDEBUG("libpointmatcher icp...done!");
00985 icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
00986
00987 float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
00988 UDEBUG("match ratio: %f", matchRatio);
00989
00990 if(!icpT.isNull())
00991 {
00992 fromCloudRegistered = util3d::transformPointCloud(fromCloudFiltered, icpT);
00993 hasConverged = true;
00994 }
00995 }
00996 catch(const std::exception & e)
00997 {
00998 msg = uFormat("libpointmatcher has failed: %s", e.what());
00999 }
01000 }
01001 else
01002 #endif
01003 {
01004 icpT = util3d::icp(
01005 fromCloudFiltered,
01006 toCloudFiltered,
01007 _maxCorrespondenceDistance,
01008 _maxIterations,
01009 hasConverged,
01010 *fromCloudRegistered,
01011 _epsilon,
01012 this->force3DoF());
01013 }
01014
01015 if(!icpT.isNull() && hasConverged)
01016 {
01017 if(tooLowComplexityForPlaneToPlane)
01018 {
01019 Transform guessInv = guess.inverse();
01020 Transform t = guessInv * icpT.inverse() * guess;
01021 Eigen::Vector3f v(t.x(), t.y(), t.z());
01022 if(complexityVectors.cols == 2)
01023 {
01024
01025 Eigen::Vector3f n(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), 0.0f);
01026 float a = v.dot(n);
01027 v = n*a;
01028 }
01029 else if(complexityVectors.rows == 3)
01030 {
01031
01032 Eigen::Vector3f n1(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2));
01033 Eigen::Vector3f n2(complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2));
01034 float a = v.dot(n1);
01035 float b = v.dot(n2);
01036 v = n1*a;
01037 v += n2*b;
01038 }
01039 else
01040 {
01041 UWARN("not supposed to be here!");
01042 v = Eigen::Vector3f(0,0,0);
01043 }
01044 float roll, pitch, yaw;
01045 t.getEulerAngles(roll, pitch, yaw);
01046 t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
01047 icpT = guess * t.inverse() * guessInv;
01048
01049 if(fromScan.hasNormals() && toScan.hasNormals())
01050 {
01051
01052 pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromScan.localTransform());
01053 pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
01054
01055 util3d::computeVarianceAndCorrespondences(
01056 fromCloudNormalsRegistered,
01057 toCloudNormals,
01058 _maxCorrespondenceDistance,
01059 _maxRotation,
01060 variance,
01061 correspondences);
01062 }
01063 else
01064 {
01065 util3d::computeVarianceAndCorrespondences(
01066 fromCloudRegistered,
01067 toCloudFiltered,
01068 _maxCorrespondenceDistance,
01069 variance,
01070 correspondences);
01071 }
01072 }
01073 else
01074 {
01075 util3d::computeVarianceAndCorrespondences(
01076 fromCloudRegistered,
01077 toCloudFiltered,
01078 _maxCorrespondenceDistance,
01079 variance,
01080 correspondences);
01081 }
01082 }
01083 }
01084 }
01085 UDEBUG("ICP (iterations=%d) time = %f s", _maxIterations, timer.ticks());
01086
01087 if(!icpT.isNull() && hasConverged)
01088 {
01089 float ix,iy,iz, iroll,ipitch,iyaw;
01090 Transform icpInTargetReferential = guess.inverse() * icpT.inverse() * guess;
01091 icpInTargetReferential.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
01092 info.icpTranslation = uMax3(fabs(ix), fabs(iy), fabs(iz));
01093 info.icpRotation = uMax3(fabs(iroll), fabs(ipitch), fabs(iyaw));
01094 if((_maxTranslation>0.0f &&
01095 info.icpTranslation > _maxTranslation)
01096 ||
01097 (_maxRotation>0.0f &&
01098 info.icpRotation > _maxRotation))
01099 {
01100 msg = uFormat("Cannot compute transform (ICP correction too large -> %f m %f rad, limits=%f m, %f rad)",
01101 info.icpTranslation,
01102 info.icpRotation,
01103 _maxTranslation,
01104 _maxRotation);
01105 UINFO(msg.c_str());
01106 }
01107 else
01108 {
01109
01110 int maxLaserScans = maxLaserScansTo?maxLaserScansTo:maxLaserScansFrom;
01111 UDEBUG("Max scans=%d (from=%d, to=%d)", maxLaserScans, maxLaserScansFrom, maxLaserScansTo);
01112
01113 if(maxLaserScans)
01114 {
01115 correspondencesRatio = float(correspondences)/float(maxLaserScans);
01116 }
01117 else
01118 {
01119 static bool warningShown = false;
01120 if(!warningShown)
01121 {
01122 UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute! This message will only appear once.",
01123 dataTo.id());
01124 warningShown = true;
01125 }
01126 correspondencesRatio = float(correspondences)/float(toScan.size()>fromScan.size()?toScan.size():fromScan.size());
01127 }
01128
01129 variance/=10.0;
01130
01131 UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%), from guess: trans=%f rot=%f",
01132 dataTo.id(), dataFrom.id(),
01133 hasConverged?"true":"false",
01134 variance,
01135 correspondences,
01136 maxLaserScans>0?maxLaserScans:(int)(toScan.size()>fromScan.size()?toScan.size():fromScan.size()),
01137 correspondencesRatio*100.0f,
01138 info.icpTranslation,
01139 info.icpRotation);
01140
01141 if(correspondences == 0)
01142 {
01143 UERROR("Transform is found but no correspondences has been found!? Variance is unknown!");
01144 }
01145 else
01146 {
01147 info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
01148 }
01149 info.icpInliersRatio = correspondencesRatio;
01150
01151 if(correspondencesRatio <= _correspondenceRatio)
01152 {
01153 msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f maxLaserScans=%d)",
01154 correspondences, correspondencesRatio, _correspondenceRatio, maxLaserScans);
01155 UINFO(msg.c_str());
01156 }
01157 else
01158 {
01159 transform = icpT.inverse()*guess;
01160 }
01161 }
01162 }
01163 else
01164 {
01165 if(msg.empty())
01166 {
01167 msg = uFormat("Cannot compute transform (converged=%s var=%f)",
01168 hasConverged?"true":"false", variance);
01169 }
01170 UINFO(msg.c_str());
01171 }
01172 }
01173 else
01174 {
01175 msg = "Laser scans empty ?!?";
01176 UWARN(msg.c_str());
01177 }
01178 }
01179 else if(dataTo.isValid())
01180 {
01181 if(guess.isNull())
01182 {
01183 msg = "RegistrationIcp cannot do registration with a null guess.";
01184 }
01185 else
01186 {
01187 msg = uFormat("Laser scans empty?!? (new[%d]=%d old[%d]=%d)",
01188 dataTo.id(), dataTo.laserScanRaw().size(),
01189 dataFrom.id(), dataFrom.laserScanRaw().size());
01190 }
01191 UERROR(msg.c_str());
01192 }
01193
01194
01195 info.rejectedMsg = msg;
01196
01197 UDEBUG("New transform = %s", transform.prettyPrint().c_str());
01198 return transform;
01199 }
01200
01201 }