transformation_estimation_wdf.h
Go to the documentation of this file.
00001 /*
00002  * TransformationEstimationWDF.h
00003  *
00004  *  Created on: Mar 15, 2012
00005  *      Author: darko490
00006  */
00007 
00008 #ifndef TRANSFORMATION_ESTIMATION_WDF_H
00009 #define TRANSFORMATION_ESTIMATION_WDF_H
00010 
00011 #include <pcl/registration/transformation_estimation.h>
00012 //#include <pcl/registration/transformation_estimation_lm.h>
00013 #include <pcl/registration/icp.h>
00014 #include "pcl/registration/warp_point_rigid.h"
00015 #include "pcl/registration/warp_point_rigid_6d.h"
00016 //#include "pcl/registration/distances.h"
00017 //#include <pcl/registration/distances.h>
00018 //#include "pcl/impl/point_types.hpp"
00019 #include <unsupported/Eigen/NonLinearOptimization>
00020 #include <iostream>
00021 
00022 template <typename PointSource, typename PointTarget>
00023 class TransformationEstimationWDF: public pcl::registration::TransformationEstimation<PointSource, PointTarget> {
00024         typedef pcl::PointCloud<PointSource> PointCloudSource;
00025         typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
00026         typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
00027 
00028         typedef pcl::PointCloud<PointTarget> PointCloudTarget;
00029 
00030         typedef pcl::PointIndices::Ptr PointIndicesPtr;
00031         typedef pcl::PointIndices::ConstPtr PointIndicesConstPtr;
00032 
00033         float alpha_; //<! weight given to distinct features term in optimization equation
00034         float beta_; //<! weight used in distance weighting of the points
00035         std::vector<int> indices_src_dfp_; 
00036         std::vector<int> indices_tgt_dfp_; 
00037         bool indices_src_dfp_set_; 
00038         bool indices_tgt_dfp_set_; 
00039         std::vector<float> weights_dfp_; 
00040         bool weights_dfp_set_; 
00041 
00047         inline void
00048         estimateRigidTransformation (
00049                     const pcl::PointCloud<PointSource> &cloud_src,
00050                     const pcl::PointCloud<PointTarget> &cloud_tgt,
00051                     Eigen::Matrix4f &transformation_matrix);
00052 
00059     inline void
00060     estimateRigidTransformation (
00061         const pcl::PointCloud<PointSource> &cloud_src,
00062         const pcl::PointCloud<PointTarget> &cloud_tgt,
00063         const pcl::Correspondences &correspondences,
00064         Eigen::Matrix4f &transformation_matrix);
00065 
00072     inline void
00073     estimateRigidTransformation (
00074         const pcl::PointCloud<PointSource> &cloud_src,
00075         const std::vector<int> &indices_src,
00076         const pcl::PointCloud<PointTarget> &cloud_tgt,
00077         Eigen::Matrix4f &transformation_matrix);
00078 public:
00079 
00080         TransformationEstimationWDF() {
00081                 // Set default alpha weight
00082                 alpha_ = 0.3;
00083                 indices_src_dfp_set_ = false;
00084                 indices_tgt_dfp_set_ = false;
00085                 weights_dfp_set_ = false;
00086         };
00087 
00088         virtual ~TransformationEstimationWDF() {};
00089 
00090         inline void
00091         setAlpha(float alpha_arg);
00092 
00093         inline float
00094         getAlpha(void);
00095 
00096         inline void
00097         setBeta(float alpha_arg);
00098 
00099         inline float
00100         getBeta(void);
00101 
00102 
00110         inline void
00111         setCorrespondecesDFP ( pcl::Correspondences correspondences_dfp );
00112 
00120         inline void
00121         setCorrespondecesDFP( std::vector<int> &indices_src_dfp_arg, std::vector<int> &indices_tgt_dfp_arg );
00122 
00123         inline void
00124         setWeightsDFP (std::vector<float> weights_dfp_arg);
00125 
00138         inline void
00139         estimateRigidTransformation (
00140                         const pcl::PointCloud<PointSource> &cloud_src,
00141                         const std::vector<int> &indices_src,
00142                         const std::vector<int> &indices_src_dfp,
00143                         const pcl::PointCloud<PointTarget> &cloud_tgt,
00144                         const std::vector<int> &indices_tgt,
00145                         const std::vector<int> &indices_tgt_dfp,
00146                         float alpha_arg,
00147                         Eigen::Matrix4f &transformation_matrix);
00148 
00158         inline void
00159         estimateRigidTransformation (
00160                         const pcl::PointCloud<PointSource> &cloud_src,
00161                         const pcl::PointCloud<PointTarget> &cloud_tgt,
00162                         const pcl::Correspondences &correspondences,
00163                         const pcl::Correspondences &correspondences_dfp,
00164                         float alpha_arg,
00165                         Eigen::Matrix4f &transformation_matrix);
00166 
00178     inline void
00179     estimateRigidTransformation (
00180         const pcl::PointCloud<PointSource> &cloud_src,
00181         const std::vector<int> &indices_src,
00182         const pcl::PointCloud<PointTarget> &cloud_tgt,
00183         const std::vector<int> &indices_tgt,
00184         Eigen::Matrix4f &transformation_matrix);
00185 
00189         void
00190         setWarpFunction (const boost::shared_ptr<pcl::WarpPointRigid<PointSource, PointTarget> > &warp_fcn)
00191         {
00192                 warp_point_ = warp_fcn;
00193         }
00194 
00195 protected:
00196         // Declare ICP class as friend
00197         friend class pcl::IterativeClosestPoint<PointSource, PointTarget>;
00198 
00207         virtual double
00208         computeDistance (const PointSource &p_src, const PointTarget &p_tgt)
00209         {
00210                 /*pcl::Vector4fMapConst*/ Eigen::Vector4f s = p_src.getVector4fMap ();
00211                 /*pcl::Vector4fMapConst*/ Eigen::Vector4f t = p_tgt.getVector4fMap ();
00212                 return ( (s - t).norm () );
00213         }
00214 
00215     virtual double
00216     computeDistancePointToPlane (const PointSource &p_src, const PointTarget &p_tgt)
00217     {
00218       // Compute the point-to-plane distance
00219       pcl::Vector4fMapConst s = p_src.getVector4fMap ();
00220       pcl::Vector4fMapConst t = p_tgt.getVector4fMap ();
00221       pcl::Vector4fMapConst n = p_tgt.getNormalVector4fMap ();
00222       return ((s - t).dot (n));
00223     }
00224 
00225         virtual inline double
00226         computeDistanceWeight ( const double &depth )
00227         {
00228 //              return (0.8*0.8)/(depth*depth);
00229                 int a = 0.8/depth;
00230                 if (a>1.07) return 0;   // if depth is less than 0.75 discard point
00231                         else return a;
00232         }
00233 
00235         std::vector<double> weights_;
00236 
00238         const PointCloudSource *tmp_src_;
00239 
00241         const PointCloudTarget  *tmp_tgt_;
00242 
00244         const std::vector<int> *tmp_idx_src_;
00245 
00247         const std::vector<int> *tmp_idx_tgt_;
00248 
00250         const std::vector<int> *tmp_idx_src_dfp_;
00251 
00253         const std::vector<int> *tmp_idx_tgt_dfp_;
00254 
00256         const std::vector<float> *tmp_weights_;
00257 
00259         const std::vector<float> *tmp_dfp_weights_;
00260 
00262         boost::shared_ptr<pcl::WarpPointRigid<PointSource, PointTarget> > warp_point_;
00263 
00265         template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic>
00266         struct Functor
00267         {
00268                 typedef _Scalar Scalar;
00269                 enum {
00270                         InputsAtCompileTime = NX,
00271                         ValuesAtCompileTime = NY
00272                 };
00273                 typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
00274                 typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
00275                 typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;
00276 
00277                 const int m_inputs, m_values;
00278 
00279                 Functor () : m_inputs (InputsAtCompileTime), m_values (ValuesAtCompileTime) {}
00280                 Functor (int inputs, int values, int np, int ndfp) : m_inputs (inputs), m_values (values), number_p(np), number_dfp(ndfp) {}
00281 
00282                 int number_p; 
00283                 int number_dfp; 
00284 
00285                 int inputs () const { return m_inputs; }
00286                 int values () const { return m_values; }
00287         };
00288 
00289         struct OptimizationFunctor : Functor<double>
00290         {
00291                 using Functor<double>::m_values;
00292                 using Functor<double>::number_p;
00293                 using Functor<double>::number_dfp;
00294 
00301                 OptimizationFunctor (int n, int m, int np, int ndfp, TransformationEstimationWDF<PointSource, PointTarget> *estimator) :
00302                         Functor<double> (n,m, np, ndfp), estimator_ (estimator) {}
00303 
00309                 int operator () (const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const;
00310 
00311                 TransformationEstimationWDF<PointSource, PointTarget> *estimator_;
00312         };
00313 
00314         struct OptimizationFunctorWithWeights : Functor<double>
00315         {
00316                 using Functor<double>::m_values;
00317                 using Functor<double>::number_p;
00318                 using Functor<double>::number_dfp;
00319 
00326                 OptimizationFunctorWithWeights (int n, int m, int np, int ndfp, TransformationEstimationWDF<PointSource, PointTarget> *estimator) :
00327                         Functor<double> (n,m, np, ndfp), estimator_ (estimator) {}
00328 
00334                 int operator () (const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const;
00335 
00336                 TransformationEstimationWDF<PointSource, PointTarget> *estimator_;
00337         };
00338 };
00339 
00340 #include "../../src/transformation_estimation_wdf.hpp"
00341 
00342 #endif /* TRANSFORMATION_ESTIMATION_WDF_H */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


rgbd_registration
Author(s): Ross Kidson
autogenerated on Thu May 23 2013 15:36:54