Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/common/include/pcl/common/transforms.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010, 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  *
00038  */
00039 #ifndef PCL_TRANSFORMS_H_
00040 #define PCL_TRANSFORMS_H_
00041 
00042 #include <pcl/point_cloud.h>
00043 #include <pcl/point_types.h>
00044 #include <pcl/common/centroid.h>
00045 #include <pcl/common/eigen.h>
00046 #include <pcl/PointIndices.h>
00047 
00048 namespace pcl
00049 {
00050   /** \brief Apply an affine transform defined by an Eigen Transform
00051     * \param[in] cloud_in the input point cloud
00052     * \param[out] cloud_out the resultant output point cloud
00053     * \param[in] transform an affine transformation (typically a rigid transformation)
00054     * \note Can be used with cloud_in equal to cloud_out
00055     * \ingroup common
00056     */
00057   template <typename PointT, typename Scalar> void 
00058   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00059                        pcl::PointCloud<PointT> &cloud_out, 
00060                        const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform);
00061 
00062   template <typename PointT> void 
00063   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00064                        pcl::PointCloud<PointT> &cloud_out, 
00065                        const Eigen::Affine3f &transform)
00066   {
00067     return (transformPointCloud<PointT, float> (cloud_in, cloud_out, transform));
00068   }
00069 
00070   /** \brief Apply an affine transform defined by an Eigen Transform
00071     * \param[in] cloud_in the input point cloud
00072     * \param[in] indices the set of point indices to use from the input point cloud
00073     * \param[out] cloud_out the resultant output point cloud
00074     * \param[in] transform an affine transformation (typically a rigid transformation)
00075     * \ingroup common
00076     */
00077   template <typename PointT, typename Scalar> void 
00078   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00079                        const std::vector<int> &indices, 
00080                        pcl::PointCloud<PointT> &cloud_out, 
00081                        const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform);
00082 
00083   template <typename PointT> void 
00084   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00085                        const std::vector<int> &indices, 
00086                        pcl::PointCloud<PointT> &cloud_out, 
00087                        const Eigen::Affine3f &transform)
00088   {
00089     return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform));
00090   }
00091 
00092   /** \brief Apply an affine transform defined by an Eigen Transform
00093     * \param[in] cloud_in the input point cloud
00094     * \param[in] indices the set of point indices to use from the input point cloud
00095     * \param[out] cloud_out the resultant output point cloud
00096     * \param[in] transform an affine transformation (typically a rigid transformation)
00097     * \ingroup common
00098     */
00099   template <typename PointT, typename Scalar> void 
00100   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00101                        const pcl::PointIndices &indices, 
00102                        pcl::PointCloud<PointT> &cloud_out, 
00103                        const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
00104   {
00105     return (transformPointCloud<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform));
00106   }
00107 
00108   template <typename PointT> void 
00109   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00110                        const pcl::PointIndices &indices, 
00111                        pcl::PointCloud<PointT> &cloud_out, 
00112                        const Eigen::Affine3f &transform)
00113   {
00114     return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform));
00115   }
00116 
00117   /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
00118     * \param[in] cloud_in the input point cloud
00119     * \param[out] cloud_out the resultant output point cloud
00120     * \param[in] transform an affine transformation (typically a rigid transformation)
00121     * \note Can be used with cloud_in equal to cloud_out
00122     */
00123   template <typename PointT, typename Scalar> void 
00124   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00125                                   pcl::PointCloud<PointT> &cloud_out, 
00126                                   const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform);
00127 
00128   template <typename PointT> void 
00129   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00130                                   pcl::PointCloud<PointT> &cloud_out, 
00131                                   const Eigen::Affine3f &transform)
00132   {
00133     return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, transform));
00134   }
00135 
00136   /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
00137     * \param[in] cloud_in the input point cloud
00138     * \param[in] indices the set of point indices to use from the input point cloud
00139     * \param[out] cloud_out the resultant output point cloud
00140     * \param[in] transform an affine transformation (typically a rigid transformation)
00141     */
00142   template <typename PointT, typename Scalar> void 
00143   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00144                                   const std::vector<int> &indices, 
00145                                   pcl::PointCloud<PointT> &cloud_out, 
00146                                   const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform);
00147 
00148   template <typename PointT> void 
00149   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00150                                   const std::vector<int> &indices, 
00151                                   pcl::PointCloud<PointT> &cloud_out, 
00152                                   const Eigen::Affine3f &transform)
00153   {
00154     return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform));
00155   }
00156 
00157   /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
00158     * \param[in] cloud_in the input point cloud
00159     * \param[in] indices the set of point indices to use from the input point cloud
00160     * \param[out] cloud_out the resultant output point cloud
00161     * \param[in] transform an affine transformation (typically a rigid transformation)
00162     */
00163   template <typename PointT, typename Scalar> void 
00164   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00165                                   const pcl::PointIndices &indices, 
00166                                   pcl::PointCloud<PointT> &cloud_out, 
00167                                   const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
00168   {
00169     return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform));
00170   }
00171 
00172 
00173   template <typename PointT> void 
00174   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00175                                   const pcl::PointIndices &indices, 
00176                                   pcl::PointCloud<PointT> &cloud_out, 
00177                                   const Eigen::Affine3f &transform)
00178   {
00179     return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform));
00180   }
00181 
00182   /** \brief Apply a rigid transform defined by a 4x4 matrix
00183     * \param[in] cloud_in the input point cloud
00184     * \param[out] cloud_out the resultant output point cloud
00185     * \param[in] transform a rigid transformation 
00186     * \note Can be used with cloud_in equal to cloud_out
00187     * \ingroup common
00188     */
00189   template <typename PointT, typename Scalar> void 
00190   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00191                        pcl::PointCloud<PointT> &cloud_out, 
00192                        const Eigen::Matrix<Scalar, 4, 4> &transform)
00193   {
00194     Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
00195     return (transformPointCloud<PointT, Scalar> (cloud_in, cloud_out, t));
00196   }
00197 
00198   template <typename PointT> void 
00199   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00200                        pcl::PointCloud<PointT> &cloud_out, 
00201                        const Eigen::Matrix4f &transform)
00202   {
00203     return (transformPointCloud<PointT, float> (cloud_in, cloud_out, transform));
00204   }
00205 
00206   /** \brief Apply a rigid transform defined by a 4x4 matrix
00207     * \param[in] cloud_in the input point cloud
00208     * \param[in] indices the set of point indices to use from the input point cloud
00209     * \param[out] cloud_out the resultant output point cloud
00210     * \param[in] transform a rigid transformation 
00211     * \ingroup common
00212     */
00213   template <typename PointT, typename Scalar> void 
00214   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00215                        const std::vector<int> &indices, 
00216                        pcl::PointCloud<PointT> &cloud_out, 
00217                        const Eigen::Matrix<Scalar, 4, 4> &transform)
00218   {
00219     Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
00220     return (transformPointCloud<PointT, Scalar> (cloud_in, indices, cloud_out, t));
00221   }
00222 
00223   template <typename PointT> void 
00224   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00225                        const std::vector<int> &indices, 
00226                        pcl::PointCloud<PointT> &cloud_out, 
00227                        const Eigen::Matrix4f &transform)
00228   {
00229     return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform));
00230   }
00231 
00232   /** \brief Apply a rigid transform defined by a 4x4 matrix
00233     * \param[in] cloud_in the input point cloud
00234     * \param[in] indices the set of point indices to use from the input point cloud
00235     * \param[out] cloud_out the resultant output point cloud
00236     * \param[in] transform a rigid transformation 
00237     * \ingroup common
00238     */
00239   template <typename PointT, typename Scalar> void 
00240   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00241                        const pcl::PointIndices &indices, 
00242                        pcl::PointCloud<PointT> &cloud_out, 
00243                        const Eigen::Matrix<Scalar, 4, 4> &transform)
00244   {
00245     return (transformPointCloud<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform));
00246   }
00247 
00248   template <typename PointT> void 
00249   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00250                        const pcl::PointIndices &indices, 
00251                        pcl::PointCloud<PointT> &cloud_out, 
00252                        const Eigen::Matrix4f &transform)
00253   {
00254     return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform));
00255   }
00256 
00257   /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
00258     * \param[in] cloud_in the input point cloud
00259     * \param[out] cloud_out the resultant output point cloud
00260     * \param[in] transform an affine transformation (typically a rigid transformation)
00261     * \note Can be used with cloud_in equal to cloud_out
00262     * \ingroup common
00263     */
00264   template <typename PointT, typename Scalar> void 
00265   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00266                                   pcl::PointCloud<PointT> &cloud_out, 
00267                                   const Eigen::Matrix<Scalar, 4, 4> &transform)
00268   {
00269     Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
00270     return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, cloud_out, t));
00271   }
00272 
00273 
00274   template <typename PointT> void 
00275   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00276                                   pcl::PointCloud<PointT> &cloud_out, 
00277                                   const Eigen::Matrix4f &transform)
00278   {
00279     return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, transform));
00280   }
00281 
00282   /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
00283     * \param[in] cloud_in the input point cloud
00284     * \param[in] indices the set of point indices to use from the input point cloud
00285     * \param[out] cloud_out the resultant output point cloud
00286     * \param[in] transform an affine transformation (typically a rigid transformation)
00287     * \note Can be used with cloud_in equal to cloud_out
00288     * \ingroup common
00289     */
00290   template <typename PointT, typename Scalar> void 
00291   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00292                                   const std::vector<int> &indices, 
00293                                   pcl::PointCloud<PointT> &cloud_out, 
00294                                   const Eigen::Matrix<Scalar, 4, 4> &transform)
00295   {
00296     Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
00297     return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices, cloud_out, t));
00298   }
00299 
00300 
00301   template <typename PointT> void 
00302   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00303                                   const std::vector<int> &indices, 
00304                                   pcl::PointCloud<PointT> &cloud_out, 
00305                                   const Eigen::Matrix4f &transform)
00306   {
00307     return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform));
00308   }
00309 
00310   /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
00311     * \param[in] cloud_in the input point cloud
00312     * \param[in] indices the set of point indices to use from the input point cloud
00313     * \param[out] cloud_out the resultant output point cloud
00314     * \param[in] transform an affine transformation (typically a rigid transformation)
00315     * \note Can be used with cloud_in equal to cloud_out
00316     * \ingroup common
00317     */
00318   template <typename PointT, typename Scalar> void 
00319   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00320                                   const pcl::PointIndices &indices, 
00321                                   pcl::PointCloud<PointT> &cloud_out, 
00322                                   const Eigen::Matrix<Scalar, 4, 4> &transform)
00323   {
00324     Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform);
00325     return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices, cloud_out, t));
00326   }
00327 
00328 
00329   template <typename PointT> void 
00330   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00331                                   const pcl::PointIndices &indices, 
00332                                   pcl::PointCloud<PointT> &cloud_out, 
00333                                   const Eigen::Matrix4f &transform)
00334   {
00335     return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform));
00336   }
00337 
00338   /** \brief Apply a rigid transform defined by a 3D offset and a quaternion
00339     * \param[in] cloud_in the input point cloud
00340     * \param[out] cloud_out the resultant output point cloud
00341     * \param[in] offset the translation component of the rigid transformation
00342     * \param[in] rotation the rotation component of the rigid transformation
00343     * \ingroup common
00344     */
00345   template <typename PointT, typename Scalar> inline void 
00346   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00347                        pcl::PointCloud<PointT> &cloud_out, 
00348                        const Eigen::Matrix<Scalar, 3, 1> &offset, 
00349                        const Eigen::Quaternion<Scalar> &rotation);
00350 
00351   template <typename PointT> inline void 
00352   transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00353                        pcl::PointCloud<PointT> &cloud_out, 
00354                        const Eigen::Vector3f &offset, 
00355                        const Eigen::Quaternionf &rotation)
00356   {
00357     return (transformPointCloud<PointT, float> (cloud_in, cloud_out, offset, rotation));
00358   }
00359 
00360   /** \brief Transform a point cloud and rotate its normals using an Eigen transform.
00361     * \param[in] cloud_in the input point cloud
00362     * \param[out] cloud_out the resultant output point cloud
00363     * \param[in] offset the translation component of the rigid transformation
00364     * \param[in] rotation the rotation component of the rigid transformation
00365     * \ingroup common
00366     */
00367   template <typename PointT, typename Scalar> inline void 
00368   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00369                                   pcl::PointCloud<PointT> &cloud_out, 
00370                                   const Eigen::Matrix<Scalar, 3, 1> &offset, 
00371                                   const Eigen::Quaternion<Scalar> &rotation);
00372 
00373   template <typename PointT> void 
00374   transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00375                                   pcl::PointCloud<PointT> &cloud_out, 
00376                                   const Eigen::Vector3f &offset, 
00377                                   const Eigen::Quaternionf &rotation)
00378   {
00379     return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, offset, rotation));
00380   }
00381 
00382   /** \brief Transform a point with members x,y,z
00383     * \param[in] point the point to transform
00384     * \param[out] transform the transformation to apply
00385     * \return the transformed point
00386     * \ingroup common
00387     */
00388   template <typename PointT, typename Scalar> inline PointT
00389   transformPoint (const PointT &point, 
00390                   const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform);
00391   
00392   template <typename PointT> inline PointT
00393   transformPoint (const PointT &point, 
00394                   const Eigen::Affine3f &transform)
00395   {
00396     return (transformPoint<PointT, float> (point, transform));
00397   }
00398 
00399   /** \brief Calculates the principal (PCA-based) alignment of the point cloud
00400     * \param[in] cloud the input point cloud
00401     * \param[out] transform the resultant transform
00402     * \return the ratio lambda1/lambda2 or lambda2/lambda3, whatever is closer to 1.
00403     * \note If the return value is close to one then the transformation might be not unique -> two principal directions have
00404     * almost same variance (extend)
00405     */
00406   template <typename PointT, typename Scalar> inline double
00407   getPrincipalTransformation (const pcl::PointCloud<PointT> &cloud, 
00408                               Eigen::Transform<Scalar, 3, Eigen::Affine> &transform);
00409 
00410   template <typename PointT> inline double
00411   getPrincipalTransformation (const pcl::PointCloud<PointT> &cloud, 
00412                               Eigen::Affine3f &transform)
00413   {
00414     return (getPrincipalTransformation<PointT, float> (cloud, transform));
00415   }
00416 }
00417 
00418 #include <pcl/common/impl/transforms.hpp>
00419 
00420 #endif // PCL_TRANSFORMS_H_