Point Cloud Library (PCL)  1.3.1
transforms.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  *
00035  */
00036 
00038 template <typename PointT> void
00039 pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00040                           pcl::PointCloud<PointT> &cloud_out,
00041                           const Eigen::Affine3f &transform)
00042 {
00043   if (&cloud_in != &cloud_out)
00044   {
00045     // Note: could be replaced by cloud_out = cloud_in
00046     cloud_out.header   = cloud_in.header;
00047     cloud_out.is_dense = cloud_in.is_dense;
00048     cloud_out.width    = cloud_in.width;
00049     cloud_out.height   = cloud_in.height;
00050     cloud_out.points.reserve (cloud_out.points.size ());
00051     cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
00052   }
00053 
00054   if (cloud_in.is_dense)
00055   {
00056     // If the dataset is dense, simply transform it!
00057     for (size_t i = 0; i < cloud_out.points.size (); ++i)
00058       cloud_out.points[i].getVector3fMap () = transform * 
00059                                               cloud_in.points[i].getVector3fMap ();
00060   }
00061   else
00062   {
00063     // Dataset might contain NaNs and Infs, so check for them first,
00064     // otherwise we get errors during the multiplication (?)
00065     for (size_t i = 0; i < cloud_out.points.size (); ++i)
00066     {
00067       if (!pcl_isfinite (cloud_in.points[i].x) || 
00068           !pcl_isfinite (cloud_in.points[i].y) || 
00069           !pcl_isfinite (cloud_in.points[i].z))
00070         continue;
00071       cloud_out.points[i].getVector3fMap () = transform * 
00072                                               cloud_in.points[i].getVector3fMap ();
00073     }
00074   }
00075 }
00076 
00078 template <typename PointT> void
00079 pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00080                           const std::vector<int> &indices, 
00081                           pcl::PointCloud<PointT> &cloud_out,
00082                           const Eigen::Affine3f &transform)
00083 {
00084   size_t npts = indices.size ();
00085   // In order to transform the data, we need to remove NaNs
00086   cloud_out.is_dense = cloud_in.is_dense;
00087   cloud_out.header   = cloud_in.header;
00088   cloud_out.width    = npts;
00089   cloud_out.height   = 1;
00090   cloud_out.points.resize (npts);
00091 
00092   if (cloud_in.is_dense)
00093   {
00094     // If the dataset is dense, simply transform it!
00095     for (size_t i = 0; i < npts; ++i)
00096       cloud_out.points[i].getVector3fMap () = transform *
00097                                               cloud_in.points[indices[i]].getVector3fMap ();
00098   }
00099   else
00100   {
00101     // Dataset might contain NaNs and Infs, so check for them first,
00102     // otherwise we get errors during the multiplication (?)
00103     for (size_t i = 0; i < npts; ++i)
00104     {
00105       if (!pcl_isfinite (cloud_in.points[indices[i]].x) || 
00106           !pcl_isfinite (cloud_in.points[indices[i]].y) || 
00107           !pcl_isfinite (cloud_in.points[indices[i]].z))
00108         continue;
00109       cloud_out.points[i].getVector3fMap () = transform *
00110                                               cloud_in.points[indices[i]].getVector3fMap ();
00111     }
00112   }
00113 }
00114 
00116 template <typename PointT> void
00117 pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00118                                      pcl::PointCloud<PointT> &cloud_out,
00119                                      const Eigen::Affine3f &transform)
00120 {
00121   if (&cloud_in != &cloud_out)
00122   {
00123     // Note: could be replaced by cloud_out = cloud_in
00124     cloud_out.header   = cloud_in.header;
00125     cloud_out.width    = cloud_in.width;
00126     cloud_out.height   = cloud_in.height;
00127     cloud_out.is_dense = cloud_in.is_dense;
00128     cloud_out.points.reserve (cloud_out.points.size ());
00129     cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
00130   }
00131 
00132   // If the data is dense, we don't need to check for NaN
00133   if (cloud_in.is_dense)
00134   {
00135     for (size_t i = 0; i < cloud_out.points.size (); ++i)
00136     {
00137       cloud_out.points[i].getVector3fMap() = transform * 
00138                                              cloud_in.points[i].getVector3fMap ();
00139 
00140       // Rotate normals
00141       cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * 
00142                                                    cloud_in.points[i].getNormalVector3fMap ();
00143     }
00144   }
00145   // Dataset might contain NaNs and Infs, so check for them first.
00146   else
00147   {
00148     for (size_t i = 0; i < cloud_out.points.size (); ++i)
00149     {
00150       if (!pcl_isfinite (cloud_in.points[i].x) || 
00151           !pcl_isfinite (cloud_in.points[i].y) || 
00152           !pcl_isfinite (cloud_in.points[i].z))
00153         continue;
00154       cloud_out.points[i].getVector3fMap() = transform * 
00155                                              cloud_in.points[i].getVector3fMap ();
00156 
00157       // Rotate normals
00158       cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * 
00159                                                    cloud_in.points[i].getNormalVector3fMap ();
00160     }
00161   }
00162 }
00163 
00165 template <typename PointT> void
00166 pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00167                           pcl::PointCloud<PointT> &cloud_out,
00168                           const Eigen::Matrix4f &transform)
00169 {
00170   if (&cloud_in != &cloud_out)
00171   {
00172     // Note: could be replaced by cloud_out = cloud_in
00173     cloud_out.header   = cloud_in.header;
00174     cloud_out.width    = cloud_in.width;
00175     cloud_out.height   = cloud_in.height;
00176     cloud_out.is_dense = cloud_in.is_dense;
00177     cloud_out.points.reserve (cloud_out.points.size ());
00178     cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
00179   }
00180 
00181   Eigen::Matrix3f rot   = transform.block<3, 3> (0, 0);
00182   Eigen::Vector3f trans = transform.block<3, 1> (0, 3);
00183   // If the data is dense, we don't need to check for NaN
00184   if (cloud_in.is_dense)
00185   {
00186     for (size_t i = 0; i < cloud_out.points.size (); ++i)
00187       cloud_out.points[i].getVector3fMap () = rot * 
00188                                               cloud_in.points[i].getVector3fMap () + trans;
00189   }
00190   // Dataset might contain NaNs and Infs, so check for them first.
00191   else
00192   {
00193     for (size_t i = 0; i < cloud_out.points.size (); ++i)
00194     {
00195       if (!pcl_isfinite (cloud_in.points[i].x) || 
00196           !pcl_isfinite (cloud_in.points[i].y) || 
00197           !pcl_isfinite (cloud_in.points[i].z))
00198         continue;
00199       cloud_out.points[i].getVector3fMap () = rot * 
00200                                               cloud_in.points[i].getVector3fMap () + trans;
00201     }
00202   }
00203 }
00204 
00206 template <typename PointT> void
00207 pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00208                                      pcl::PointCloud<PointT> &cloud_out,
00209                                      const Eigen::Matrix4f &transform)
00210 {
00211   if (&cloud_in != &cloud_out)
00212   {
00213     // Note: could be replaced by cloud_out = cloud_in
00214     cloud_out.header   = cloud_in.header;
00215     cloud_out.width    = cloud_in.width;
00216     cloud_out.height   = cloud_in.height;
00217     cloud_out.is_dense = cloud_in.is_dense;
00218     cloud_out.points.reserve (cloud_out.points.size ());
00219     cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
00220   }
00221 
00222   Eigen::Matrix3f rot   = transform.block<3, 3> (0, 0);
00223   Eigen::Vector3f trans = transform.block<3, 1> (0, 3);
00224 
00225   // If the data is dense, we don't need to check for NaN
00226   if (cloud_in.is_dense)
00227   {
00228     for (size_t i = 0; i < cloud_out.points.size (); ++i)
00229     {
00230       cloud_out.points[i].getVector3fMap () = rot * 
00231                                               cloud_in.points[i].getVector3fMap () + trans;
00232 
00233       // Rotate normals
00234       cloud_out.points[i].getNormalVector3fMap() = rot * 
00235                                                    cloud_in.points[i].getNormalVector3fMap ();
00236     }
00237   }
00238   // Dataset might contain NaNs and Infs, so check for them first.
00239   else
00240   {
00241     for (size_t i = 0; i < cloud_out.points.size (); ++i)
00242     {
00243       if (!pcl_isfinite (cloud_in.points[i].x) || 
00244           !pcl_isfinite (cloud_in.points[i].y) || 
00245           !pcl_isfinite (cloud_in.points[i].z))
00246         continue;
00247       cloud_out.points[i].getVector3fMap () = rot * 
00248                                               cloud_in.points[i].getVector3fMap () + trans;
00249 
00250       // Rotate normals
00251       cloud_out.points[i].getNormalVector3fMap() = rot * 
00252                                                    cloud_in.points[i].getNormalVector3fMap ();
00253     }
00254   }
00255 }
00256 
00258 template <typename PointT> inline void
00259 pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00260                           pcl::PointCloud<PointT> &cloud_out,
00261                           const Eigen::Vector3f &offset, 
00262                           const Eigen::Quaternionf &rotation)
00263 {
00264   Eigen::Translation3f translation (offset);
00265   // Assemble an Eigen Transform
00266   Eigen::Affine3f t;
00267   t = translation * rotation;
00268   transformPointCloud (cloud_in, cloud_out, t);
00269 }
00270 
00272 template <typename PointT> inline void
00273 pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00274                                      pcl::PointCloud<PointT> &cloud_out,
00275                                      const Eigen::Vector3f &offset, 
00276                                      const Eigen::Quaternionf &rotation)
00277 {
00278   Eigen::Translation3f translation (offset);
00279   // Assemble an Eigen Transform
00280   Eigen::Affine3f t;
00281   t = translation * rotation;
00282   transformPointCloudWithNormals (cloud_in, cloud_out, t);
00283 }
00284 
00285 template <typename PointT> inline PointT
00286 pcl::transformPoint (const PointT &point, const Eigen::Affine3f &tranform)
00287 {
00288   PointT ret = point;
00289   ret.getVector3fMap () = tranform * point.getVector3fMap ();
00290   return ret;
00291 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines