transformation_estimation_wdf.h

Go to the documentation of this file.

/*
 * TransformationEstimationWDF.h
 *
 *  Created on: Mar 15, 2012
 *      Author: darko490
 */

#ifndef TRANSFORMATION_ESTIMATION_WDF_H
#define TRANSFORMATION_ESTIMATION_WDF_H

#include <pcl/registration/transformation_estimation.h>
//#include <pcl/registration/transformation_estimation_lm.h>
#include <pcl/registration/icp.h>
#include "pcl/registration/warp_point_rigid.h"
#include "pcl/registration/warp_point_rigid_6d.h"
//#include "pcl/registration/distances.h"
//#include <pcl/registration/distances.h>
//#include "pcl/impl/point_types.hpp"
#include <unsupported/Eigen/NonLinearOptimization>
#include <iostream>

template <typename PointSource, typename PointTarget>
class TransformationEstimationWDF: public pcl::registration::TransformationEstimation<PointSource, PointTarget> {
        typedef pcl::PointCloud<PointSource> PointCloudSource;
        typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
        typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;

        typedef pcl::PointCloud<PointTarget> PointCloudTarget;

        typedef pcl::PointIndices::Ptr PointIndicesPtr;
        typedef pcl::PointIndices::ConstPtr PointIndicesConstPtr;

        float alpha_; //<! weight given to distinct features term in optimization equation
        float beta_; //<! weight used in distance weighting of the points
        std::vector<int> indices_src_dfp_; 
        std::vector<int> indices_tgt_dfp_; 
        bool indices_src_dfp_set_; 
        bool indices_tgt_dfp_set_; 
        std::vector<float> weights_dfp_; 
        bool weights_dfp_set_; 

        inline void
        estimateRigidTransformation (
                    const pcl::PointCloud<PointSource> &cloud_src,
                    const pcl::PointCloud<PointTarget> &cloud_tgt,
                    Eigen::Matrix4f &transformation_matrix);

    inline void
    estimateRigidTransformation (
        const pcl::PointCloud<PointSource> &cloud_src,
        const pcl::PointCloud<PointTarget> &cloud_tgt,
        const pcl::Correspondences &correspondences,
        Eigen::Matrix4f &transformation_matrix);

    inline void
    estimateRigidTransformation (
        const pcl::PointCloud<PointSource> &cloud_src,
        const std::vector<int> &indices_src,
        const pcl::PointCloud<PointTarget> &cloud_tgt,
        Eigen::Matrix4f &transformation_matrix);
public:

        TransformationEstimationWDF() {
                // Set default alpha weight
                alpha_ = 0.3;
                indices_src_dfp_set_ = false;
                indices_tgt_dfp_set_ = false;
                weights_dfp_set_ = false;
        };

        virtual ~TransformationEstimationWDF() {};

        inline void
        setAlpha(float alpha_arg);

        inline float
        getAlpha(void);

        inline void
        setBeta(float alpha_arg);

        inline float
        getBeta(void);


        inline void
        setCorrespondecesDFP ( pcl::Correspondences correspondences_dfp );

        inline void
        setCorrespondecesDFP( std::vector<int> &indices_src_dfp_arg, std::vector<int> &indices_tgt_dfp_arg );

        inline void
        setWeightsDFP (std::vector<float> weights_dfp_arg);

        inline void
        estimateRigidTransformation (
                        const pcl::PointCloud<PointSource> &cloud_src,
                        const std::vector<int> &indices_src,
                        const std::vector<int> &indices_src_dfp,
                        const pcl::PointCloud<PointTarget> &cloud_tgt,
                        const std::vector<int> &indices_tgt,
                        const std::vector<int> &indices_tgt_dfp,
                        float alpha_arg,
                        Eigen::Matrix4f &transformation_matrix);

        inline void
        estimateRigidTransformation (
                        const pcl::PointCloud<PointSource> &cloud_src,
                        const pcl::PointCloud<PointTarget> &cloud_tgt,
                        const pcl::Correspondences &correspondences,
                        const pcl::Correspondences &correspondences_dfp,
                        float alpha_arg,
                        Eigen::Matrix4f &transformation_matrix);

    inline void
    estimateRigidTransformation (
        const pcl::PointCloud<PointSource> &cloud_src,
        const std::vector<int> &indices_src,
        const pcl::PointCloud<PointTarget> &cloud_tgt,
        const std::vector<int> &indices_tgt,
        Eigen::Matrix4f &transformation_matrix);

        void
        setWarpFunction (const boost::shared_ptr<pcl::WarpPointRigid<PointSource, PointTarget> > &warp_fcn)
        {
                warp_point_ = warp_fcn;
        }

protected:
        // Declare ICP class as friend
        friend class pcl::IterativeClosestPoint<PointSource, PointTarget>;

        virtual double
        computeDistance (const PointSource &p_src, const PointTarget &p_tgt)
        {
                /*pcl::Vector4fMapConst*/ Eigen::Vector4f s = p_src.getVector4fMap ();
                /*pcl::Vector4fMapConst*/ Eigen::Vector4f t = p_tgt.getVector4fMap ();
                return ( (s - t).norm () );
        }

    virtual double
    computeDistancePointToPlane (const PointSource &p_src, const PointTarget &p_tgt)
    {
      // Compute the point-to-plane distance
      pcl::Vector4fMapConst s = p_src.getVector4fMap ();
      pcl::Vector4fMapConst t = p_tgt.getVector4fMap ();
      pcl::Vector4fMapConst n = p_tgt.getNormalVector4fMap ();
      return ((s - t).dot (n));
    }

        virtual inline double
        computeDistanceWeight ( const double &depth )
        {
//              return (0.8*0.8)/(depth*depth);
                int a = 0.8/depth;
                if (a>1.07) return 0;   // if depth is less than 0.75 discard point
                        else return a;
        }

        std::vector<double> weights_;

        const PointCloudSource *tmp_src_;

        const PointCloudTarget  *tmp_tgt_;

        const std::vector<int> *tmp_idx_src_;

        const std::vector<int> *tmp_idx_tgt_;

        const std::vector<int> *tmp_idx_src_dfp_;

        const std::vector<int> *tmp_idx_tgt_dfp_;

        const std::vector<float> *tmp_weights_;

        const std::vector<float> *tmp_dfp_weights_;

        boost::shared_ptr<pcl::WarpPointRigid<PointSource, PointTarget> > warp_point_;

        template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic>
        struct Functor
        {
                typedef _Scalar Scalar;
                enum {
                        InputsAtCompileTime = NX,
                        ValuesAtCompileTime = NY
                };
                typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
                typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
                typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;

                const int m_inputs, m_values;

                Functor () : m_inputs (InputsAtCompileTime), m_values (ValuesAtCompileTime) {}
                Functor (int inputs, int values, int np, int ndfp) : m_inputs (inputs), m_values (values), number_p(np), number_dfp(ndfp) {}

                int number_p; 
                int number_dfp; 

                int inputs () const { return m_inputs; }
                int values () const { return m_values; }
        };

        struct OptimizationFunctor : Functor<double>
        {
                using Functor<double>::m_values;
                using Functor<double>::number_p;
                using Functor<double>::number_dfp;

                OptimizationFunctor (int n, int m, int np, int ndfp, TransformationEstimationWDF<PointSource, PointTarget> *estimator) :
                        Functor<double> (n,m, np, ndfp), estimator_ (estimator) {}

                int operator () (const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const;

                TransformationEstimationWDF<PointSource, PointTarget> *estimator_;
        };

        struct OptimizationFunctorWithWeights : Functor<double>
        {
                using Functor<double>::m_values;
                using Functor<double>::number_p;
                using Functor<double>::number_dfp;

                OptimizationFunctorWithWeights (int n, int m, int np, int ndfp, TransformationEstimationWDF<PointSource, PointTarget> *estimator) :
                        Functor<double> (n,m, np, ndfp), estimator_ (estimator) {}

                int operator () (const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const;

                TransformationEstimationWDF<PointSource, PointTarget> *estimator_;
        };
};

#include "../../src/transformation_estimation_wdf.hpp"

#endif /* TRANSFORMATION_ESTIMATION_WDF_H */