libpointmatcher  1.0.2
PointMatcher.h
Go to the documentation of this file.
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