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) 2009-2012, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * Copyright (c) Alexandru-Eugen Ichim 00008 * 00009 * All rights reserved. 00010 * 00011 * Redistribution and use in source and binary forms, with or without 00012 * modification, are permitted provided that the following conditions 00013 * are met: 00014 * 00015 * * Redistributions of source code must retain the above copyright 00016 * notice, this list of conditions and the following disclaimer. 00017 * * Redistributions in binary form must reproduce the above 00018 * copyright notice, this list of conditions and the following 00019 * disclaimer in the documentation and/or other materials provided 00020 * with the distribution. 00021 * * Neither the name of the copyright holder(s) nor the names of its 00022 * contributors may be used to endorse or promote products derived 00023 * from this software without specific prior written permission. 00024 * 00025 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00026 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00027 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00028 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00029 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00030 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00031 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00032 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00033 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00034 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00035 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00036 * POSSIBILITY OF SUCH DAMAGE. 00037 */ 00038 00039 00040 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_H_ 00041 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_H_ 00042 00043 #include <pcl/registration/transformation_estimation_point_to_plane.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 TransformationEstimationPointToPlaneWeighted uses Levenberg Marquardt optimization to find the transformation 00052 * that minimizes the point-to-plane distance between the given correspondences. In addition to the 00053 * TransformationEstimationPointToPlane class, this version takes per-correspondence weights and optimizes accordingly. 00054 * 00055 * \author Alexandru-Eugen Ichim 00056 * \ingroup registration 00057 */ 00058 template <typename PointSource, typename PointTarget, typename MatScalar = float> 00059 class TransformationEstimationPointToPlaneWeighted : public TransformationEstimationPointToPlane<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<TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar> > Ptr; 00072 typedef boost::shared_ptr<const TransformationEstimationPointToPlaneWeighted<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 TransformationEstimationPointToPlaneWeighted (); 00080 00081 /** \brief Copy constructor. 00082 * \param[in] src the TransformationEstimationPointToPlaneWeighted object to copy into this 00083 */ 00084 TransformationEstimationPointToPlaneWeighted (const TransformationEstimationPointToPlaneWeighted &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 correspondence_weights_ (src.correspondence_weights_), 00091 use_correspondence_weights_ (src.use_correspondence_weights_) 00092 {}; 00093 00094 /** \brief Copy operator. 00095 * \param[in] src the TransformationEstimationPointToPlaneWeighted object to copy into this 00096 */ 00097 TransformationEstimationPointToPlaneWeighted& 00098 operator = (const TransformationEstimationPointToPlaneWeighted &src) 00099 { 00100 tmp_src_ = src.tmp_src_; 00101 tmp_tgt_ = src.tmp_tgt_; 00102 tmp_idx_src_ = src.tmp_idx_src_; 00103 tmp_idx_tgt_ = src.tmp_idx_tgt_; 00104 warp_point_ = src.warp_point_; 00105 correspondence_weights_ = src.correspondence_weights_; 00106 use_correspondence_weights_ = src.use_correspondence_weights_; 00107 } 00108 00109 /** \brief Destructor. */ 00110 virtual ~TransformationEstimationPointToPlaneWeighted () {}; 00111 00112 /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM. 00113 * \param[in] cloud_src the source point cloud dataset 00114 * \param[in] cloud_tgt the target point cloud dataset 00115 * \param[out] transformation_matrix the resultant transformation matrix 00116 * \note Uses the weights given by setWeights. 00117 */ 00118 inline void 00119 estimateRigidTransformation ( 00120 const pcl::PointCloud<PointSource> &cloud_src, 00121 const pcl::PointCloud<PointTarget> &cloud_tgt, 00122 Matrix4 &transformation_matrix) const; 00123 00124 /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM. 00125 * \param[in] cloud_src the source point cloud dataset 00126 * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src 00127 * \param[in] cloud_tgt the target point cloud dataset 00128 * \param[out] transformation_matrix the resultant transformation matrix 00129 * \note Uses the weights given by setWeights. 00130 */ 00131 inline void 00132 estimateRigidTransformation ( 00133 const pcl::PointCloud<PointSource> &cloud_src, 00134 const std::vector<int> &indices_src, 00135 const pcl::PointCloud<PointTarget> &cloud_tgt, 00136 Matrix4 &transformation_matrix) const; 00137 00138 /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM. 00139 * \param[in] cloud_src the source point cloud dataset 00140 * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src 00141 * \param[in] cloud_tgt the target point cloud dataset 00142 * \param[in] indices_tgt the vector of indices describing the correspondences of the interst points from 00143 * \a indices_src 00144 * \param[out] transformation_matrix the resultant transformation matrix 00145 * \note Uses the weights given by setWeights. 00146 */ 00147 void 00148 estimateRigidTransformation ( 00149 const pcl::PointCloud<PointSource> &cloud_src, 00150 const std::vector<int> &indices_src, 00151 const pcl::PointCloud<PointTarget> &cloud_tgt, 00152 const std::vector<int> &indices_tgt, 00153 Matrix4 &transformation_matrix) const; 00154 00155 /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM. 00156 * \param[in] cloud_src the source point cloud dataset 00157 * \param[in] cloud_tgt the target point cloud dataset 00158 * \param[in] correspondences the vector of correspondences between source and target point cloud 00159 * \param[out] transformation_matrix the resultant transformation matrix 00160 * \note Uses the weights given by setWeights. 00161 */ 00162 void 00163 estimateRigidTransformation ( 00164 const pcl::PointCloud<PointSource> &cloud_src, 00165 const pcl::PointCloud<PointTarget> &cloud_tgt, 00166 const pcl::Correspondences &correspondences, 00167 Matrix4 &transformation_matrix) const; 00168 00169 00170 inline void 00171 setWeights (const std::vector<double> &weights) 00172 { correspondence_weights_ = weights; } 00173 00174 /// use the weights given in the pcl::CorrespondencesPtr for one of the estimateTransformation (...) methods 00175 inline void 00176 setUseCorrespondenceWeights (bool use_correspondence_weights) 00177 { use_correspondence_weights_ = use_correspondence_weights; } 00178 00179 /** \brief Set the function we use to warp points. Defaults to rigid 6D warp. 00180 * \param[in] warp_fcn a shared pointer to an object that warps points 00181 */ 00182 void 00183 setWarpFunction (const boost::shared_ptr<WarpPointRigid<PointSource, PointTarget, MatScalar> > &warp_fcn) 00184 { warp_point_ = warp_fcn; } 00185 00186 protected: 00187 bool use_correspondence_weights_; 00188 mutable std::vector<double> correspondence_weights_; 00189 00190 /** \brief Temporary pointer to the source dataset. */ 00191 mutable const PointCloudSource *tmp_src_; 00192 00193 /** \brief Temporary pointer to the target dataset. */ 00194 mutable const PointCloudTarget *tmp_tgt_; 00195 00196 /** \brief Temporary pointer to the source dataset indices. */ 00197 mutable const std::vector<int> *tmp_idx_src_; 00198 00199 /** \brief Temporary pointer to the target dataset indices. */ 00200 mutable const std::vector<int> *tmp_idx_tgt_; 00201 00202 /** \brief The parameterized function used to warp the source to the target. */ 00203 boost::shared_ptr<pcl::registration::WarpPointRigid<PointSource, PointTarget, MatScalar> > warp_point_; 00204 00205 /** Base functor all the models that need non linear optimization must 00206 * define their own one and implement operator() (const Eigen::VectorXd& x, Eigen::VectorXd& fvec) 00207 * or operator() (const Eigen::VectorXf& x, Eigen::VectorXf& fvec) dependening on the choosen _Scalar 00208 */ 00209 template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic> 00210 struct Functor 00211 { 00212 typedef _Scalar Scalar; 00213 enum 00214 { 00215 InputsAtCompileTime = NX, 00216 ValuesAtCompileTime = NY 00217 }; 00218 typedef Eigen::Matrix<_Scalar,InputsAtCompileTime,1> InputType; 00219 typedef Eigen::Matrix<_Scalar,ValuesAtCompileTime,1> ValueType; 00220 typedef Eigen::Matrix<_Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType; 00221 00222 /** \brief Empty Construtor. */ 00223 Functor () : m_data_points_ (ValuesAtCompileTime) {} 00224 00225 /** \brief Constructor 00226 * \param[in] m_data_points number of data points to evaluate. 00227 */ 00228 Functor (int m_data_points) : m_data_points_ (m_data_points) {} 00229 00230 /** \brief Destructor. */ 00231 virtual ~Functor () {} 00232 00233 /** \brief Get the number of values. */ 00234 int 00235 values () const { return (m_data_points_); } 00236 00237 protected: 00238 int m_data_points_; 00239 }; 00240 00241 struct OptimizationFunctor : public Functor<MatScalar> 00242 { 00243 using Functor<MatScalar>::values; 00244 00245 /** Functor constructor 00246 * \param[in] m_data_points the number of data points to evaluate 00247 * \param[in,out] estimator pointer to the estimator object 00248 */ 00249 OptimizationFunctor (int m_data_points, 00250 const TransformationEstimationPointToPlaneWeighted *estimator) 00251 : Functor<MatScalar> (m_data_points), estimator_ (estimator) 00252 {} 00253 00254 /** Copy constructor 00255 * \param[in] the optimization functor to copy into this 00256 */ 00257 inline OptimizationFunctor (const OptimizationFunctor &src) : 00258 Functor<MatScalar> (src.m_data_points_), estimator_ () 00259 { 00260 *this = src; 00261 } 00262 00263 /** Copy operator 00264 * \param[in] the optimization functor to copy into this 00265 */ 00266 inline OptimizationFunctor& 00267 operator = (const OptimizationFunctor &src) 00268 { 00269 Functor<MatScalar>::operator=(src); 00270 estimator_ = src.estimator_; 00271 return (*this); 00272 } 00273 00274 /** \brief Destructor. */ 00275 virtual ~OptimizationFunctor () {} 00276 00277 /** Fill fvec from x. For the current state vector x fill the f values 00278 * \param[in] x state vector 00279 * \param[out] fvec f values vector 00280 */ 00281 int 00282 operator () (const VectorX &x, VectorX &fvec) const; 00283 00284 const TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar> *estimator_; 00285 }; 00286 00287 struct OptimizationFunctorWithIndices : public Functor<MatScalar> 00288 { 00289 using Functor<MatScalar>::values; 00290 00291 /** Functor constructor 00292 * \param[in] m_data_points the number of data points to evaluate 00293 * \param[in,out] estimator pointer to the estimator object 00294 */ 00295 OptimizationFunctorWithIndices (int m_data_points, 00296 const TransformationEstimationPointToPlaneWeighted *estimator) 00297 : Functor<MatScalar> (m_data_points), estimator_ (estimator) 00298 {} 00299 00300 /** Copy constructor 00301 * \param[in] the optimization functor to copy into this 00302 */ 00303 inline OptimizationFunctorWithIndices (const OptimizationFunctorWithIndices &src) 00304 : Functor<MatScalar> (src.m_data_points_), estimator_ () 00305 { 00306 *this = src; 00307 } 00308 00309 /** Copy operator 00310 * \param[in] the optimization functor to copy into this 00311 */ 00312 inline OptimizationFunctorWithIndices& 00313 operator = (const OptimizationFunctorWithIndices &src) 00314 { 00315 Functor<MatScalar>::operator=(src); 00316 estimator_ = src.estimator_; 00317 return (*this); 00318 } 00319 00320 /** \brief Destructor. */ 00321 virtual ~OptimizationFunctorWithIndices () {} 00322 00323 /** Fill fvec from x. For the current state vector x fill the f values 00324 * \param[in] x state vector 00325 * \param[out] fvec f values vector 00326 */ 00327 int 00328 operator () (const VectorX &x, VectorX &fvec) const; 00329 00330 const TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar> *estimator_; 00331 }; 00332 public: 00333 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00334 }; 00335 } 00336 } 00337 00338 #include <pcl/registration/impl/transformation_estimation_point_to_plane_weighted.hpp> 00339 00340 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_H_ */ 00341