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

//TODO: investigate if that causes more problems down the road
#define EIGEN_NO_DEBUG

#include "Eigen/StdVector"
#include "Eigen/Core"
#include "Eigen/Geometry"


#include <boost/thread/mutex.hpp>

#include <stdexcept>
#include <limits>
#include <iostream>
#include <ostream>
#include <memory>
//#include <cstdint>
#include <boost/cstdint.hpp>

#include "DeprecationWarnings.h"
#include "Parametrizable.h"
#include "Registrar.h"
 
#define POINTMATCHER_VERSION "1.3.1"
#define POINTMATCHER_VERSION_INT 10301

namespace PointMatcherSupport
{
    // TODO: gather all exceptions

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

    struct TransformationError: std::runtime_error
    {
        TransformationError(const std::string& reason);
    };
    
    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(std::shared_ptr<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 typename Eigen::Matrix<std::int64_t, Eigen::Dynamic, Eigen::Dynamic> Int64Matrix;
    typedef typename Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> Array;



    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 Eigen::Block<Int64Matrix> TimeView;
        typedef const Eigen::Block<const Matrix> ConstView;
        typedef const Eigen::Block<const Int64Matrix> TimeConstView;
        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;
            friend std::ostream& operator<< (std::ostream& stream, const Labels& labels)
            {
                for(size_t i=0; i<labels.size(); i++)
                {
                    stream << labels[i].text;
                    if(i != (labels.size() -1))
                        stream << ", ";
                }

                return stream;
            };
        };
        
        struct InvalidField: std::runtime_error
        {
            InvalidField(const std::string& reason);
        };
        
        // Constructors from descriptions (reserve memory)
        DataPoints();
        DataPoints(const Labels& featureLabels, const Labels& descriptorLabels, const size_t pointCount);
        DataPoints(const Labels& featureLabels, const Labels& descriptorLabels, const Labels& timeLabels, const size_t pointCount);

        // Copy constructors from partial data
        DataPoints(const Matrix& features, const Labels& featureLabels);
        DataPoints(const Matrix& features, const Labels& featureLabels, const Matrix& descriptors, const Labels& descriptorLabels);
        DataPoints(const Matrix& features, const Labels& featureLabels, const Matrix& descriptors, const Labels& descriptorLabels, const Int64Matrix& times, const Labels& timeLabels);
        
        bool operator ==(const DataPoints& that) const;
    
        unsigned getNbPoints() const;
        unsigned getEuclideanDim() const;
        unsigned getHomogeneousDim() const;
        unsigned getNbGroupedDescriptors() const;
        unsigned getDescriptorDim() const;
        unsigned getTimeDim() const;

        void save(const std::string& fileName, bool binary = false) 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 swapCols(Index iCol,Index jCol);
        
        // methods related to features
        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;
        
        // methods related to descriptors
        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;
        
        // methods related to times
        void allocateTime(const std::string& name, const unsigned dim);
        void allocateTimes(const Labels& newLabels);
        void addTime(const std::string& name, const Int64Matrix& newTime);
        void removeTime(const std::string& name);
        Int64Matrix getTimeCopyByName(const std::string& name) const;
        TimeConstView getTimeViewByName(const std::string& name) const;
        TimeView getTimeViewByName(const std::string& name);
        TimeConstView getTimeRowViewByName(const std::string& name, const unsigned row) const;
        TimeView getTimeRowViewByName(const std::string& name, const unsigned row);
        bool timeExists(const std::string& name) const;
        bool timeExists(const std::string& name, const unsigned dim) const;
        unsigned getTimeDimension(const std::string& name) const;
        unsigned getTimeStartingRow(const std::string& name) const;
        void assertTimesConsistency() const;

        Matrix features; 
        Labels featureLabels; 
        Matrix descriptors; 
        Labels descriptorLabels; 
        Int64Matrix times; 
        Labels timeLabels; 
    
