40 #include <pcl/conversions.h>
41 #include <pcl/common/pca.h>
42 #include <pcl/common/io.h>
43 #include <pcl/io/pcd_io.h>
44 #include <pcl/io/ply_io.h>
45 #include <pcl/io/vtk_io.h>
47 #ifdef RTABMAP_CCCORELIB
51 #ifdef RTABMAP_POINTMATCHER
60 _maxTranslation(
Parameters::defaultIcpMaxTranslation()),
61 _maxRotation(
Parameters::defaultIcpMaxRotation()),
63 _downsamplingStep(
Parameters::defaultIcpDownsamplingStep()),
66 _maxCorrespondenceDistance(
Parameters::defaultIcpMaxCorrespondenceDistance()),
67 _reciprocalCorrespondences(
Parameters::defaultIcpReciprocalCorrespondences()),
68 _maxIterations(
Parameters::defaultIcpIterations()),
70 _correspondenceRatio(
Parameters::defaultIcpCorrespondenceRatio()),
72 _filtersEnabled(
Parameters::defaultIcpFiltersEnabled()),
73 _pointToPlane(
Parameters::defaultIcpPointToPlane()),
74 _pointToPlaneK(
Parameters::defaultIcpPointToPlaneK()),
75 _pointToPlaneRadius(
Parameters::defaultIcpPointToPlaneRadius()),
76 _pointToPlaneGroundNormalsUp(
Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
77 _pointToPlaneMinComplexity(
Parameters::defaultIcpPointToPlaneMinComplexity()),
78 _pointToPlaneLowComplexityStrategy(
Parameters::defaultIcpPointToPlaneLowComplexityStrategy()),
79 _libpointmatcherConfig(
Parameters::defaultIcpPMConfig()),
80 _libpointmatcherKnn(
Parameters::defaultIcpPMMatcherKnn()),
81 _libpointmatcherEpsilon(
Parameters::defaultIcpPMMatcherEpsilon()),
82 _libpointmatcherIntensity(
Parameters::defaultIcpPMMatcherIntensity()),
83 _outlierRatio(
Parameters::defaultIcpOutlierRatio()),
84 _ccSamplingLimit (
Parameters::defaultIcpCCSamplingLimit()),
85 _ccFilterOutFarthestPoints (
Parameters::defaultIcpCCFilterOutFarthestPoints()),
86 _ccMaxFinalRMS (
Parameters::defaultIcpCCMaxFinalRMS()),
87 _debugExportFormat(
Parameters::defaultIcpDebugExportFormat()),
88 _libpointmatcherICP(0),
89 _libpointmatcherICPFilters(0)
96 #ifdef RTABMAP_POINTMATCHER
140 ParametersMap::const_iterator
iter;
141 if((
iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
148 #ifndef RTABMAP_POINTMATCHER
151 UWARN(
"Parameter %s is set to 1 but RTAB-Map has not been built with libpointmatcher support. Setting to 0.", Parameters::kIcpStrategy().
c_str());
166 bool useDefaults =
true;
174 icp->loadFromYaml(ifs);
179 icpFilters->readingDataPointsFilters =
icp->readingDataPointsFilters;
180 icpFilters->referenceDataPointsFilters =
icp->referenceDataPointsFilters;
182 icp->readingDataPointsFilters.clear();
183 icp->readingDataPointsFilters.push_back(
PM::get().DataPointsFilterRegistrar.create(
"IdentityDataPointsFilter"));
185 icp->referenceDataPointsFilters.clear();
186 icp->referenceDataPointsFilters.push_back(
PM::get().DataPointsFilterRegistrar.create(
"IdentityDataPointsFilter"));
188 catch (
const std::exception &
e)
204 icp->readingDataPointsFilters.clear();
207 icp->referenceDataPointsFilters.clear();
221 #if POINTMATCHER_VERSION_INT >= 10300
230 icp->outlierFilters.clear();
243 params[
"force4DOF"] =
"1";
245 #if POINTMATCHER_VERSION_INT >= 10300
246 icp->errorMinimizer =
PM::get().ErrorMinimizerRegistrar.create(
"PointToPlaneErrorMinimizer",
params);
254 #if POINTMATCHER_VERSION_INT >= 10300
255 icp->errorMinimizer =
PM::get().ErrorMinimizerRegistrar.create(
"PointToPointErrorMinimizer");
261 icp->transformationCheckers.clear();
277 pointToPlane =
icp->errorMinimizer->className.compare(
"PointToPlaneErrorMinimizer")==0;
281 #ifndef RTABMAP_CCCORELIB
284 #ifdef RTABMAP_POINTMATCHER
289 UWARN(
"Parameter %s is set to 2 but RTAB-Map has not been built with CCCoreLib support. Setting to %d.", Parameters::kIcpStrategy().
c_str(),
_strategy);
294 UWARN(
"%s cannot be used with %s=2 (CCCoreLib), setting %s to false", Parameters::kIcpPointToPlane().
c_str(), Parameters::kIcpStrategy().
c_str(), Parameters::kIcpPointToPlane().
c_str());
301 UWARN(
"%s cannot be used with %s == 0.", Parameters::kIcpForce4DoF().
c_str(), Parameters::kIcpStrategy().
c_str());
321 #ifdef RTABMAP_POINTMATCHER
325 pointToPlane =
icp->errorMinimizer->className.compare(
"PointToPlaneErrorMinimizer")==0;
331 UDEBUG(
"PointToPlane=%d", pointToPlane?1:0);
354 UDEBUG(
"size before filtering, from=%d (format=%s, max pts=%d) to=%d (format=%s, max pts=%d)",
377 #ifdef RTABMAP_POINTMATCHER
381 UDEBUG(
"icp.referenceDataPointsFilters.size()=%d", (
int)filters.referenceDataPointsFilters.size());
382 if(filters.referenceDataPointsFilters.size()>1 ||
383 (filters.referenceDataPointsFilters.size() == 1 && filters.referenceDataPointsFilters[0]->className.compare(
"IdentityDataPointsFilter")!=0))
387 filters.referenceDataPointsFilters.apply(
data);
395 catch(
const std::exception &
e)
397 msg =
uFormat(
"libpointmatcher's data filters have failed: %s",
e.what());
405 maxLaserScansFrom =
int(
float(maxLaserScansFrom) *
ratio);
418 #ifdef RTABMAP_POINTMATCHER
422 UDEBUG(
"icp.readingDataPointsFilters.size()=%d", (
int)filters.readingDataPointsFilters.size());
423 if(filters.readingDataPointsFilters.size()>1 ||
424 (filters.readingDataPointsFilters.size() == 1 && filters.readingDataPointsFilters[0]->className.compare(
"IdentityDataPointsFilter")!=0))
428 filters.readingDataPointsFilters.apply(
data);
436 catch(
const std::exception &
e)
438 msg =
uFormat(
"libpointmatcher's data filters have failed: %s",
e.what());
446 maxLaserScansTo =
int(
float(maxLaserScansTo) *
ratio);
449 UDEBUG(
"size after filtering, from=%d (format=%s, max pts=%d) to=%d (format=%s, max pts=%d), filtering time=%fs",
465 if(fromScan.
size() && toScan.
size())
467 std::string toPrefix =
"rtabmap_icp_scan";
471 std::string fromPrefix =
"rtabmap_icp_scan";
486 UWARN(
"Saved %s.vtk and %s.vtk (%s=\"%s\") to working directory (%s=%s)", fromPrefix.c_str(), toPrefix.c_str(), Parameters::kIcpDebugExportFormat().c_str(),
_debugExportFormat.c_str(), Parameters::kRtabmapWorkingDirectory().c_str(),
_workingDir.c_str());
492 UWARN(
"Saved %s.ply and %s.ply (%s=\"%s\") to directory (%s=%s)", fromPrefix.c_str(), toPrefix.c_str(), Parameters::kIcpDebugExportFormat().c_str(),
_debugExportFormat.c_str(), Parameters::kRtabmapWorkingDirectory().c_str(),
_workingDir.c_str());
498 UWARN(
"Saved %s.pcd and %s.pcd (%s=\"%s\") to working directory (%s=%s)", fromPrefix.c_str(), toPrefix.c_str(), Parameters::kIcpDebugExportFormat().c_str(),
_debugExportFormat.c_str(), Parameters::kRtabmapWorkingDirectory().c_str(),
_workingDir.c_str());
503 UWARN(
"%s is enabled but %s is not, cannot export debug scans.", Parameters::kIcpDebugExportFormat().
c_str(), Parameters::kRtabmapWorkingDirectory().
c_str());
506 bool tooLowComplexityForPlaneToPlane =
false;
507 float secondEigenValue = 1.0f;
508 cv::Mat complexityVectors;
512 UWARN(
"ICP PointToPlane ignored for 2d scans with PCL registration "
513 "(some crash issues). Use libpointmatcher (%s=1) or disable %s "
514 "to avoid this warning.",
515 Parameters::kIcpStrategy().
c_str(),
516 Parameters::kIcpPointToPlane().
c_str());
517 pointToPlane =
false;
526 cv::Mat complexityVectorsFrom, complexityVectorsTo;
527 cv::Mat complexityValuesFrom, complexityValuesTo;
530 float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
531 info.icpStructuralComplexity = complexity;
534 tooLowComplexityForPlaneToPlane =
true;
535 pointToPlane =
false;
536 if(complexity > 0.0
f)
538 complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
540 UASSERT((complexityVectors.rows == 2 && complexityVectors.cols == 2)||
541 (complexityVectors.rows == 3 && complexityVectors.cols == 3));
542 secondEigenValue = complexityValuesFrom.at<
float>(1,0)<complexityValuesTo.at<
float>(1,0)?complexityValuesFrom.at<
float>(1,0):complexityValuesTo.at<
float>(1,0);
543 UWARN(
"ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): (from=%f || to=%f) < %f (%s). Second eigen value=%f. "
544 "PointToPoint is done instead, orientation is still optimized but translation will be limited to "
545 "direction of normals (%s: %s).",
548 fromComplexity<toComplexity?
"From":
"To",
549 complexityVectors.rows==2?
550 uFormat(
"n=%f,%f", complexityVectors.at<
float>(0,0), complexityVectors.at<
float>(0,1)).c_str():
552 uFormat(
"n=%f,%f,%f", complexityVectors.at<
float>(0,0), complexityVectors.at<
float>(0,1), complexityVectors.at<
float>(0,2)).c_str():
553 uFormat(
"n1=%f,%f,%f n2=%f,%f,%f", complexityVectors.at<
float>(0,0), complexityVectors.at<
float>(0,1), complexityVectors.at<
float>(0,2), complexityVectors.at<
float>(1,0), complexityVectors.at<
float>(1,1), complexityVectors.at<
float>(1,2)).c_str());
557 UWARN(
"ICP PointToPlane ignored as structural complexity cannot be computed (from=%f to=%f)!? PointToPoint is done instead.", fromComplexity, toComplexity);
561 std::cout <<
"complexityVectorsFrom = " << std::endl << complexityVectorsFrom << std::endl;
562 std::cout <<
"complexityValuesFrom = " << std::endl << complexityValuesFrom << std::endl;
563 std::cout <<
"complexityVectorsTo = " << std::endl << complexityVectorsTo << std::endl;
564 std::cout <<
"complexityValuesTo = " << std::endl << complexityValuesTo << std::endl;
570 msg =
uFormat(
"Rejecting transform because too low complexity %f (%s=0)",
571 info.icpStructuralComplexity,
572 Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
581 pointToPlane =
false;
585 bool hasConverged =
false;
586 float correspondencesRatio = 0.0f;
587 int correspondences = 0;
588 double variance = 1.0;
602 pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormalsRegistered(
new pcl::PointCloud<pcl::PointXYZINormal>());
603 #ifdef RTABMAP_POINTMATCHER
616 UDEBUG(
"libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
621 float matchRatio =
icp.errorMinimizer->getWeightedPointUsedRatio();
622 UDEBUG(
"match ratio: %f", matchRatio);
630 catch(
const std::exception &
e)
632 msg =
uFormat(
"libpointmatcher has failed: %s",
e.what());
644 *fromCloudNormalsRegistered,
649 if(!icpT.
isNull() && hasConverged)
652 fromCloudNormalsRegistered,
670 pcl::PointCloud<pcl::PointXYZI>::Ptr fromCloudRegistered(
new pcl::PointCloud<pcl::PointXYZI>());
672 #ifdef RTABMAP_POINTMATCHER
685 UDEBUG(
"libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
686 if(tooLowComplexityForPlaneToPlane)
689 PM::ICP icpTmp =
icp;
702 #if POINTMATCHER_VERSION_INT >= 10300
703 icpTmp.matcher =
PM::get().MatcherRegistrar.create(
"KDTreeMatcher",
params);
709 #if POINTMATCHER_VERSION_INT >= 10300
710 icpTmp.errorMinimizer =
PM::get().ErrorMinimizerRegistrar.create(
"PointToPointErrorMinimizer");
714 for(PM::OutlierFilters::iterator
iter=icpTmp.outlierFilters.begin();
iter!=icpTmp.outlierFilters.end();)
716 if((*iter)->className.compare(
"SurfaceNormalOutlierFilter") == 0)
718 iter = icpTmp.outlierFilters.erase(
iter);
732 UDEBUG(
"libpointmatcher icp...done!");
735 float matchRatio =
icp.errorMinimizer->getWeightedPointUsedRatio();
736 UDEBUG(
"match ratio: %f", matchRatio);
743 catch(
const std::exception &
e)
745 msg =
uFormat(
"libpointmatcher has failed: %s",
e.what());
751 #ifdef RTABMAP_CCCORELIB
767 hasConverged = !icpT.
isNull();
778 *fromCloudRegistered,
784 if(!icpT.
isNull() && hasConverged)
786 if(tooLowComplexityForPlaneToPlane)
792 Eigen::Vector3f
v(
t.
x(),
t.
y(),
t.z());
793 if(complexityVectors.cols == 2)
796 Eigen::Vector3f
n(complexityVectors.at<
float>(0,0), complexityVectors.at<
float>(0,1), 0.0f);
798 Eigen::Vector3f vp =
n*
a;
799 UWARN(
"Normals low complexity (%f): Limiting translation from (%f,%f) to (%f,%f)",
800 info.icpStructuralComplexity,
801 v[0],
v[1], vp[0], vp[1]);
804 else if(complexityVectors.rows == 3)
807 Eigen::Vector3f
n1(complexityVectors.at<
float>(0,0), complexityVectors.at<
float>(0,1), complexityVectors.at<
float>(0,2));
808 Eigen::Vector3f
n2(complexityVectors.at<
float>(1,0), complexityVectors.at<
float>(1,1), complexityVectors.at<
float>(1,2));
811 Eigen::Vector3f vp =
n1*
a;
816 UWARN(
"Normals low complexity: Limiting translation from (%f,%f,%f) to (%f,%f,%f)",
817 v[0],
v[1],
v[2], vp[0], vp[1], vp[2]);
822 UWARN(
"not supposed to be here!");
823 v = Eigen::Vector3f(0,0,0);
828 icpT = guess *
t.inverse() * guessInv;
834 UWARN(
"Even if complexity is low (%f), PointToPoint transformation is accepted \"as is\" (%s=2)",
835 info.icpStructuralComplexity,
836 Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
837 if(fromCloudRegistered->empty())
841 else if(fromCloudRegistered->empty())
855 if(!icpT.
isNull() && hasConverged)
857 float ix,iy,iz, iroll,ipitch,iyaw;
868 msg =
uFormat(
"Cannot compute transform (ICP correction too large -> %f m %f rad, limits=%f m, %f rad)",
878 int maxLaserScans = maxLaserScansTo?maxLaserScansTo:maxLaserScansFrom;
879 UDEBUG(
"Max scans=%d (from=%d, to=%d)", maxLaserScans, maxLaserScansFrom, maxLaserScansTo);
883 correspondencesRatio =
float(correspondences)/
float(maxLaserScans);
887 static bool warningShown =
false;
890 UWARN(
"Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute! This message will only appear once.",
899 UDEBUG(
"%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%), from guess: trans=%f rot=%f",
900 dataTo.
id(), dataFrom.
id(),
901 hasConverged?
"true":
"false",
904 maxLaserScans>0?maxLaserScans:(
int)(toScan.
size()>fromScan.
size()?toScan.
size():fromScan.
size()),
905 correspondencesRatio*100.0f,
909 if(correspondences == 0)
911 UWARN(
"Transform is found (%s) but no correspondences has been found!? Variance is unknown!",
916 info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
917 info.covariance(cv::Range(3,6),cv::Range(3,6))/=10.0;
923 info.covariance(cv::Range(3,5),cv::Range(3,5))/=10.0;
926 info.icpInliersRatio = correspondencesRatio;
927 info.icpCorrespondences = correspondences;
931 msg =
uFormat(
"Cannot compute transform (cor=%d corrRatio=%f/%f maxLaserScans=%d)",
941 toPrefix+=
"_registered";
965 msg =
uFormat(
"Cannot compute transform (converged=%s var=%f)",
966 hasConverged?
"true":
"false", variance);
973 msg =
"Laser scans empty ?!?";
979 msg =
"RegistrationIcp cannot do registration with a null guess.";