libpointmatcher 1.0.2
|
00001 // kate: replace-tabs off; indent-width 4; indent-mode normal 00002 // vim: ts=4:sw=4:noexpandtab 00003 /* 00004 00005 Copyright (c) 2010--2012, 00006 François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland 00007 You can contact the authors at <f dot pomerleau at gmail dot com> and 00008 <stephane at magnenat dot net> 00009 00010 All rights reserved. 00011 00012 Redistribution and use in source and binary forms, with or without 00013 modification, are permitted provided that the following conditions are met: 00014 * Redistributions of source code must retain the above copyright 00015 notice, this list of conditions and the following disclaimer. 00016 * Redistributions in binary form must reproduce the above copyright 00017 notice, this list of conditions and the following disclaimer in the 00018 documentation and/or other materials provided with the distribution. 00019 * Neither the name of the <organization> nor the 00020 names of its contributors may be used to endorse or promote products 00021 derived from this software without specific prior written permission. 00022 00023 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00024 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00025 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00026 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY 00027 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00028 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00030 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00031 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00032 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 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 // macros for constants 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 // exceptions 00132 // --------------------------------- 00133 00135 struct ConvergenceError: std::runtime_error 00136 { 00137 ConvergenceError(const std::string& reason); 00138 }; 00139 00140 // --------------------------------- 00141 // eigen and nabo-based types 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 // alias for scope reasons 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 // input types 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 // intermediate types 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 // types of processing bricks 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 // helper functions 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 // performance statistics 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 // data statistics 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 // algorithms 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 /*template<typename R> 00582 typename R::TargetType* createModuleFromRegistrar(const YAML::Node& module, const R& registrar);*/ 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 // Instance-related functions 00636 // --------------------------------- 00637 00638 PointMatcher(); 00639 00640 static const PointMatcher& get(); 00641 }; // PointMatcher<T> 00642 00643 #endif // __POINTMATCHER_CORE_H 00644