    private:
        void assertConsistency(const std::string& dataName, const int dataRows, const int dataCols, const Labels& labels) const;
        template<typename MatrixType> 
        void allocateFields(const Labels& newLabels, Labels& labels, MatrixType& data) const;
        template<typename MatrixType> 
        void allocateField(const std::string& name, const unsigned dim, Labels& labels, MatrixType& data) const;
        template<typename MatrixType> 
        void addField(const std::string& name, const MatrixType& newField, Labels& labels, MatrixType& data) const;
        template<typename MatrixType>
        void removeField(const std::string& name, Labels& labels, MatrixType& data) const;
        template<typename MatrixType>
        const Eigen::Block<const MatrixType> getConstViewByName(const std::string& name, const Labels& labels, const MatrixType& data, const int viewRow = -1) const;
        template<typename MatrixType>
        Eigen::Block<MatrixType> getViewByName(const std::string& name, const Labels& labels, MatrixType& 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;

        template<typename MatrixType>
        void concatenateLabelledMatrix(Labels* labels, MatrixType* data, const Labels extraLabels, const MatrixType extraData);
    };
    
    static void swapDataPoints(DataPoints& a, DataPoints& b);

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

    struct Matches
    {
        typedef Matrix Dists; 
        typedef IntMatrix Ids; 
    

        static constexpr int InvalidId = -1; 
        static constexpr T InvalidDist = std::numeric_limits<T>::infinity(); 

        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;
        T getMedianAbsDeviation() const;
        T getStandardDeviation() 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 std::vector<std::shared_ptr<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 std::vector<std::shared_ptr<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 std::vector<std::shared_ptr<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; 
            int nbRejectedMatches; 
            int nbRejectedPoints; 
            T pointUsedRatio;  
            T weightedPointUsedRatio;

            ErrorElements();
            ErrorElements(const DataPoints& requestedPts, const DataPoints& sourcePts, const OutlierWeights& outlierWeights, const 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 T getResidualError(const DataPoints& filteredReading, const DataPoints& filteredReference, const OutlierWeights& outlierWeights, const Matches& matches) const;
        
        virtual TransformationParameters compute(const DataPoints& filteredReading, const DataPoints& filteredReference, const OutlierWeights& outlierWeights, const Matches& matches);
        virtual TransformationParameters compute(const ErrorElements& matchedPoints) = 0;
        
        // helper functions
        static Matrix crossProduct(const Matrix& A, const Matrix& B);//TODO: this might go in pointmatcher_support namespace
        
    protected:
        //T pointUsedRatio; //!< the ratio of how many points were used for error minimization
        //T weightedPointUsedRatio; //!< the ratio of how many points were used (with weight) for error minimization
        //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 std::vector<std::shared_ptr<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; 
        std::shared_ptr<Matcher> matcher; 
        OutlierFilters outlierFilters; 
        std::shared_ptr<ErrorMinimizer> errorMinimizer; 
        TransformationCheckers transformationCheckers; 
        std::shared_ptr<Inspector> inspector; 
        
        virtual ~ICPChainBase();

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

        bool getMaxNumIterationsReached() const;
        
    protected:
        unsigned prefilteredReadingPtsCount; 
        unsigned prefilteredReferencePtsCount; 
        bool maxNumIterationsReached; 

        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, std::vector<std::shared_ptr<typename R::TargetType> >& modules);
        
        template<typename R>
        const std::string& createModuleFromRegistrar(const std::string& regName, const PointMatcherSupport::YAML::Node& doc, const R& registrar, std::shared_ptr<typename R::TargetType>& module);
        
        std::string nodeVal(const std::string& regName, const PointMatcherSupport::YAML::Node& doc);
    };
    
    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();
        PM_DEPRECATED("Use getPrefilteredInternalMap instead. "
                        "Function now always returns map with filter chain applied. "
                        "This may have altered your program behavior."
                      "Reasons for this stated here and in associated PR: "
                      "https://github.com/ethz-asl/libpointmatcher/issues/209.")
        const DataPoints& getInternalMap() const;
        const DataPoints& getPrefilteredInternalMap() const;
        PM_DEPRECATED("Use getPrefilteredMap instead. "
                                    "Function now always returns map with filter chain applied. "
                        "This may have altered your program behavior."
                        "Reasons for this stated here and in associated PR: "
                        "https://github.com/ethz-asl/libpointmatcher/issues/209")
        const DataPoints getMap() const;
        const DataPoints getPrefilteredMap() const;
        
    protected:
        DataPoints mapPointCloud; 
        TransformationParameters T_refIn_refMean; 
    };
    
    // ---------------------------------
    // Instance-related functions
    // ---------------------------------
    
    PointMatcher();
    
    static const PointMatcher& get();

    
}; // PointMatcher<T>

#endif // __POINTMATCHER_CORE_H