libpointmatcher  1.1.0
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.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