49 #include <yaml-cpp/yaml.h>
55 InvalidModuleType::InvalidModuleType(
const std::string& reason):
62 prefilteredReadingPtsCount(0),
63 prefilteredReferencePtsCount(0),
64 maxNumIterationsReached(false)
78 readingDataPointsFilters.clear();
79 readingStepDataPointsFilters.clear();
80 referenceDataPointsFilters.clear();
82 outlierFilters.clear();
84 transformationCheckers.clear();
104 this->
matcher = std::make_shared<typename MatchersImpl<T>::KDTreeMatcher>();
105 this->
errorMinimizer = std::make_shared<PointToPlaneErrorMinimizer<T> >();
108 this->
inspector = std::make_shared<typename InspectorsImpl<T>::NullInspector>();
117 YAML::Node doc = YAML::Load(in);
118 typedef set<string> StringSet;
119 StringSet usedModuleTypes;
128 usedModuleTypes.insert(createModuleFromRegistrar(
"logger", doc, pm.REG(
Logger),
logger));
130 usedModuleTypes.insert(createModulesFromRegistrar(
"readingDataPointsFilters", doc, pm.REG(
DataPointsFilter), readingDataPointsFilters));
131 usedModuleTypes.insert(createModulesFromRegistrar(
"readingStepDataPointsFilters", doc, pm.REG(
DataPointsFilter), readingStepDataPointsFilters));
132 usedModuleTypes.insert(createModulesFromRegistrar(
"referenceDataPointsFilters", doc, pm.REG(
DataPointsFilter), referenceDataPointsFilters));
134 usedModuleTypes.insert(createModuleFromRegistrar(
"matcher", doc, pm.REG(
Matcher),
matcher));
135 usedModuleTypes.insert(createModulesFromRegistrar(
"outlierFilters", doc, pm.REG(
OutlierFilter), outlierFilters));
139 if (nodeVal(
"errorMinimizer", doc) !=
"PointToPointSimilarityErrorMinimizer")
144 usedModuleTypes.insert(createModulesFromRegistrar(
"transformationCheckers", doc, pm.REG(
TransformationChecker), transformationCheckers));
145 usedModuleTypes.insert(createModuleFromRegistrar(
"inspector", doc, pm.REG(
Inspector),
inspector));
152 for(YAML::const_iterator moduleTypeIt = doc.begin(); moduleTypeIt != doc.end(); ++moduleTypeIt)
156 if (usedModuleTypes.find(moduleType) == usedModuleTypes.end())
158 (boost::format(
"Module type %1% does not exist") % moduleType).str()
167 return prefilteredReadingPtsCount;
174 return prefilteredReferencePtsCount;
181 return maxNumIterationsReached;
189 const YAML::Node reg = doc[regName];
194 for(YAML::const_iterator moduleIt = reg.begin(); moduleIt != reg.end(); ++moduleIt)
196 const YAML::Node& module(*moduleIt);
197 modules.push_back(registrar.createFromYAML(module));
209 const YAML::Node reg = doc[regName];
214 module = registrar.createFromYAML(reg);
225 const YAML::Node reg = doc[regName];
251 return this->compute(readingIn, referenceIn, identity);
261 return this->compute(readingIn, referenceIn, initialTransformationParameters);
273 throw runtime_error(
"You must setup a matcher before running ICP");
275 throw runtime_error(
"You must setup an error minimizer before running ICP");
277 throw runtime_error(
"You must setup an inspector before running ICP");
287 this->referenceDataPointsFilters.init();
288 this->referenceDataPointsFilters.apply(
reference);
292 const int nbPtsReference =
reference.features.cols();
293 const Vector meanReference =
reference.features.rowwise().sum() / nbPtsReference;
295 T_refIn_refMean.block(0,
dim-1,
dim-1, 1) = meanReference.head(
dim-1);
300 reference.features.topRows(
dim-1).colwise() -= meanReference.head(
dim-1);
303 this->
matcher->init(reference);
310 this->prefilteredReferencePtsCount =
reference.features.cols();
312 return computeWithTransformedReference(readingIn,
reference, T_refIn_refMean, T_refIn_dataIn);
326 if (T_refIn_dataIn.cols() != T_refIn_dataIn.rows()) {
327 throw runtime_error(
"The initial transformation matrix must be squared.");
329 if (
dim != T_refIn_dataIn.cols()) {
330 throw runtime_error(
"The shape of initial transformation matrix must be NxN. "
331 "Where N is the number of rows in the read/reference scans.");
340 this->readingDataPointsFilters.init();
341 this->readingDataPointsFilters.apply(
reading);
347 T_refMean_dataIn = T_refIn_refMean.inverse() * T_refIn_dataIn;
351 this->readingStepDataPointsFilters.init();
358 this->maxNumIterationsReached =
false;
359 this->transformationCheckers.init(T_iter, iterate);
361 size_t iterationCount(0);
368 this->prefilteredReadingPtsCount =
reading.features.cols();
378 this->readingStepDataPointsFilters.apply(stepReading);
387 this->
matcher->findClosests(stepReading)
396 assert(outlierWeights.rows() ==
matches.ids.rows());
397 assert(outlierWeights.cols() ==
matches.ids.cols());
405 iterationCount, T_iter,
reference, stepReading,
matches, outlierWeights, this->transformationCheckers
422 this->transformationCheckers.check(T_iter, iterate);
427 this->maxNumIterationsReached =
true;
433 this->
inspector->addStat(
"IterationsCount", iterationCount);
434 this->
inspector->addStat(
"PointCountTouched", this->
matcher->getVisitCount());
435 this->
matcher->resetVisitCount();
440 LOG_INFO_STREAM(
"PointMatcher::icp - " << iterationCount <<
" iterations took " << t.
elapsed() <<
" [s]");
449 return (T_refIn_refMean * T_iter * T_refMean_dataIn);
460 return (mapPointCloud.features.cols() != 0);
469 throw runtime_error(
"You must setup a matcher before running ICP");
471 throw runtime_error(
"You must setup an inspector before running ICP");
475 const int ptCount(inputCloud.
features.cols());
487 mapPointCloud = inputCloud;
491 const Vector meanMap = mapPointCloud.
features.rowwise().sum() / ptCount;
492 T_refIn_refMean = Matrix::Identity(
dim,
dim);
493 T_refIn_refMean.block(0,
dim-1,
dim-1, 1) = meanMap.head(
dim-1);
498 mapPointCloud.features.topRows(
dim-1).colwise() -= meanMap.head(
dim-1);
501 this->referenceDataPointsFilters.init();
502 this->referenceDataPointsFilters.apply(mapPointCloud);
504 this->
matcher->init(mapPointCloud);
515 const int dim(mapPointCloud.features.rows());
516 T_refIn_refMean = Matrix::Identity(
dim,
dim);
525 if(mapPointCloud.getNbPoints() > 0)
527 this->
matcher->init(mapPointCloud);
536 if(mapPointCloud.getNbPoints() > 0)
538 this->
matcher->init(mapPointCloud);
549 const int dim(mapPointCloud.features.rows());
550 const Vector meanMapNonHomo(T_refIn_refMean.block(0,
dim-1,
dim-1, 1));
551 globalMap.
features.topRows(
dim-1).colwise() += meanMapNonHomo;
567 return mapPointCloud;
583 return this->compute(cloudIn, identity);
591 return this->compute(cloudIn, T_dataInOld_dataInNew);
604 return Matrix::Identity(
dim,
dim);
609 return this->computeWithTransformedReference(cloudIn, mapPointCloud, T_refIn_refMean, T_refIn_dataIn);