Point Cloud Library (PCL)
1.7.0
|
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 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 * 00039 */ 00040 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_H_ 00041 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_H_ 00042 00043 #include <pcl/registration/transformation_estimation.h> 00044 #include <pcl/registration/warp_point_rigid.h> 00045 #include <pcl/registration/distances.h> 00046 00047 namespace pcl 00048 { 00049 namespace registration 00050 { 00051 /** @b TransformationEstimationLM implements Levenberg Marquardt-based 00052 * estimation of the transformation aligning the given correspondences. 00053 * 00054 * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float. 00055 * \author Radu B. Rusu 00056 * \ingroup registration 00057 */ 00058 template <typename PointSource, typename PointTarget, typename MatScalar = float> 00059 class TransformationEstimationLM : public TransformationEstimation<PointSource, PointTarget, MatScalar> 00060 { 00061 typedef pcl::PointCloud<PointSource> PointCloudSource; 00062 typedef typename PointCloudSource::Ptr PointCloudSourcePtr; 00063 typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; 00064 00065 typedef pcl::PointCloud<PointTarget> PointCloudTarget; 00066 00067 typedef PointIndices::Ptr PointIndicesPtr; 00068 typedef PointIndices::ConstPtr PointIndicesConstPtr; 00069 00070 public: 00071 typedef boost::shared_ptr<TransformationEstimationLM<PointSource, PointTarget, MatScalar> > Ptr; 00072 typedef boost::shared_ptr<const TransformationEstimationLM<PointSource, PointTarget, MatScalar> > ConstPtr; 00073 00074 typedef Eigen::Matrix<MatScalar, Eigen::Dynamic, 1> VectorX; 00075 typedef Eigen::Matrix<MatScalar, 4, 1> Vector4; 00076 typedef typename TransformationEstimation<PointSource, PointTarget, MatScalar>::Matrix4 Matrix4; 00077 00078 /** \brief Constructor. */ 00079 TransformationEstimationLM (); 00080 00081 /** \brief Copy constructor. 00082 * \param[in] src the TransformationEstimationLM object to copy into this 00083 */ 00084 TransformationEstimationLM (const TransformationEstimationLM &src) : 00085 tmp_src_ (src.tmp_src_), 00086 tmp_tgt_ (src.tmp_tgt_), 00087 tmp_idx_src_ (src.tmp_idx_src_), 00088 tmp_idx_tgt_ (src.tmp_idx_tgt_), 00089 warp_point_ (src.warp_point_) 00090 {}; 00091 00092 /** \brief Copy operator. 00093 * \param[in] src the TransformationEstimationLM object to copy into this 00094 */ 00095 TransformationEstimationLM& 00096 operator = (const TransformationEstimationLM &src) 00097 { 00098 tmp_src_ = src.tmp_src_; 00099 tmp_tgt_ = src.tmp_tgt_; 00100 tmp_idx_src_ = src.tmp_idx_src_; 00101 tmp_idx_tgt_ = src.tmp_idx_tgt_; 00102 warp_point_ = src.warp_point_; 00103 } 00104 00105 /** \brief Destructor. */ 00106 virtual ~TransformationEstimationLM () {}; 00107 00108 /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM. 00109 * \param[in] cloud_src the source point cloud dataset 00110 * \param[in] cloud_tgt the target point cloud dataset 00111 * \param[out] transformation_matrix the resultant transformation matrix 00112 */ 00113 inline void 00114 estimateRigidTransformation ( 00115 const pcl::PointCloud<PointSource> &cloud_src, 00116 const pcl::PointCloud<PointTarget> &cloud_tgt, 00117 Matrix4 &transformation_matrix) const; 00118 00119 /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM. 00120 * \param[in] cloud_src the source point cloud dataset 00121 * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src 00122 * \param[in] cloud_tgt the target point cloud dataset 00123 * \param[out] transformation_matrix the resultant transformation matrix 00124 */ 00125 inline void 00126 estimateRigidTransformation ( 00127 const pcl::PointCloud<PointSource> &cloud_src, 00128 const std::vector<int> &indices_src, 00129 const pcl::PointCloud<PointTarget> &cloud_tgt, 00130 Matrix4 &transformation_matrix) const; 00131 00132 /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM. 00133 * \param[in] cloud_src the source point cloud dataset 00134 * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src 00135 * \param[in] cloud_tgt the target point cloud dataset 00136 * \param[in] indices_tgt the vector of indices describing the correspondences of the interst points from 00137 * \a indices_src 00138 * \param[out] transformation_matrix the resultant transformation matrix 00139 */ 00140 inline void 00141 estimateRigidTransformation ( 00142 const pcl::PointCloud<PointSource> &cloud_src, 00143 const std::vector<int> &indices_src, 00144 const pcl::PointCloud<PointTarget> &cloud_tgt, 00145 const std::vector<int> &indices_tgt, 00146 Matrix4 &transformation_matrix) const; 00147 00148 /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM. 00149 * \param[in] cloud_src the source point cloud dataset 00150 * \param[in] cloud_tgt the target point cloud dataset 00151 * \param[in] correspondences the vector of correspondences between source and target point cloud 00152 * \param[out] transformation_matrix the resultant transformation matrix 00153 */ 00154 inline void 00155 estimateRigidTransformation ( 00156 const pcl::PointCloud<PointSource> &cloud_src, 00157 const pcl::PointCloud<PointTarget> &cloud_tgt, 00158 const pcl::Correspondences &correspondences, 00159 Matrix4 &transformation_matrix) const; 00160 00161 /** \brief Set the function we use to warp points. Defaults to rigid 6D warp. 00162 * \param[in] warp_fcn a shared pointer to an object that warps points 00163 */ 00164 void 00165 setWarpFunction (const boost::shared_ptr<WarpPointRigid<PointSource, PointTarget, MatScalar> > &warp_fcn) 00166 { 00167 warp_point_ = warp_fcn; 00168 } 00169 00170 protected: 00171 /** \brief Compute the distance between a source point and its corresponding target point 00172 * \param[in] p_src The source point 00173 * \param[in] p_tgt The target point 00174 * \return The distance between \a p_src and \a p_tgt 00175 * 00176 * \note Older versions of PCL used this method internally for calculating the 00177 * optimization gradient. Since PCL 1.7, a switch has been made to the 00178 * computeDistance method using Vector4 types instead. This method is only 00179 * kept for API compatibility reasons. 00180 */ 00181 virtual MatScalar 00182 computeDistance (const PointSource &p_src, const PointTarget &p_tgt) const 00183 { 00184 Vector4 s (p_src.x, p_src.y, p_src.z, 0); 00185 Vector4 t (p_tgt.x, p_tgt.y, p_tgt.z, 0); 00186 return ((s - t).norm ()); 00187 } 00188 00189 /** \brief Compute the distance between a source point and its corresponding target point 00190 * \param[in] p_src The source point 00191 * \param[in] p_tgt The target point 00192 * \return The distance between \a p_src and \a p_tgt 00193 * 00194 * \note A different distance function can be defined by creating a subclass of 00195 * TransformationEstimationLM and overriding this method. 00196 * (See \a TransformationEstimationPointToPlane) 00197 */ 00198 virtual MatScalar 00199 computeDistance (const Vector4 &p_src, const PointTarget &p_tgt) const 00200 { 00201 Vector4 t (p_tgt.x, p_tgt.y, p_tgt.z, 0); 00202 return ((p_src - t).norm ()); 00203 } 00204 00205 /** \brief Temporary pointer to the source dataset. */ 00206 mutable const PointCloudSource *tmp_src_; 00207 00208 /** \brief Temporary pointer to the target dataset. */ 00209 mutable const PointCloudTarget *tmp_tgt_; 00210 00211 /** \brief Temporary pointer to the source dataset indices. */ 00212 mutable const std::vector<int> *tmp_idx_src_; 00213 00214 /** \brief Temporary pointer to the target dataset indices. */ 00215 mutable const std::vector<int> *tmp_idx_tgt_; 00216 00217 /** \brief The parameterized function used to warp the source to the target. */ 00218 boost::shared_ptr<pcl::registration::WarpPointRigid<PointSource, PointTarget, MatScalar> > warp_point_; 00219 00220 /** Base functor all the models that need non linear optimization must 00221 * define their own one and implement operator() (const Eigen::VectorXd& x, Eigen::VectorXd& fvec) 00222 * or operator() (const Eigen::VectorXf& x, Eigen::VectorXf& fvec) dependening on the choosen _Scalar 00223 */ 00224 template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic> 00225 struct Functor 00226 { 00227 typedef _Scalar Scalar; 00228 enum 00229 { 00230 InputsAtCompileTime = NX, 00231 ValuesAtCompileTime = NY 00232 }; 00233 typedef Eigen::Matrix<_Scalar,InputsAtCompileTime,1> InputType; 00234 typedef Eigen::Matrix<_Scalar,ValuesAtCompileTime,1> ValueType; 00235 typedef Eigen::Matrix<_Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType; 00236 00237 /** \brief Empty Construtor. */ 00238 Functor () : m_data_points_ (ValuesAtCompileTime) {} 00239 00240 /** \brief Constructor 00241 * \param[in] m_data_points number of data points to evaluate. 00242 */ 00243 Functor (int m_data_points) : m_data_points_ (m_data_points) {} 00244 00245 /** \brief Destructor. */ 00246 virtual ~Functor () {} 00247 00248 /** \brief Get the number of values. */ 00249 int 00250 values () const { return (m_data_points_); } 00251 00252 protected: 00253 int m_data_points_; 00254 }; 00255 00256 struct OptimizationFunctor : public Functor<MatScalar> 00257 { 00258 using Functor<MatScalar>::values; 00259 00260 /** Functor constructor 00261 * \param[in] m_data_points the number of data points to evaluate 00262 * \param[in,out] estimator pointer to the estimator object 00263 */ 00264 OptimizationFunctor (int m_data_points, 00265 const TransformationEstimationLM *estimator) 00266 : Functor<MatScalar> (m_data_points), estimator_ (estimator) 00267 {} 00268 00269 /** Copy constructor 00270 * \param[in] the optimization functor to copy into this 00271 */ 00272 inline OptimizationFunctor (const OptimizationFunctor &src) : 00273 Functor<MatScalar> (src.m_data_points_), estimator_ () 00274 { 00275 *this = src; 00276 } 00277 00278 /** Copy operator 00279 * \param[in] the optimization functor to copy into this 00280 */ 00281 inline OptimizationFunctor& 00282 operator = (const OptimizationFunctor &src) 00283 { 00284 Functor<MatScalar>::operator=(src); 00285 estimator_ = src.estimator_; 00286 return (*this); 00287 } 00288 00289 /** \brief Destructor. */ 00290 virtual ~OptimizationFunctor () {} 00291 00292 /** Fill fvec from x. For the current state vector x fill the f values 00293 * \param[in] x state vector 00294 * \param[out] fvec f values vector 00295 */ 00296 int 00297 operator () (const VectorX &x, VectorX &fvec) const; 00298 00299 const TransformationEstimationLM<PointSource, PointTarget, MatScalar> *estimator_; 00300 }; 00301 00302 struct OptimizationFunctorWithIndices : public Functor<MatScalar> 00303 { 00304 using Functor<MatScalar>::values; 00305 00306 /** Functor constructor 00307 * \param[in] m_data_points the number of data points to evaluate 00308 * \param[in,out] estimator pointer to the estimator object 00309 */ 00310 OptimizationFunctorWithIndices (int m_data_points, 00311 const TransformationEstimationLM *estimator) 00312 : Functor<MatScalar> (m_data_points), estimator_ (estimator) 00313 {} 00314 00315 /** Copy constructor 00316 * \param[in] the optimization functor to copy into this 00317 */ 00318 inline OptimizationFunctorWithIndices (const OptimizationFunctorWithIndices &src) 00319 : Functor<MatScalar> (src.m_data_points_), estimator_ () 00320 { 00321 *this = src; 00322 } 00323 00324 /** Copy operator 00325 * \param[in] the optimization functor to copy into this 00326 */ 00327 inline OptimizationFunctorWithIndices& 00328 operator = (const OptimizationFunctorWithIndices &src) 00329 { 00330 Functor<MatScalar>::operator=(src); 00331 estimator_ = src.estimator_; 00332 return (*this); 00333 } 00334 00335 /** \brief Destructor. */ 00336 virtual ~OptimizationFunctorWithIndices () {} 00337 00338 /** Fill fvec from x. For the current state vector x fill the f values 00339 * \param[in] x state vector 00340 * \param[out] fvec f values vector 00341 */ 00342 int 00343 operator () (const VectorX &x, VectorX &fvec) const; 00344 00345 const TransformationEstimationLM<PointSource, PointTarget, MatScalar> *estimator_; 00346 }; 00347 public: 00348 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00349 }; 00350 } 00351 } 00352 00353 #include <pcl/registration/impl/transformation_estimation_lm.hpp> 00354 00355 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_H_ */ 00356