PointMatcher.h

Go to the documentation of this file.

// kate: replace-tabs off; indent-width 4; indent-mode normal
// vim: ts=4:sw=4:noexpandtab
/*

Copyright (c) 2010--2012,
Francois Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
You can contact the authors at <f dot pomerleau at gmail dot com> and
<stephane at magnenat dot net>

All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the <organization> nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef __POINTMATCHER_CORE_H
#define __POINTMATCHER_CORE_H

#ifndef EIGEN_USE_NEW_STDVECTOR
#define EIGEN_USE_NEW_STDVECTOR
#endif // EIGEN_USE_NEW_STDVECTOR
//#define EIGEN2_SUPPORT
#include "Eigen/StdVector"
#include "Eigen/Core"
#include "Eigen/Geometry"

#include "nabo/nabo.h"

#include <boost/thread/mutex.hpp>

#include <stdexcept>
#include <limits>
#include <iostream>
#include <ostream>
#include <memory>

#include "Parametrizable.h"
#include "Registrar.h"
 
#if NABO_VERSION_INT < 10006
        #error "You need libnabo version 1.0.6 or greater"
#endif


#define POINTMATCHER_VERSION "1.2.3"

#define POINTMATCHER_VERSION_INT 10203

namespace PointMatcherSupport
{
        using boost::assign::list_of;
        using boost::assign::map_list_of;
        // TODO: gather all exceptions

        struct InvalidModuleType: std::runtime_error
        {
                InvalidModuleType(const std::string& reason);
        };

        struct TransformationError: std::runtime_error
        {
                TransformationError(const std::string& reason);
        };

        template<typename S>
        struct SharedPtrVector: public std::vector<boost::shared_ptr<S> >
        {
                void push_back(S* v) { std::vector<boost::shared_ptr<S> >::push_back(boost::shared_ptr<S>(v)); }
        };
        
        struct Logger: public Parametrizable
        {
                Logger();
                Logger(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
                
                virtual ~Logger();
                virtual bool hasInfoChannel() const;
                virtual void beginInfoEntry(const char *file, unsigned line, const char *func);
                virtual std::ostream* infoStream();
                virtual void finishInfoEntry(const char *file, unsigned line, const char *func);
                virtual bool hasWarningChannel() const;
                virtual void beginWarningEntry(const char *file, unsigned line, const char *func);
                virtual std::ostream* warningStream();
                virtual void finishWarningEntry(const char *file, unsigned line, const char *func);
        };
        
        void setLogger(Logger* newLogger);
        
        void validateFile(const std::string& fileName);
        
        typedef std::map<std::string, std::vector<std::string> > CsvElements;
}

template<typename T>
struct PointMatcher
{
        // ---------------------------------
        // macros for constants
        // ---------------------------------
        
        #define ZERO_PLUS_EPS (0. + std::numeric_limits<double>::epsilon())

        #define ONE_MINUS_EPS (1. - std::numeric_limits<double>::epsilon())
        
        // ---------------------------------
        // exceptions
        // ---------------------------------

        //TODO: gather exceptions here and in Exceptions.cpp

        struct ConvergenceError: std::runtime_error
        {
                ConvergenceError(const std::string& reason);
        };


        // ---------------------------------
        // eigen and nabo-based types
        // ---------------------------------
        
        typedef T ScalarType;
        typedef typename Eigen::Matrix<T, Eigen::Dynamic, 1> Vector;
        typedef std::vector<Vector, Eigen::aligned_allocator<Vector> > VectorVector;
        typedef typename Eigen::Quaternion<T> Quaternion;
        typedef std::vector<Quaternion, Eigen::aligned_allocator<Quaternion> > QuaternionVector;
        typedef typename Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Matrix;
        typedef typename Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic> IntMatrix;
        

        typedef Matrix TransformationParameters;
        
        // alias for scope reasons
        typedef PointMatcherSupport::Parametrizable Parametrizable; 
        typedef Parametrizable::Parameters Parameters; 
        typedef Parametrizable::ParameterDoc ParameterDoc; 
        typedef Parametrizable::ParametersDoc ParametersDoc; 
        typedef Parametrizable::InvalidParameter InvalidParameter; 
        
        // ---------------------------------
        // input types
        // ---------------------------------
        

        struct DataPoints
        {
                typedef Eigen::Block<Matrix> View;
                typedef const Eigen::Block<const Matrix> ConstView;
                typedef typename Matrix::Index Index;
                
                struct Label
                {
                        std::string text; 
                        size_t span; 
                        Label(const std::string& text = "", const size_t span = 0);
                        bool operator ==(const Label& that) const;
                };
                struct Labels: std::vector<Label>
                {
                        typedef typename std::vector<Label>::const_iterator const_iterator; 
                        Labels();
                        Labels(const Label& label);
                        bool contains(const std::string& text) const;
                        size_t totalDim() const;
                };
                
                struct InvalidField: std::runtime_error
                {
                        InvalidField(const std::string& reason);
                };
                
                DataPoints();
                DataPoints(const Labels& featureLabels, const Labels& descriptorLabels, const size_t pointCount);
                DataPoints(const Matrix& features, const Labels& featureLabels);
                DataPoints(const Matrix& features, const Labels& featureLabels, const Matrix& descriptors, const Labels& descriptorLabels);
                bool operator ==(const DataPoints& that) const;
        
                unsigned getNbPoints() const;
                unsigned getEuclideanDim() const;
                unsigned getHomogeneousDim() const;
                unsigned getNbGroupedDescriptors() const;
                unsigned getDescriptorDim() const;

                void save(const std::string& fileName) const;
                static DataPoints load(const std::string& fileName);
                
                void concatenate(const DataPoints& dp);
                void conservativeResize(Index pointCount);
                DataPoints createSimilarEmpty() const;
                DataPoints createSimilarEmpty(Index pointCount) const;
                void setColFrom(Index thisCol, const DataPoints& that, Index thatCol);
                
                void allocateFeature(const std::string& name, const unsigned dim);
                void allocateFeatures(const Labels& newLabels);
                void addFeature(const std::string& name, const Matrix& newFeature);
                void removeFeature(const std::string& name);
                Matrix getFeatureCopyByName(const std::string& name) const;
                ConstView getFeatureViewByName(const std::string& name) const;
                View getFeatureViewByName(const std::string& name);
                ConstView getFeatureRowViewByName(const std::string& name, const unsigned row) const;
                View getFeatureRowViewByName(const std::string& name, const unsigned row);
                bool featureExists(const std::string& name) const;
                bool featureExists(const std::string& name, const unsigned dim) const;
                unsigned getFeatureDimension(const std::string& name) const;
                unsigned getFeatureStartingRow(const std::string& name) const;
                
                void allocateDescriptor(const std::string& name, const unsigned dim);
                void allocateDescriptors(const Labels& newLabels);
                void addDescriptor(const std::string& name, const Matrix& newDescriptor);
                void removeDescriptor(const std::string& name);
                Matrix getDescriptorCopyByName(const std::string& name) const;
                ConstView getDescriptorViewByName(const std::string& name) const;
                View getDescriptorViewByName(const std::string& name);
                ConstView getDescriptorRowViewByName(const std::string& name, const unsigned row) const;
                View getDescriptorRowViewByName(const std::string& name, const unsigned row);
                bool descriptorExists(const std::string& name) const;
                bool descriptorExists(const std::string& name, const unsigned dim) const;
                unsigned getDescriptorDimension(const std::string& name) const;
                unsigned getDescriptorStartingRow(const std::string& name) const;
                void assertDescriptorConsistency() const;
                
                Matrix features; 
                Labels featureLabels; 
                Matrix descriptors; 
                Labels descriptorLabels; 
        
        private:
                void allocateFields(const Labels& newLabels, Labels& labels, Matrix& data) const;
                void allocateField(const std::string& name, const unsigned dim, Labels& labels, Matrix& data) const;
                void addField(const std::string& name, const Matrix& newField, Labels& labels, Matrix& data) const;
                void removeField(const std::string& name, Labels& labels, Matrix& data) const;
                ConstView getConstViewByName(const std::string& name, const Labels& labels, const Matrix& data, const int viewRow = -1) const;
                View getViewByName(const std::string& name, const Labels& labels, Matrix& data, const int viewRow = -1) const;
                bool fieldExists(const std::string& name, const unsigned dim, const Labels& labels) const;
                unsigned getFieldDimension(const std::string& name, const Labels& labels) const;
                unsigned getFieldStartingRow(const std::string& name, const Labels& labels) const;
        };
        
        static void swapDataPoints(DataPoints& a, DataPoints& b);

        // ---------------------------------
        // intermediate types
        // ---------------------------------
        

        struct Matches
        {
                typedef Matrix Dists; 
                typedef IntMatrix Ids; 
        
                Matches();
                Matches(const Dists& dists, const Ids ids);
                Matches(const int knn, const int pointsCount);
                
                Dists dists; 
                Ids ids; 
                
                T getDistsQuantile(const T quantile) const;
        };


        typedef Matrix OutlierWeights;
        
        // ---------------------------------
        // types of processing bricks
        // ---------------------------------
        
        struct Transformation: public Parametrizable
        {
                Transformation();
                Transformation(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
                virtual ~Transformation();
                
                virtual DataPoints compute(const DataPoints& input, const TransformationParameters& parameters) const = 0; 

                virtual bool checkParameters(const TransformationParameters& parameters) const = 0;

                virtual TransformationParameters correctParameters(const TransformationParameters& parameters) const = 0;

        };
        
        struct Transformations: public PointMatcherSupport::SharedPtrVector<Transformation>
        {
                void apply(DataPoints& cloud, const TransformationParameters& parameters) const;
        };
        typedef typename Transformations::iterator TransformationsIt; 
        typedef typename Transformations::const_iterator TransformationsConstIt; 
        
        DEF_REGISTRAR(Transformation)
        
        // ---------------------------------
        
        

        struct DataPointsFilter: public Parametrizable
        {
                DataPointsFilter();
                DataPointsFilter(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
                virtual ~DataPointsFilter();
                virtual void init();

                virtual DataPoints filter(const DataPoints& input) = 0;

                virtual void inPlaceFilter(DataPoints& cloud) = 0;
        };
        
        struct DataPointsFilters: public PointMatcherSupport::SharedPtrVector<DataPointsFilter>
        {
                DataPointsFilters();
                DataPointsFilters(std::istream& in);
                void init();
                void apply(DataPoints& cloud);
        };
        typedef typename DataPointsFilters::iterator DataPointsFiltersIt; 
        typedef typename DataPointsFilters::const_iterator DataPointsFiltersConstIt; 
        
        DEF_REGISTRAR(DataPointsFilter)
        
        // ---------------------------------
        
        

        struct Matcher: public Parametrizable
        {
                unsigned long visitCounter; 
                
                Matcher();
                Matcher(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
                virtual ~Matcher();
                
                void resetVisitCount();
                unsigned long getVisitCount() const;
                
                virtual void init(const DataPoints& filteredReference) = 0;
                virtual Matches findClosests(const DataPoints& filteredReading) = 0;
        };
        
        DEF_REGISTRAR(Matcher)
        
        // ---------------------------------
        
        

        struct OutlierFilter: public Parametrizable
        {
                OutlierFilter();
                OutlierFilter(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
                
                virtual ~OutlierFilter();
                
                virtual OutlierWeights compute(const DataPoints& filteredReading, const DataPoints& filteredReference, const Matches& input) = 0;
        };
        
        
        struct OutlierFilters: public PointMatcherSupport::SharedPtrVector<OutlierFilter>
        {
                
                OutlierWeights compute(const DataPoints& filteredReading, const DataPoints& filteredReference, const Matches& input);
                
        };
        
        typedef typename OutlierFilters::const_iterator OutlierFiltersConstIt; 
        typedef typename OutlierFilters::iterator OutlierFiltersIt; 
        
        DEF_REGISTRAR(OutlierFilter)

        // ---------------------------------
        
        

        struct ErrorMinimizer: public Parametrizable
        {
                struct ErrorElements
                {
                        DataPoints reading; 
                        DataPoints reference; 
                        OutlierWeights weights; 
                        Matches matches; 

                        ErrorElements(const DataPoints& reading=DataPoints(), const DataPoints reference = DataPoints(), const OutlierWeights weights = OutlierWeights(), const Matches matches = Matches());
                };
                
                ErrorMinimizer();
                ErrorMinimizer(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
                virtual ~ErrorMinimizer();
                
                T getPointUsedRatio() const;
                T getWeightedPointUsedRatio() const;
                ErrorElements getErrorElements() const; //TODO: ensure that is return a usable value
                virtual T getOverlap() const;
                virtual Matrix getCovariance() const;
                
                virtual TransformationParameters compute(const DataPoints& filteredReading, const DataPoints& filteredReference, const OutlierWeights& outlierWeights, const Matches& matches) = 0;
                
                
        //protected:
                // helper functions
                static Matrix crossProduct(const Matrix& A, const Matrix& B);//TODO: this might go in pointmatcher_support namespace
                ErrorElements& getMatchedPoints(const DataPoints& reading, const DataPoints& reference, const Matches& matches, const OutlierWeights& outlierWeights);
                
        protected:
                T pointUsedRatio; 
                T weightedPointUsedRatio; 
                //TODO: standardize the use of this variable
                ErrorElements lastErrorElements; 
        };
        
        DEF_REGISTRAR(ErrorMinimizer)
        
        // ---------------------------------
        
        

        struct TransformationChecker: public Parametrizable
        {
        protected:
                typedef std::vector<std::string> StringVector; 
                Vector limits; 
                Vector conditionVariables; 
                StringVector limitNames; 
                StringVector conditionVariableNames; 

        public:
                TransformationChecker();
                TransformationChecker(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
                virtual ~TransformationChecker();
                virtual void init(const TransformationParameters& parameters, bool& iterate) = 0;
                virtual void check(const TransformationParameters& parameters, bool& iterate) = 0;
                
                const Vector& getLimits() const;
                const Vector& getConditionVariables() const;
                const StringVector& getLimitNames() const;
                const StringVector& getConditionVariableNames() const;
                
        protected:
                static Vector matrixToAngles(const TransformationParameters& parameters);
        };
        
        struct TransformationCheckers: public PointMatcherSupport::SharedPtrVector<TransformationChecker>
        {
                void init(const TransformationParameters& parameters, bool& iterate);
                void check(const TransformationParameters& parameters, bool& iterate);
        };
        typedef typename TransformationCheckers::iterator TransformationCheckersIt; 
        typedef typename TransformationCheckers::const_iterator TransformationCheckersConstIt; 
        
        DEF_REGISTRAR(TransformationChecker)

        // ---------------------------------
        
        
        struct Inspector: public Parametrizable
        {
                
                Inspector();
                Inspector(const std::string& className, const ParametersDoc paramsDoc, const Parameters& params);
                
                // 
                virtual ~Inspector();
                virtual void init();
                
                // performance statistics
                virtual void addStat(const std::string& name, double data);
                virtual void dumpStats(std::ostream& stream);
                virtual void dumpStatsHeader(std::ostream& stream);
                
                // data statistics 
                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);
                virtual void finish(const size_t iterationCount);
        };
        
        DEF_REGISTRAR(Inspector) 
        
        // ---------------------------------
        
        DEF_REGISTRAR_IFACE(Logger, PointMatcherSupport::Logger)

        // ---------------------------------
        
        // algorithms
        
        struct ICPChainBase
        {
        public:
                DataPointsFilters readingDataPointsFilters; 
                DataPointsFilters readingStepDataPointsFilters; 
                DataPointsFilters referenceDataPointsFilters; 
                Transformations transformations; 
                boost::shared_ptr<Matcher> matcher; 
                OutlierFilters outlierFilters; 
                boost::shared_ptr<ErrorMinimizer> errorMinimizer; 
                TransformationCheckers transformationCheckers; 
                boost::shared_ptr<Inspector> inspector; 
                
                virtual ~ICPChainBase();

                virtual void setDefault();
                
                void loadFromYaml(std::istream& in);
                unsigned getPrefilteredReadingPtsCount() const;
                unsigned getPrefilteredReferencePtsCount() const;
                
        protected:
                unsigned prefilteredReadingPtsCount; 
                unsigned prefilteredReferencePtsCount; 

                ICPChainBase();
                
                void cleanup();
                
        virtual void loadAdditionalYAMLContent(PointMatcherSupport::YAML::Node& doc);
                
                template<typename R>
        const std::string& createModulesFromRegistrar(const std::string& regName, const PointMatcherSupport::YAML::Node& doc, const R& registrar, PointMatcherSupport::SharedPtrVector<typename R::TargetType>& modules);
                
                template<typename R>
        const std::string& createModuleFromRegistrar(const std::string& regName, const PointMatcherSupport::YAML::Node& doc, const R& registrar, boost::shared_ptr<typename R::TargetType>& module);
                
                /*template<typename R>
                typename R::TargetType* createModuleFromRegistrar(const PointMatcherSupport::YAML::Node& module, const R& registrar);*/
        };
        
        struct ICP: ICPChainBase
        {
                TransformationParameters operator()(
                        const DataPoints& readingIn,
                        const DataPoints& referenceIn);

                TransformationParameters operator()(
                        const DataPoints& readingIn,
                        const DataPoints& referenceIn,
                        const TransformationParameters& initialTransformationParameters);
                
                TransformationParameters compute(
                        const DataPoints& readingIn,
                        const DataPoints& referenceIn,
                        const TransformationParameters& initialTransformationParameters);
                        const DataPoints& getReadingFiltered() const { return readingFiltered; }

        protected:
                TransformationParameters computeWithTransformedReference(
                        const DataPoints& readingIn, 
                        const DataPoints& reference, 
                        const TransformationParameters& T_refIn_refMean,
                        const TransformationParameters& initialTransformationParameters);

                DataPoints readingFiltered;
        };
        
        struct ICPSequence: public ICP
        {
                TransformationParameters operator()(
                        const DataPoints& cloudIn);
                TransformationParameters operator()(
                        const DataPoints& cloudIn,
                        const TransformationParameters& initialTransformationParameters);
                TransformationParameters compute(
                        const DataPoints& cloudIn,
                        const TransformationParameters& initialTransformationParameters);
                
                bool hasMap() const;
                bool setMap(const DataPoints& map);
                void clearMap();
                const DataPoints& getInternalMap() const;
                const DataPoints getMap() const;
                
        protected:
                DataPoints mapPointCloud; 
                TransformationParameters T_refIn_refMean; 
        };
        
        // ---------------------------------
        // Instance-related functions
        // ---------------------------------
        
        PointMatcher();
        
        static const PointMatcher& get();

        
}; // PointMatcher<T>

#endif // __POINTMATCHER_CORE_H