Point Cloud Library (PCL)  1.3.1
transformation_estimation_lm.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: transformation_estimation_lm.h 3041 2011-11-01 04:44:41Z rusu $
00037  *
00038  */
00039 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_H_
00040 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_H_
00041 
00042 #include <pcl/registration/transformation_estimation.h>
00043 #include <pcl/registration/warp_point_rigid.h>
00044 #include <pcl/registration/distances.h>
00045 
00046 namespace pcl
00047 {
00048   namespace registration
00049   {
00056     template <typename PointSource, typename PointTarget>
00057     class TransformationEstimationLM : public TransformationEstimation<PointSource, PointTarget>
00058     {
00059       typedef pcl::PointCloud<PointSource> PointCloudSource;
00060       typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
00061       typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
00062 
00063       typedef pcl::PointCloud<PointTarget> PointCloudTarget;
00064 
00065       typedef PointIndices::Ptr PointIndicesPtr;
00066       typedef PointIndices::ConstPtr PointIndicesConstPtr;
00067 
00068       public:
00069         TransformationEstimationLM () {};
00070         virtual ~TransformationEstimationLM () {};
00071 
00077         inline void
00078         estimateRigidTransformation (
00079             const pcl::PointCloud<PointSource> &cloud_src,
00080             const pcl::PointCloud<PointTarget> &cloud_tgt,
00081             Eigen::Matrix4f &transformation_matrix);
00082 
00089         inline void
00090         estimateRigidTransformation (
00091             const pcl::PointCloud<PointSource> &cloud_src,
00092             const std::vector<int> &indices_src,
00093             const pcl::PointCloud<PointTarget> &cloud_tgt,
00094             Eigen::Matrix4f &transformation_matrix);
00095 
00104         inline void
00105         estimateRigidTransformation (
00106             const pcl::PointCloud<PointSource> &cloud_src,
00107             const std::vector<int> &indices_src,
00108             const pcl::PointCloud<PointTarget> &cloud_tgt,
00109             const std::vector<int> &indices_tgt,
00110             Eigen::Matrix4f &transformation_matrix);
00111 
00118         inline void
00119         estimateRigidTransformation (
00120             const pcl::PointCloud<PointSource> &cloud_src,
00121             const pcl::PointCloud<PointTarget> &cloud_tgt,
00122             const pcl::Correspondences &correspondences,
00123             Eigen::Matrix4f &transformation_matrix);
00124 
00128         void
00129         setWarpFunction (const boost::shared_ptr<WarpPointRigid<PointSource, PointTarget> > &warp_fcn)
00130         {
00131           warp_point_ = warp_fcn;
00132         }
00133 
00134       protected:
00143         virtual double 
00144         computeDistance (const PointSource &p_src, const PointTarget &p_tgt)
00145         {
00146           Vector4fMapConst s = p_src.getVector4fMap ();
00147           Vector4fMapConst t = p_tgt.getVector4fMap ();
00148           return (pcl::distances::l2 (s, t));
00149         }
00150 
00152         std::vector<double> weights_;
00153 
00155         const PointCloudSource *tmp_src_;
00156 
00158         const PointCloudTarget  *tmp_tgt_;
00159 
00161         const std::vector<int> *tmp_idx_src_;
00162 
00164         const std::vector<int> *tmp_idx_tgt_;
00165 
00167         boost::shared_ptr<WarpPointRigid<PointSource, PointTarget> > warp_point_;
00168         
00170         template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic>
00171         struct Functor
00172         {
00173           typedef _Scalar Scalar;
00174           enum {
00175             InputsAtCompileTime = NX,
00176             ValuesAtCompileTime = NY
00177           };
00178           typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
00179           typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
00180           typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;
00181           
00182           const int m_inputs, m_values;
00183           
00184           Functor () : m_inputs (InputsAtCompileTime), m_values (ValuesAtCompileTime) {}
00185           Functor (int inputs, int values) : m_inputs (inputs), m_values (values) {}
00186           
00187           int inputs () const { return m_inputs; }
00188           int values () const { return m_values; }
00189         };
00190 
00191         struct OptimizationFunctor : Functor<double>
00192         {
00193           using Functor<double>::m_values;
00194 
00201           OptimizationFunctor (int n, int m, TransformationEstimationLM<PointSource, PointTarget> *estimator) : 
00202             Functor<double> (n,m), estimator_ (estimator) {}
00203 
00209           int operator () (const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const;
00210 
00211           TransformationEstimationLM<PointSource, PointTarget> *estimator_;
00212         };
00213 
00214         struct OptimizationFunctorWithIndices : Functor<double>
00215         {
00216           using Functor<double>::m_values;
00217 
00224           OptimizationFunctorWithIndices (int n, int m, TransformationEstimationLM *estimator) :
00225             Functor<double> (n,m), estimator_(estimator) {}
00226 
00232           int operator () (const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const;
00233 
00234           TransformationEstimationLM<PointSource, PointTarget> *estimator_;
00235         };
00236     };
00237   }
00238 }
00239 
00240 #include <pcl/registration/impl/transformation_estimation_lm.hpp>
00241 
00242 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_H_ */
00243 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines