libpointmatcher
1.1.0
|
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.1.0" 00072 00073 #define POINTMATCHER_VERSION_INT 10100 00074 00076 namespace PointMatcherSupport 00077 { 00078 using boost::assign::list_of; 00079 using boost::assign::map_list_of; 00080 // TODO: gather all exceptions 00081 00083 struct InvalidModuleType: std::runtime_error 00084 { 00085 InvalidModuleType(const std::string& reason); 00086 }; 00087 00089 struct TransformationError: std::runtime_error 00090 { 00092 TransformationError(const std::string& reason); 00093 }; 00094 00096 template<typename S> 00097 struct SharedPtrVector: public std::vector<boost::shared_ptr<S> > 00098 { 00100 void push_back(S* v) { std::vector<boost::shared_ptr<S> >::push_back(boost::shared_ptr<S>(v)); } 00101 }; 00102 00104 struct Logger: public Parametrizable 00105 { 00106 Logger(); 00107 Logger(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params); 00108 00109 virtual ~Logger(); 00110 virtual bool hasInfoChannel() const; 00111 virtual void beginInfoEntry(const char *file, unsigned line, const char *func); 00112 virtual std::ostream* infoStream(); 00113 virtual void finishInfoEntry(const char *file, unsigned line, const char *func); 00114 virtual bool hasWarningChannel() const; 00115 virtual void beginWarningEntry(const char *file, unsigned line, const char *func); 00116 virtual std::ostream* warningStream(); 00117 virtual void finishWarningEntry(const char *file, unsigned line, const char *func); 00118 }; 00119 00120 void setLogger(Logger* newLogger); 00121 00122 void validateFile(const std::string& fileName); 00123 00125 typedef std::map<std::string, std::vector<std::string> > CsvElements; 00126 } 00127 00129 template<typename T> 00130 struct PointMatcher 00131 { 00132 // --------------------------------- 00133 // macros for constants 00134 // --------------------------------- 00135 00137 #define ZERO_PLUS_EPS (0. + std::numeric_limits<double>::epsilon()) 00138 00139 #define ONE_MINUS_EPS (1. - std::numeric_limits<double>::epsilon()) 00140 00141 // --------------------------------- 00142 // exceptions 00143 // --------------------------------- 00144 00145 //TODO: gather exceptions here and in Exceptions.cpp 00146 00148 struct ConvergenceError: std::runtime_error 00149 { 00150 ConvergenceError(const std::string& reason); 00151 }; 00152 00153 00154 // --------------------------------- 00155 // eigen and nabo-based types 00156 // --------------------------------- 00157 00159 typedef T ScalarType; 00161 typedef typename Eigen::Matrix<T, Eigen::Dynamic, 1> Vector; 00163 typedef std::vector<Vector, Eigen::aligned_allocator<Vector> > VectorVector; 00165 typedef typename Eigen::Quaternion<T> Quaternion; 00167 typedef std::vector<Quaternion, Eigen::aligned_allocator<Quaternion> > QuaternionVector; 00169 typedef typename Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Matrix; 00171 typedef typename Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic> IntMatrix; 00172 00174 00177 typedef Matrix TransformationParameters; 00178 00179 // alias for scope reasons 00180 typedef PointMatcherSupport::Parametrizable Parametrizable; 00181 typedef Parametrizable::Parameters Parameters; 00182 typedef Parametrizable::ParameterDoc ParameterDoc; 00183 typedef Parametrizable::ParametersDoc ParametersDoc; 00184 typedef Parametrizable::InvalidParameter InvalidParameter; 00185 00186 // --------------------------------- 00187 // input types 00188 // --------------------------------- 00189 00191 00202 struct DataPoints 00203 { 00205 typedef Eigen::Block<Matrix> View; 00207 typedef const Eigen::Block<const Matrix> ConstView; 00209 typedef typename Matrix::Index Index; 00210 00212 struct Label 00213 { 00214 std::string text; 00215 size_t span; 00216 Label(const std::string& text = "", const size_t span = 0); 00217 bool operator ==(const Label& that) const; 00218 }; 00220 struct Labels: std::vector<Label> 00221 { 00222 typedef typename std::vector<Label>::const_iterator const_iterator; 00223 Labels(); 00224 Labels(const Label& label); 00225 bool contains(const std::string& text) const; 00226 size_t totalDim() const; 00227 }; 00228 00230 struct InvalidField: std::runtime_error 00231 { 00232 InvalidField(const std::string& reason); 00233 }; 00234 00235 DataPoints(); 00236 DataPoints(const Labels& featureLabels, const Labels& descriptorLabels, const size_t pointCount); 00237 DataPoints(const Matrix& features, const Labels& featureLabels); 00238 DataPoints(const Matrix& features, const Labels& featureLabels, const Matrix& descriptors, const Labels& descriptorLabels); 00239 bool operator ==(const DataPoints& that) const; 00240 00241 void save(const std::string& fileName) const; 00242 static DataPoints load(const std::string& fileName); 00243 00244 void concatenate(const DataPoints& dp); 00245 void conservativeResize(Index pointCount); 00246 DataPoints createSimilarEmpty() const; 00247 DataPoints createSimilarEmpty(Index pointCount) const; 00248 void setColFrom(Index thisCol, const DataPoints& that, Index thatCol); 00249 00250 void allocateFeature(const std::string& name, const unsigned dim); 00251 void allocateFeatures(const Labels& newLabels); 00252 void addFeature(const std::string& name, const Matrix& newFeature); 00253 Matrix getFeatureCopyByName(const std::string& name) const; 00254 ConstView getFeatureViewByName(const std::string& name) const; 00255 View getFeatureViewByName(const std::string& name); 00256 ConstView getFeatureRowViewByName(const std::string& name, const unsigned row) const; 00257 View getFeatureRowViewByName(const std::string& name, const unsigned row); 00258 bool featureExists(const std::string& name) const; 00259 bool featureExists(const std::string& name, const unsigned dim) const; 00260 unsigned getFeatureDimension(const std::string& name) const; 00261 unsigned getFeatureStartingRow(const std::string& name) const; 00262 00263 void allocateDescriptor(const std::string& name, const unsigned dim); 00264 void allocateDescriptors(const Labels& newLabels); 00265 void addDescriptor(const std::string& name, const Matrix& newDescriptor); 00266 Matrix getDescriptorCopyByName(const std::string& name) const; 00267 ConstView getDescriptorViewByName(const std::string& name) const; 00268 View getDescriptorViewByName(const std::string& name); 00269 ConstView getDescriptorRowViewByName(const std::string& name, const unsigned row) const; 00270 View getDescriptorRowViewByName(const std::string& name, const unsigned row); 00271 bool descriptorExists(const std::string& name) const; 00272 bool descriptorExists(const std::string& name, const unsigned dim) const; 00273 unsigned getDescriptorDimension(const std::string& name) const; 00274 unsigned getDescriptorStartingRow(const std::string& name) const; 00275 void assertDescriptorConsistency() const; 00276 00277 Matrix features; 00278 Labels featureLabels; 00279 Matrix descriptors; 00280 Labels descriptorLabels; 00281 00282 private: 00283 void allocateFields(const Labels& newLabels, Labels& labels, Matrix& data) const; 00284 void allocateField(const std::string& name, const unsigned dim, Labels& labels, Matrix& data) const; 00285 void addField(const std::string& name, const Matrix& newField, Labels& labels, Matrix& data) const; 00286 ConstView getConstViewByName(const std::string& name, const Labels& labels, const Matrix& data, const int viewRow = -1) const; 00287 View getViewByName(const std::string& name, const Labels& labels, Matrix& data, const int viewRow = -1) const; 00288 bool fieldExists(const std::string& name, const unsigned dim, const Labels& labels) const; 00289 unsigned getFieldDimension(const std::string& name, const Labels& labels) const; 00290 unsigned getFieldStartingRow(const std::string& name, const Labels& labels) const; 00291 }; 00292 00293 static void swapDataPoints(DataPoints& a, DataPoints& b); 00294 00295 // --------------------------------- 00296 // intermediate types 00297 // --------------------------------- 00298 00300 00304 struct Matches 00305 { 00306 typedef Matrix Dists; 00307 typedef IntMatrix Ids; 00308 00309 Matches(); 00310 Matches(const Dists& dists, const Ids ids); 00311 Matches(const int knn, const int pointsCount); 00312 00313 Dists dists; 00314 Ids ids; 00315 00316 T getDistsQuantile(const T quantile) const; 00317 }; 00318 00320 00323 typedef Matrix OutlierWeights; 00324 00325 // --------------------------------- 00326 // types of processing bricks 00327 // --------------------------------- 00328 00330 struct Transformation: public Parametrizable 00331 { 00332 Transformation(); 00333 Transformation(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params); 00334 virtual ~Transformation(); 00335 00337 virtual DataPoints compute(const DataPoints& input, const TransformationParameters& parameters) const = 0; 00338 00340 virtual bool checkParameters(const TransformationParameters& parameters) const = 0; 00341 00343 virtual TransformationParameters correctParameters(const TransformationParameters& parameters) const = 0; 00344 00345 }; 00346 00348 struct Transformations: public PointMatcherSupport::SharedPtrVector<Transformation> 00349 { 00350 void apply(DataPoints& cloud, const TransformationParameters& parameters) const; 00351 }; 00352 typedef typename Transformations::iterator TransformationsIt; 00353 typedef typename Transformations::const_iterator TransformationsConstIt; 00354 00355 DEF_REGISTRAR(Transformation) 00356 00357 // --------------------------------- 00358 00359 00360 00363 struct DataPointsFilter: public Parametrizable 00364 { 00365 DataPointsFilter(); 00366 DataPointsFilter(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params); 00367 virtual ~DataPointsFilter(); 00368 virtual void init(); 00369 00371 virtual DataPoints filter(const DataPoints& input) = 0; 00372 }; 00373 00375 struct DataPointsFilters: public PointMatcherSupport::SharedPtrVector<DataPointsFilter> 00376 { 00377 DataPointsFilters(); 00378 DataPointsFilters(std::istream& in); 00379 void init(); 00380 void apply(DataPoints& cloud); 00381 }; 00382 typedef typename DataPointsFilters::iterator DataPointsFiltersIt; 00383 typedef typename DataPointsFilters::const_iterator DataPointsFiltersConstIt; 00384 00385 DEF_REGISTRAR(DataPointsFilter) 00386 00387 // --------------------------------- 00388 00389 00390 00393 struct Matcher: public Parametrizable 00394 { 00395 unsigned long visitCounter; 00396 00397 Matcher(); 00398 Matcher(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params); 00399 virtual ~Matcher(); 00400 00401 void resetVisitCount(); 00402 unsigned long getVisitCount() const; 00403 00405 virtual void init(const DataPoints& filteredReference) = 0; 00407 virtual Matches findClosests(const DataPoints& filteredReading) = 0; 00408 }; 00409 00410 DEF_REGISTRAR(Matcher) 00411 00412 // --------------------------------- 00413 00414 00415 00419 struct OutlierFilter: public Parametrizable 00420 { 00421 OutlierFilter(); 00422 OutlierFilter(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params); 00423 00424 virtual ~OutlierFilter(); 00425 00427 virtual OutlierWeights compute(const DataPoints& filteredReading, const DataPoints& filteredReference, const Matches& input) = 0; 00428 }; 00429 00430 00432 struct OutlierFilters: public PointMatcherSupport::SharedPtrVector<OutlierFilter> 00433 { 00434 00435 OutlierWeights compute(const DataPoints& filteredReading, const DataPoints& filteredReference, const Matches& input); 00436 00437 }; 00438 00439 typedef typename OutlierFilters::const_iterator OutlierFiltersConstIt; 00440 typedef typename OutlierFilters::iterator OutlierFiltersIt; 00441 00442 DEF_REGISTRAR(OutlierFilter) 00443 00444 // --------------------------------- 00445 00446 00447 00450 struct ErrorMinimizer: public Parametrizable 00451 { 00453 struct ErrorElements 00454 { 00455 DataPoints reading; 00456 DataPoints reference; 00457 OutlierWeights weights; 00458 Matches matches; 00459 00460 ErrorElements(const DataPoints& reading=DataPoints(), const DataPoints reference = DataPoints(), const OutlierWeights weights = OutlierWeights(), const Matches matches = Matches()); 00461 }; 00462 00463 ErrorMinimizer(); 00464 ErrorMinimizer(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params); 00465 virtual ~ErrorMinimizer(); 00466 00467 T getPointUsedRatio() const; 00468 T getWeightedPointUsedRatio() const; 00469 virtual T getOverlap() const; 00470 00472 virtual TransformationParameters compute(const DataPoints& filteredReading, const DataPoints& filteredReference, const OutlierWeights& outlierWeights, const Matches& matches) = 0; 00473 00474 00475 protected: 00476 // helper functions 00477 static Matrix crossProduct(const Matrix& A, const Matrix& B); 00478 ErrorElements& getMatchedPoints(const DataPoints& reading, const DataPoints& reference, const Matches& matches, const OutlierWeights& outlierWeights); 00479 00480 protected: 00481 T pointUsedRatio; 00482 T weightedPointUsedRatio; 00483 ErrorElements lastErrorElements; 00484 }; 00485 00486 DEF_REGISTRAR(ErrorMinimizer) 00487 00488 // --------------------------------- 00489 00490 00491 00495 struct TransformationChecker: public Parametrizable 00496 { 00497 protected: 00498 typedef std::vector<std::string> StringVector; 00499 Vector limits; 00500 Vector conditionVariables; 00501 StringVector limitNames; 00502 StringVector conditionVariableNames; 00503 00504 public: 00505 TransformationChecker(); 00506 TransformationChecker(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params); 00507 virtual ~TransformationChecker(); 00509 virtual void init(const TransformationParameters& parameters, bool& iterate) = 0; 00511 virtual void check(const TransformationParameters& parameters, bool& iterate) = 0; 00512 00513 const Vector& getLimits() const; 00514 const Vector& getConditionVariables() const; 00515 const StringVector& getLimitNames() const; 00516 const StringVector& getConditionVariableNames() const; 00517 00518 protected: 00519 static Vector matrixToAngles(const TransformationParameters& parameters); 00520 }; 00521 00523 struct TransformationCheckers: public PointMatcherSupport::SharedPtrVector<TransformationChecker> 00524 { 00525 void init(const TransformationParameters& parameters, bool& iterate); 00526 void check(const TransformationParameters& parameters, bool& iterate); 00527 }; 00528 typedef typename TransformationCheckers::iterator TransformationCheckersIt; 00529 typedef typename TransformationCheckers::const_iterator TransformationCheckersConstIt; 00530 00531 DEF_REGISTRAR(TransformationChecker) 00532 00533 // --------------------------------- 00534 00535 00536 struct Inspector: public Parametrizable 00537 { 00538 00539 Inspector(); 00540 Inspector(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params); 00541 00542 // 00543 virtual ~Inspector(); 00544 virtual void init(); 00545 00546 // performance statistics 00547 virtual void addStat(const std::string& name, double data); 00548 virtual void dumpStats(std::ostream& stream); 00549 virtual void dumpStatsHeader(std::ostream& stream); 00550 00551 // data statistics 00552 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); 00553 virtual void finish(const size_t iterationCount); 00554 }; 00555 00556 DEF_REGISTRAR(Inspector) 00557 00558 // --------------------------------- 00559 00560 DEF_REGISTRAR_IFACE(Logger, PointMatcherSupport::Logger) 00561 00562 // --------------------------------- 00563 00564 // algorithms 00565 00567 struct ICPChainBase 00568 { 00569 public: 00570 DataPointsFilters readingDataPointsFilters; 00571 DataPointsFilters readingStepDataPointsFilters; 00572 DataPointsFilters referenceDataPointsFilters; 00573 Transformations transformations; 00574 boost::shared_ptr<Matcher> matcher; 00575 OutlierFilters outlierFilters; 00576 boost::shared_ptr<ErrorMinimizer> errorMinimizer; 00577 TransformationCheckers transformationCheckers; 00578 boost::shared_ptr<Inspector> inspector; 00579 00580 virtual ~ICPChainBase(); 00581 00582 virtual void setDefault(); 00583 00584 void loadFromYaml(std::istream& in); 00585 unsigned getPrefilteredReadingPtsCount() const; 00586 unsigned getPrefilteredReferencePtsCount() const; 00587 00588 protected: 00589 unsigned prefilteredReadingPtsCount; 00590 unsigned prefilteredReferencePtsCount; 00591 00592 ICPChainBase(); 00593 00594 void cleanup(); 00595 00596 #ifdef HAVE_YAML_CPP 00597 virtual void loadAdditionalYAMLContent(YAML::Node& doc); 00598 00599 template<typename R> 00600 const std::string& createModulesFromRegistrar(const std::string& regName, const YAML::Node& doc, const R& registrar, PointMatcherSupport::SharedPtrVector<typename R::TargetType>& modules); 00601 00602 template<typename R> 00603 const std::string& createModuleFromRegistrar(const std::string& regName, const YAML::Node& doc, const R& registrar, boost::shared_ptr<typename R::TargetType>& module); 00604 00605 /*template<typename R> 00606 typename R::TargetType* createModuleFromRegistrar(const YAML::Node& module, const R& registrar);*/ 00607 #endif // HAVE_YAML_CPP 00608 }; 00609 00611 struct ICP: ICPChainBase 00612 { 00613 TransformationParameters operator()( 00614 const DataPoints& readingIn, 00615 const DataPoints& referenceIn); 00616 00617 TransformationParameters operator()( 00618 const DataPoints& readingIn, 00619 const DataPoints& referenceIn, 00620 const TransformationParameters& initialTransformationParameters); 00621 00622 TransformationParameters compute( 00623 const DataPoints& readingIn, 00624 const DataPoints& referenceIn, 00625 const TransformationParameters& initialTransformationParameters); 00626 00627 protected: 00628 TransformationParameters computeWithTransformedReference( 00629 const DataPoints& readingIn, 00630 const DataPoints& reference, 00631 const TransformationParameters& T_refIn_refMean, 00632 const TransformationParameters& initialTransformationParameters); 00633 }; 00634 00636 struct ICPSequence: public ICP 00637 { 00638 TransformationParameters operator()( 00639 const DataPoints& cloudIn); 00640 TransformationParameters operator()( 00641 const DataPoints& cloudIn, 00642 const TransformationParameters& initialTransformationParameters); 00643 TransformationParameters compute( 00644 const DataPoints& cloudIn, 00645 const TransformationParameters& initialTransformationParameters); 00646 00647 bool hasMap() const; 00648 bool setMap(const DataPoints& map); 00649 void clearMap(); 00650 const DataPoints& getInternalMap() const; 00651 const DataPoints getMap() const; 00652 00653 protected: 00654 DataPoints mapPointCloud; 00655 TransformationParameters T_refIn_refMean; 00656 }; 00657 00658 // --------------------------------- 00659 // Instance-related functions 00660 // --------------------------------- 00661 00662 PointMatcher(); 00663 00664 static const PointMatcher& get(); 00665 00666 00667 }; // PointMatcher<T> 00668 00669 #endif // __POINTMATCHER_CORE_H 00670