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
00030
00031
00032
00033
00034
00035
00036 #ifndef __POINTMATCHER_CORE_H
00037 #define __POINTMATCHER_CORE_H
00038
00039 #ifndef EIGEN_USE_NEW_STDVECTOR
00040 #define EIGEN_USE_NEW_STDVECTOR
00041 #endif // EIGEN_USE_NEW_STDVECTOR
00042 #define EIGEN2_SUPPORT
00043 #include "Eigen/StdVector"
00044 #include "Eigen/Core"
00045 #include "Eigen/Geometry"
00046
00047 #include "nabo/nabo.h"
00048
00049 #include <boost/thread/mutex.hpp>
00050
00051 #include <stdexcept>
00052 #include <limits>
00053 #include <iostream>
00054 #include <ostream>
00055 #include <memory>
00056
00057 #include "Parametrizable.h"
00058 #include "Registrar.h"
00059
00060 #if NABO_VERSION_INT < 10001
00061 #error "You need libnabo version 1.0.1 or greater"
00062 #endif
00063
00070
00071 #define POINTMATCHER_VERSION "1.0.2"
00072
00073 #define POINTMATCHER_VERSION_INT 10002
00074
00076 namespace PointMatcherSupport
00077 {
00079 struct InvalidModuleType: std::runtime_error
00080 {
00081 InvalidModuleType(const std::string& reason);
00082 };
00083
00085 template<typename S>
00086 struct SharedPtrVector: public std::vector<std::shared_ptr<S>>
00087 {
00089 void push_back(S* v) { std::vector<std::shared_ptr<S>>::push_back(std::shared_ptr<S>(v)); }
00090 };
00091
00093 struct Logger: public Parametrizable
00094 {
00095 Logger();
00096 Logger(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
00097
00098 virtual ~Logger();
00099 virtual bool hasInfoChannel() const;
00100 virtual void beginInfoEntry(const char *file, unsigned line, const char *func);
00101 virtual std::ostream* infoStream();
00102 virtual void finishInfoEntry(const char *file, unsigned line, const char *func);
00103 virtual bool hasWarningChannel() const;
00104 virtual void beginWarningEntry(const char *file, unsigned line, const char *func);
00105 virtual std::ostream* warningStream();
00106 virtual void finishWarningEntry(const char *file, unsigned line, const char *func);
00107 };
00108
00109 void setLogger(Logger* newLogger);
00110
00111 void validateFile(const std::string& fileName);
00112
00114 typedef std::map<std::string, std::vector<std::string>> CsvElements;
00115 }
00116
00118 template<typename T>
00119 struct PointMatcher
00120 {
00121
00122
00123
00124
00126 #define ZERO_PLUS_EPS (0. + std::numeric_limits<double>::epsilon())
00127
00128 #define ONE_MINUS_EPS (1. - std::numeric_limits<double>::epsilon())
00129
00130
00131
00132
00133
00135 struct ConvergenceError: std::runtime_error
00136 {
00137 ConvergenceError(const std::string& reason);
00138 };
00139
00140
00141
00142
00143
00145 typedef T ScalarType;
00147 typedef typename Eigen::Matrix<T, Eigen::Dynamic, 1> Vector;
00149 typedef std::vector<Vector, Eigen::aligned_allocator<Vector> > VectorVector;
00151 typedef typename Eigen::Quaternion<T> Quaternion;
00153 typedef std::vector<Quaternion, Eigen::aligned_allocator<Quaternion> > QuaternionVector;
00155 typedef typename Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Matrix;
00157 typedef typename Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic> IntMatrix;
00158
00160
00163 typedef Matrix TransformationParameters;
00164
00165
00166 typedef PointMatcherSupport::Parametrizable Parametrizable;
00167 typedef Parametrizable::Parameters Parameters;
00168 typedef Parametrizable::ParameterDoc ParameterDoc;
00169 typedef Parametrizable::ParametersDoc ParametersDoc;
00170 typedef Parametrizable::InvalidParameter InvalidParameter;
00171
00172
00173
00174
00175
00177
00188 struct DataPoints
00189 {
00191 typedef Eigen::Block<Matrix> View;
00193 typedef const Eigen::Block<const Matrix> ConstView;
00195 typedef typename Matrix::Index Index;
00196
00198 struct Label
00199 {
00200 std::string text;
00201 size_t span;
00202 Label(const std::string& text = "", const size_t span = 0);
00203 bool operator ==(const Label& that) const;
00204 };
00206 struct Labels: std::vector<Label>
00207 {
00208 typedef typename std::vector<Label>::const_iterator const_iterator;
00209 Labels();
00210 Labels(const Label& label);
00211 bool contains(const std::string& text) const;
00212 size_t totalDim() const;
00213 };
00214
00216 struct InvalidField: std::runtime_error
00217 {
00218 InvalidField(const std::string& reason);
00219 };
00220
00221 DataPoints();
00222 DataPoints(const Labels& featureLabels, const Labels& descriptorLabels, const size_t pointCount);
00223 DataPoints(const Matrix& features, const Labels& featureLabels);
00224 DataPoints(const Matrix& features, const Labels& featureLabels, const Matrix& descriptors, const Labels& descriptorLabels);
00225 bool operator ==(const DataPoints& that) const;
00226
00227 void save(const std::string& fileName) const;
00228 static DataPoints load(const std::string& fileName);
00229
00230 void concatenate(const DataPoints& dp);
00231 void conservativeResize(Index pointCount);
00232 DataPoints createSimilarEmpty() const;
00233 DataPoints createSimilarEmpty(Index pointCount) const;
00234 void setColFrom(Index thisCol, const DataPoints& that, Index thatCol);
00235
00236 void allocateFeature(const std::string& name, const unsigned dim);
00237 void allocateFeatures(const Labels& newLabels);
00238 void addFeature(const std::string& name, const Matrix& newFeature);
00239 Matrix getFeatureCopyByName(const std::string& name) const;
00240 ConstView getFeatureViewByName(const std::string& name) const;
00241 View getFeatureViewByName(const std::string& name);
00242 ConstView getFeatureRowViewByName(const std::string& name, const unsigned row) const;
00243 View getFeatureRowViewByName(const std::string& name, const unsigned row);
00244 bool featureExists(const std::string& name) const;
00245 bool featureExists(const std::string& name, const unsigned dim) const;
00246 unsigned getFeatureDimension(const std::string& name) const;
00247 unsigned getFeatureStartingRow(const std::string& name) const;
00248
00249 void allocateDescriptor(const std::string& name, const unsigned dim);
00250 void allocateDescriptors(const Labels& newLabels);
00251 void addDescriptor(const std::string& name, const Matrix& newDescriptor);
00252 Matrix getDescriptorCopyByName(const std::string& name) const;
00253 ConstView getDescriptorViewByName(const std::string& name) const;
00254 View getDescriptorViewByName(const std::string& name);
00255 ConstView getDescriptorRowViewByName(const std::string& name, const unsigned row) const;
00256 View getDescriptorRowViewByName(const std::string& name, const unsigned row);
00257 bool descriptorExists(const std::string& name) const;
00258 bool descriptorExists(const std::string& name, const unsigned dim) const;
00259 unsigned getDescriptorDimension(const std::string& name) const;
00260 unsigned getDescriptorStartingRow(const std::string& name) const;
00261 void assertDescriptorConsistency() const;
00262
00263 Matrix features;
00264 Labels featureLabels;
00265 Matrix descriptors;
00266 Labels descriptorLabels;
00267
00268 private:
00269 void allocateFields(const Labels& newLabels, Labels& labels, Matrix& data) const;
00270 void allocateField(const std::string& name, const unsigned dim, Labels& labels, Matrix& data) const;
00271 void addField(const std::string& name, const Matrix& newField, Labels& labels, Matrix& data) const;
00272 ConstView getConstViewByName(const std::string& name, const Labels& labels, const Matrix& data, const int viewRow = -1) const;
00273 View getViewByName(const std::string& name, const Labels& labels, Matrix& data, const int viewRow = -1) const;
00274 bool fieldExists(const std::string& name, const unsigned dim, const Labels& labels) const;
00275 unsigned getFieldDimension(const std::string& name, const Labels& labels) const;
00276 unsigned getFieldStartingRow(const std::string& name, const Labels& labels) const;
00277 };
00278
00279 static void swapDataPoints(DataPoints& a, DataPoints& b);
00280
00281
00282
00283
00284
00286
00290 struct Matches
00291 {
00292 typedef Matrix Dists;
00293 typedef IntMatrix Ids;
00294
00295 Matches();
00296 Matches(const Dists& dists, const Ids ids);
00297 Matches(const int knn, const int pointsCount);
00298
00299 Dists dists;
00300 Ids ids;
00301
00302 T getDistsQuantile(const T quantile) const;
00303 };
00304
00306
00309 typedef Matrix OutlierWeights;
00310
00311
00312
00313
00314
00316 struct Transformation: public Parametrizable
00317 {
00318 Transformation();
00319 Transformation(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
00320 virtual ~Transformation();
00321
00323 virtual DataPoints compute(const DataPoints& input, const TransformationParameters& parameters) const = 0;
00324 };
00325
00327 struct Transformations: public PointMatcherSupport::SharedPtrVector<Transformation>
00328 {
00329 void apply(DataPoints& cloud, const TransformationParameters& parameters) const;
00330 };
00331 typedef typename Transformations::iterator TransformationsIt;
00332 typedef typename Transformations::const_iterator TransformationsConstIt;
00333
00334 DEF_REGISTRAR(Transformation)
00335
00336
00337
00338
00339
00342 struct DataPointsFilter: public Parametrizable
00343 {
00344 DataPointsFilter();
00345 DataPointsFilter(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
00346 virtual ~DataPointsFilter();
00347 virtual void init();
00348
00350 virtual DataPoints filter(const DataPoints& input) = 0;
00351 };
00352
00354 struct DataPointsFilters: public PointMatcherSupport::SharedPtrVector<DataPointsFilter>
00355 {
00356 DataPointsFilters();
00357 DataPointsFilters(std::istream& in);
00358 void init();
00359 void apply(DataPoints& cloud);
00360 };
00361 typedef typename DataPointsFilters::iterator DataPointsFiltersIt;
00362 typedef typename DataPointsFilters::const_iterator DataPointsFiltersConstIt;
00363
00364 DEF_REGISTRAR(DataPointsFilter)
00365
00366
00367
00368
00369
00372 struct Matcher: public Parametrizable
00373 {
00374 unsigned long visitCounter;
00375
00376 Matcher();
00377 Matcher(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
00378 virtual ~Matcher();
00379
00380 void resetVisitCount();
00381 unsigned long getVisitCount() const;
00382
00384 virtual void init(const DataPoints& filteredReference) = 0;
00386 virtual Matches findClosests(const DataPoints& filteredReading) = 0;
00387 };
00388
00389 DEF_REGISTRAR(Matcher)
00390
00391
00392
00393
00394
00398 struct OutlierFilter: public Parametrizable
00399 {
00400 OutlierFilter();
00401 OutlierFilter(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
00402
00403 virtual ~OutlierFilter();
00404
00406 virtual OutlierWeights compute(const DataPoints& filteredReading, const DataPoints& filteredReference, const Matches& input) = 0;
00407 };
00408
00409
00411 struct OutlierFilters: public PointMatcherSupport::SharedPtrVector<OutlierFilter>
00412 {
00413 OutlierWeights compute(const DataPoints& filteredReading, const DataPoints& filteredReference, const Matches& input);
00414 };
00415
00416 typedef typename OutlierFilters::const_iterator OutlierFiltersConstIt;
00417 typedef typename OutlierFilters::iterator OutlierFiltersIt;
00418
00419 DEF_REGISTRAR(OutlierFilter)
00420
00421
00422
00423
00424
00427 struct ErrorMinimizer: public Parametrizable
00428 {
00430 struct ErrorElements
00431 {
00432 DataPoints reading;
00433 DataPoints reference;
00434 OutlierWeights weights;
00435 Matches matches;
00436
00437 ErrorElements(const DataPoints& reading=DataPoints(), const DataPoints reference = DataPoints(), const OutlierWeights weights = OutlierWeights(), const Matches matches = Matches());
00438 };
00439
00440 ErrorMinimizer();
00441 virtual ~ErrorMinimizer();
00442
00443 T getPointUsedRatio() const;
00444 T getWeightedPointUsedRatio() const;
00445 virtual T getOverlap() const;
00446
00448 virtual TransformationParameters compute(const DataPoints& filteredReading, const DataPoints& filteredReference, const OutlierWeights& outlierWeights, const Matches& matches) = 0;
00449
00450
00451 protected:
00452
00453 static Matrix crossProduct(const Matrix& A, const Matrix& B);
00454 ErrorElements& getMatchedPoints(const DataPoints& reading, const DataPoints& reference, const Matches& matches, const OutlierWeights& outlierWeights);
00455
00456 protected:
00457 T pointUsedRatio;
00458 T weightedPointUsedRatio;
00459 ErrorElements lastErrorElements;
00460 };
00461
00462 DEF_REGISTRAR(ErrorMinimizer)
00463
00464
00465
00466
00467
00471 struct TransformationChecker: public Parametrizable
00472 {
00473 protected:
00474 typedef std::vector<std::string> StringVector;
00475 Vector limits;
00476 Vector conditionVariables;
00477 StringVector limitNames;
00478 StringVector conditionVariableNames;
00479
00480 public:
00481 TransformationChecker();
00482 TransformationChecker(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
00483 virtual ~TransformationChecker();
00485 virtual void init(const TransformationParameters& parameters, bool& iterate) = 0;
00487 virtual void check(const TransformationParameters& parameters, bool& iterate) = 0;
00488
00489 const Vector& getLimits() const;
00490 const Vector& getConditionVariables() const;
00491 const StringVector& getLimitNames() const;
00492 const StringVector& getConditionVariableNames() const;
00493
00494 protected:
00495 static Vector matrixToAngles(const TransformationParameters& parameters);
00496 };
00497
00499 struct TransformationCheckers: public PointMatcherSupport::SharedPtrVector<TransformationChecker>
00500 {
00501 void init(const TransformationParameters& parameters, bool& iterate);
00502 void check(const TransformationParameters& parameters, bool& iterate);
00503 };
00504 typedef typename TransformationCheckers::iterator TransformationCheckersIt;
00505 typedef typename TransformationCheckers::const_iterator TransformationCheckersConstIt;
00506
00507 DEF_REGISTRAR(TransformationChecker)
00508
00509
00510
00511
00512 struct Inspector: public Parametrizable
00513 {
00514
00515 Inspector();
00516 Inspector(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
00517
00518
00519 virtual ~Inspector();
00520 virtual void init();
00521
00522
00523 virtual void addStat(const std::string& name, double data);
00524 virtual void dumpStats(std::ostream& stream);
00525 virtual void dumpStatsHeader(std::ostream& stream);
00526
00527
00528 virtual void dumpIteration(const size_t iterationNumber, const TransformationParameters& parameters, const DataPoints& filteredReference, const DataPoints& reading, const Matches& matches, const OutlierWeights& outlierWeights, const TransformationCheckers& transformationCheckers);
00529 virtual void finish(const size_t iterationCount);
00530 };
00531
00532 DEF_REGISTRAR(Inspector)
00533
00534
00535
00536 DEF_REGISTRAR_IFACE(Logger, PointMatcherSupport::Logger)
00537
00538
00539
00540
00541
00543 struct ICPChainBase
00544 {
00545 public:
00546 DataPointsFilters readingDataPointsFilters;
00547 DataPointsFilters readingStepDataPointsFilters;
00548 DataPointsFilters referenceDataPointsFilters;
00549 Transformations transformations;
00550 std::shared_ptr<Matcher> matcher;
00551 OutlierFilters outlierFilters;
00552 std::shared_ptr<ErrorMinimizer> errorMinimizer;
00553 TransformationCheckers transformationCheckers;
00554 std::shared_ptr<Inspector> inspector;
00555
00556 virtual ~ICPChainBase();
00557
00558 virtual void setDefault();
00559
00560 void loadFromYaml(std::istream& in);
00561 unsigned getPrefilteredReadingPtsCount() const;
00562 unsigned getPrefilteredReferencePtsCount() const;
00563
00564 protected:
00565 unsigned prefilteredReadingPtsCount;
00566 unsigned prefilteredReferencePtsCount;
00567
00568 ICPChainBase();
00569
00570 void cleanup();
00571
00572 #ifdef HAVE_YAML_CPP
00573 virtual void loadAdditionalYAMLContent(YAML::Node& doc);
00574
00575 template<typename R>
00576 const std::string& createModulesFromRegistrar(const std::string& regName, const YAML::Node& doc, const R& registrar, PointMatcherSupport::SharedPtrVector<typename R::TargetType>& modules);
00577
00578 template<typename R>
00579 const std::string& createModuleFromRegistrar(const std::string& regName, const YAML::Node& doc, const R& registrar, std::shared_ptr<typename R::TargetType>& module);
00580
00581
00582
00583 #endif // HAVE_YAML_CPP
00584 };
00585
00587 struct ICP: ICPChainBase
00588 {
00589 TransformationParameters operator()(
00590 const DataPoints& readingIn,
00591 const DataPoints& referenceIn);
00592
00593 TransformationParameters operator()(
00594 const DataPoints& readingIn,
00595 const DataPoints& referenceIn,
00596 const TransformationParameters& initialTransformationParameters);
00597
00598 TransformationParameters compute(
00599 const DataPoints& readingIn,
00600 const DataPoints& referenceIn,
00601 const TransformationParameters& initialTransformationParameters);
00602
00603 protected:
00604 TransformationParameters computeWithTransformedReference(
00605 const DataPoints& readingIn,
00606 const DataPoints& reference,
00607 const TransformationParameters& T_refIn_refMean,
00608 const TransformationParameters& initialTransformationParameters);
00609 };
00610
00612 struct ICPSequence: public ICP
00613 {
00614 TransformationParameters operator()(
00615 const DataPoints& cloudIn);
00616 TransformationParameters operator()(
00617 const DataPoints& cloudIn,
00618 const TransformationParameters& initialTransformationParameters);
00619 TransformationParameters compute(
00620 const DataPoints& cloudIn,
00621 const TransformationParameters& initialTransformationParameters);
00622
00623 bool hasMap() const;
00624 bool setMap(const DataPoints& map);
00625 void clearMap();
00626 const DataPoints& getInternalMap() const;
00627 const DataPoints getMap() const;
00628
00629 protected:
00630 DataPoints mapPointCloud;
00631 TransformationParameters T_refIn_refMean;
00632 };
00633
00634
00635
00636
00637
00638 PointMatcher();
00639
00640 static const PointMatcher& get();
00641 };
00642
00643 #endif // __POINTMATCHER_CORE_H
00644