Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/registration/include/pcl/registration/transformation_estimation_2D.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2012-, Open Perception 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 the copyright holder(s) 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  *
00037  */
00038 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_H_
00039 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_H_
00040 
00041 #include <pcl/registration/transformation_estimation.h>
00042 
00043 namespace pcl
00044 {
00045   namespace registration
00046   {
00047     /** @b TransformationEstimation2D implements a simple 2D rigid transformation 
00048       * estimation (x, y, theta) for a given pair of datasets. 
00049       *
00050       * The two datasets should already be transformed so that the reference plane 
00051       * equals z = 0.
00052       *
00053       * \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.
00054       *
00055       * \author Suat Gedikli
00056       * \ingroup registration
00057       */
00058     template <typename PointSource, typename PointTarget, typename Scalar = float>
00059     class TransformationEstimation2D : public TransformationEstimation<PointSource, PointTarget, Scalar>
00060     {
00061       public:
00062         typedef boost::shared_ptr<TransformationEstimation2D<PointSource, PointTarget, Scalar> > Ptr;
00063         typedef boost::shared_ptr<const TransformationEstimation2D<PointSource, PointTarget, Scalar> > ConstPtr;
00064 
00065         typedef typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4 Matrix4;
00066 
00067         TransformationEstimation2D () {};
00068         virtual ~TransformationEstimation2D () {};
00069 
00070         /** \brief Estimate a rigid transformation between a source and a target point cloud in 2D.
00071           * \param[in] cloud_src the source point cloud dataset
00072           * \param[in] cloud_tgt the target point cloud dataset
00073           * \param[out] transformation_matrix the resultant transformation matrix
00074           */
00075         inline void
00076         estimateRigidTransformation (
00077             const pcl::PointCloud<PointSource> &cloud_src,
00078             const pcl::PointCloud<PointTarget> &cloud_tgt,
00079             Matrix4 &transformation_matrix) const;
00080 
00081         /** \brief Estimate a rigid transformation between a source and a target point cloud in 2D.
00082           * \param[in] cloud_src the source point cloud dataset
00083           * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
00084           * \param[in] cloud_tgt the target point cloud dataset
00085           * \param[out] transformation_matrix the resultant transformation matrix
00086           */
00087         inline void
00088         estimateRigidTransformation (
00089             const pcl::PointCloud<PointSource> &cloud_src,
00090             const std::vector<int> &indices_src,
00091             const pcl::PointCloud<PointTarget> &cloud_tgt,
00092             Matrix4 &transformation_matrix) const;
00093 
00094         /** \brief Estimate a rigid transformation between a source and a target point cloud in 2D.
00095           * \param[in] cloud_src the source point cloud dataset
00096           * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
00097           * \param[in] cloud_tgt the target point cloud dataset
00098           * \param[in] indices_tgt the vector of indices describing the correspondences of the interst points from \a indices_src
00099           * \param[out] transformation_matrix the resultant transformation matrix
00100           */
00101         virtual void
00102         estimateRigidTransformation (
00103             const pcl::PointCloud<PointSource> &cloud_src,
00104             const std::vector<int> &indices_src,
00105             const pcl::PointCloud<PointTarget> &cloud_tgt,
00106             const std::vector<int> &indices_tgt,
00107             Matrix4 &transformation_matrix) const;
00108 
00109         /** \brief Estimate a rigid transformation between a source and a target point cloud in 2D.
00110           * \param[in] cloud_src the source point cloud dataset
00111           * \param[in] cloud_tgt the target point cloud dataset
00112           * \param[in] correspondences the vector of correspondences between source and target point cloud
00113           * \param[out] transformation_matrix the resultant transformation matrix
00114           */
00115         virtual void
00116         estimateRigidTransformation (
00117             const pcl::PointCloud<PointSource> &cloud_src,
00118             const pcl::PointCloud<PointTarget> &cloud_tgt,
00119             const pcl::Correspondences &correspondences,
00120             Matrix4 &transformation_matrix) const;
00121 
00122       protected:
00123 
00124         /** \brief Estimate a rigid rotation transformation between a source and a target
00125           * \param[in] source_it an iterator over the source point cloud dataset
00126           * \param[in] target_it an iterator over the target point cloud dataset
00127           * \param[out] transformation_matrix the resultant transformation matrix
00128           */
00129         void
00130         estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it,
00131                                      ConstCloudIterator<PointTarget>& target_it,
00132                                      Matrix4 &transformation_matrix) const;
00133 
00134         /** \brief Obtain a 4x4 rigid transformation matrix from a correlation matrix H = src * tgt'
00135           * \param[in] cloud_src_demean the input source cloud, demeaned, in Eigen format
00136           * \param[in] centroid_src the input source centroid, in Eigen format
00137           * \param[in] cloud_tgt_demean the input target cloud, demeaned, in Eigen format
00138           * \param[in] centroid_tgt the input target cloud, in Eigen format
00139           * \param[out] transformation_matrix the resultant 4x4 rigid transformation matrix
00140           */ 
00141         void
00142         getTransformationFromCorrelation (
00143             const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
00144             const Eigen::Matrix<Scalar, 4, 1> &centroid_src,
00145             const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
00146             const Eigen::Matrix<Scalar, 4, 1> &centroid_tgt,
00147             Matrix4 &transformation_matrix) const;
00148     };
00149 
00150   }
00151 }
00152 
00153 #include <pcl/registration/impl/transformation_estimation_2D.hpp>
00154 
00155 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_H_ */