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     {
00097       // Copy fields first, then transform xyz data
00098       cloud_out.points[i] = cloud_in.points[indices[i]]; 
00099       cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
00100     }
00101   }
00102   else
00103   {
00104     // Dataset might contain NaNs and Infs, so check for them first,
00105     // otherwise we get errors during the multiplication (?)
00106     for (size_t i = 0; i < npts; ++i)
00107     {
00108       if (!pcl_isfinite (cloud_in.points[indices[i]].x) || 
00109           !pcl_isfinite (cloud_in.points[indices[i]].y) || 
00110           !pcl_isfinite (cloud_in.points[indices[i]].z))
00111         continue;
00112       cloud_out.points[i] = cloud_in.points[indices[i]]; 
00113       cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
00114     }
00115   }
00116 }
00117 
00119 template <typename PointT> void
00120 pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00121                                      pcl::PointCloud<PointT> &cloud_out,
00122                                      const Eigen::Affine3f &transform)
00123 {
00124   if (&cloud_in != &cloud_out)
00125   {
00126     // Note: could be replaced by cloud_out = cloud_in
00127     cloud_out.header   = cloud_in.header;
00128     cloud_out.width    = cloud_in.width;
00129     cloud_out.height   = cloud_in.height;
00130     cloud_out.is_dense = cloud_in.is_dense;
00131     cloud_out.points.reserve (cloud_out.points.size ());
00132     cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
00133   }
00134 
00135   // If the data is dense, we don't need to check for NaN
00136   if (cloud_in.is_dense)
00137   {
00138     for (size_t i = 0; i < cloud_out.points.size (); ++i)
00139     {
00140       cloud_out.points[i].getVector3fMap() = transform * 
00141                                              cloud_in.points[i].getVector3fMap ();
00142 
00143       // Rotate normals
00144       cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * 
00145                                                    cloud_in.points[i].getNormalVector3fMap ();
00146     }
00147   }
00148   // Dataset might contain NaNs and Infs, so check for them first.
00149   else
00150   {
00151     for (size_t i = 0; i < cloud_out.points.size (); ++i)
00152     {
00153       if (!pcl_isfinite (cloud_in.points[i].x) || 
00154           !pcl_isfinite (cloud_in.points[i].y) || 
00155           !pcl_isfinite (cloud_in.points[i].z))
00156         continue;
00157       cloud_out.points[i].getVector3fMap() = transform * 
00158                                              cloud_in.points[i].getVector3fMap ();
00159 
00160       // Rotate normals
00161       cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * 
00162                                                    cloud_in.points[i].getNormalVector3fMap ();
00163     }
00164   }
00165 }
00166 
00168 template <typename PointT> void
00169 pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00170                           pcl::PointCloud<PointT> &cloud_out,
00171                           const Eigen::Matrix4f &transform)
00172 {
00173   if (&cloud_in != &cloud_out)
00174   {
00175     // Note: could be replaced by cloud_out = cloud_in
00176     cloud_out.header   = cloud_in.header;
00177     cloud_out.width    = cloud_in.width;
00178     cloud_out.height   = cloud_in.height;
00179     cloud_out.is_dense = cloud_in.is_dense;
00180     cloud_out.points.reserve (cloud_out.points.size ());
00181     cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
00182   }
00183 
00184   Eigen::Matrix3f rot   = transform.block<3, 3> (0, 0);
00185   Eigen::Vector3f trans = transform.block<3, 1> (0, 3);
00186   // If the data is dense, we don't need to check for NaN
00187   if (cloud_in.is_dense)
00188   {
00189     for (size_t i = 0; i < cloud_out.points.size (); ++i)
00190       cloud_out.points[i].getVector3fMap () = rot * 
00191                                               cloud_in.points[i].getVector3fMap () + trans;
00192   }
00193   // Dataset might contain NaNs and Infs, so check for them first.
00194   else
00195   {
00196     for (size_t i = 0; i < cloud_out.points.size (); ++i)
00197     {
00198       if (!pcl_isfinite (cloud_in.points[i].x) || 
00199           !pcl_isfinite (cloud_in.points[i].y) || 
00200           !pcl_isfinite (cloud_in.points[i].z))
00201         continue;
00202       cloud_out.points[i].getVector3fMap () = rot * 
00203                                               cloud_in.points[i].getVector3fMap () + trans;
00204     }
00205   }
00206 }
00207 
00209 template <typename PointT> void
00210 pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00211                                      pcl::PointCloud<PointT> &cloud_out,
00212                                      const Eigen::Matrix4f &transform)
00213 {
00214   if (&cloud_in != &cloud_out)
00215   {
00216     // Note: could be replaced by cloud_out = cloud_in
00217     cloud_out.header   = cloud_in.header;
00218     cloud_out.width    = cloud_in.width;
00219     cloud_out.height   = cloud_in.height;
00220     cloud_out.is_dense = cloud_in.is_dense;
00221     cloud_out.points.reserve (cloud_out.points.size ());
00222     cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
00223   }
00224 
00225   Eigen::Matrix3f rot   = transform.block<3, 3> (0, 0);
00226   Eigen::Vector3f trans = transform.block<3, 1> (0, 3);
00227 
00228   // If the data is dense, we don't need to check for NaN
00229   if (cloud_in.is_dense)
00230   {
00231     for (size_t i = 0; i < cloud_out.points.size (); ++i)
00232     {
00233       cloud_out.points[i].getVector3fMap () = rot * 
00234                                               cloud_in.points[i].getVector3fMap () + trans;
00235 
00236       // Rotate normals
00237       cloud_out.points[i].getNormalVector3fMap() = rot * 
00238                                                    cloud_in.points[i].getNormalVector3fMap ();
00239     }
00240   }
00241   // Dataset might contain NaNs and Infs, so check for them first.
00242   else
00243   {
00244     for (size_t i = 0; i < cloud_out.points.size (); ++i)
00245     {
00246       if (!pcl_isfinite (cloud_in.points[i].x) || 
00247           !pcl_isfinite (cloud_in.points[i].y) || 
00248           !pcl_isfinite (cloud_in.points[i].z))
00249         continue;
00250       cloud_out.points[i].getVector3fMap () = rot * 
00251                                               cloud_in.points[i].getVector3fMap () + trans;
00252 
00253       // Rotate normals
00254       cloud_out.points[i].getNormalVector3fMap() = rot * 
00255                                                    cloud_in.points[i].getNormalVector3fMap ();
00256     }
00257   }
00258 }
00259 
00261 template <typename PointT> inline void
00262 pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00263                           pcl::PointCloud<PointT> &cloud_out,
00264                           const Eigen::Vector3f &offset, 
00265                           const Eigen::Quaternionf &rotation)
00266 {
00267   Eigen::Translation3f translation (offset);
00268   // Assemble an Eigen Transform
00269   Eigen::Affine3f t;
00270   t = translation * rotation;
00271   transformPointCloud (cloud_in, cloud_out, t);
00272 }
00273 
00275 template <typename PointT> inline void
00276 pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00277                                      pcl::PointCloud<PointT> &cloud_out,
00278                                      const Eigen::Vector3f &offset, 
00279                                      const Eigen::Quaternionf &rotation)
00280 {
00281   Eigen::Translation3f translation (offset);
00282   // Assemble an Eigen Transform
00283   Eigen::Affine3f t;
00284   t = translation * rotation;
00285   transformPointCloudWithNormals (cloud_in, cloud_out, t);
00286 }
00287 
00289 template <typename PointT> inline PointT
00290 pcl::transformPoint (const PointT &point, const Eigen::Affine3f &transform)
00291 {
00292   PointT ret = point;
00293   ret.getVector3fMap () = transform * point.getVector3fMap ();
00294   return ret;
00295 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:18:53