transforms.hpp
Go to the documentation of this file.
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 
00041 template <typename PointT, typename Scalar> void
00042 pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00043                           pcl::PointCloud<PointT> &cloud_out,
00044                           const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
00045 {
00046   if (&cloud_in != &cloud_out)
00047   {
00048     // Note: could be replaced by cloud_out = cloud_in
00049     cloud_out.header   = cloud_in.header;
00050     cloud_out.is_dense = cloud_in.is_dense;
00051     cloud_out.width    = cloud_in.width;
00052     cloud_out.height   = cloud_in.height;
00053     cloud_out.points.reserve (cloud_out.points.size ());
00054     cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
00055     cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
00056     cloud_out.sensor_origin_      = cloud_in.sensor_origin_;
00057   }
00058 
00059   if (cloud_in.is_dense)
00060   {
00061     // If the dataset is dense, simply transform it!
00062     for (size_t i = 0; i < cloud_out.points.size (); ++i)
00063     {
00064       //cloud_out.points[i].getVector3fMap () = transform * cloud_in.points[i].getVector3fMap ();
00065       Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
00066       cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
00067       cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
00068       cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
00069     }
00070   }
00071   else
00072   {
00073     // Dataset might contain NaNs and Infs, so check for them first,
00074     // otherwise we get errors during the multiplication (?)
00075     for (size_t i = 0; i < cloud_out.points.size (); ++i)
00076     {
00077       if (!pcl_isfinite (cloud_in.points[i].x) || 
00078           !pcl_isfinite (cloud_in.points[i].y) || 
00079           !pcl_isfinite (cloud_in.points[i].z))
00080         continue;
00081       //cloud_out.points[i].getVector3fMap () = transform * cloud_in.points[i].getVector3fMap ();
00082       Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
00083       cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
00084       cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
00085       cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
00086     }
00087   }
00088 }
00089 
00091 template <typename PointT, typename Scalar> void
00092 pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00093                           const std::vector<int> &indices, 
00094                           pcl::PointCloud<PointT> &cloud_out,
00095                           const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
00096 {
00097   size_t npts = indices.size ();
00098   // In order to transform the data, we need to remove NaNs
00099   cloud_out.is_dense = cloud_in.is_dense;
00100   cloud_out.header   = cloud_in.header;
00101   cloud_out.width    = static_cast<int> (npts);
00102   cloud_out.height   = 1;
00103   cloud_out.points.resize (npts);
00104   cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
00105   cloud_out.sensor_origin_      = cloud_in.sensor_origin_;
00106 
00107   if (cloud_in.is_dense)
00108   {
00109     // If the dataset is dense, simply transform it!
00110     for (size_t i = 0; i < npts; ++i)
00111     {
00112       // Copy fields first, then transform xyz data
00113       //cloud_out.points[i] = cloud_in.points[indices[i]]; 
00114       //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
00115       Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
00116       cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
00117       cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
00118       cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
00119     }
00120   }
00121   else
00122   {
00123     // Dataset might contain NaNs and Infs, so check for them first,
00124     // otherwise we get errors during the multiplication (?)
00125     for (size_t i = 0; i < npts; ++i)
00126     {
00127       if (!pcl_isfinite (cloud_in.points[indices[i]].x) || 
00128           !pcl_isfinite (cloud_in.points[indices[i]].y) || 
00129           !pcl_isfinite (cloud_in.points[indices[i]].z))
00130         continue;
00131       //cloud_out.points[i] = cloud_in.points[indices[i]]; 
00132       //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
00133       Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
00134       cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
00135       cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
00136       cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
00137     }
00138   }
00139 }
00140 
00142 template <typename PointT, typename Scalar> void
00143 pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00144                                      pcl::PointCloud<PointT> &cloud_out,
00145                                      const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
00146 {
00147   if (&cloud_in != &cloud_out)
00148   {
00149     // Note: could be replaced by cloud_out = cloud_in
00150     cloud_out.header   = cloud_in.header;
00151     cloud_out.width    = cloud_in.width;
00152     cloud_out.height   = cloud_in.height;
00153     cloud_out.is_dense = cloud_in.is_dense;
00154     cloud_out.points.reserve (cloud_out.points.size ());
00155     cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
00156     cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
00157     cloud_out.sensor_origin_      = cloud_in.sensor_origin_;
00158   }
00159 
00160   // If the data is dense, we don't need to check for NaN
00161   if (cloud_in.is_dense)
00162   {
00163     for (size_t i = 0; i < cloud_out.points.size (); ++i)
00164     {
00165       //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
00166       Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
00167       cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
00168       cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
00169       cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
00170 
00171       // Rotate normals (WARNING: transform.rotation () uses SVD internally!)
00172       //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
00173       Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[i].normal_x, cloud_in[i].normal_y, cloud_in[i].normal_z);
00174       cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
00175       cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
00176       cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
00177     }
00178   }
00179   // Dataset might contain NaNs and Infs, so check for them first.
00180   else
00181   {
00182     for (size_t i = 0; i < cloud_out.points.size (); ++i)
00183     {
00184       if (!pcl_isfinite (cloud_in.points[i].x) || 
00185           !pcl_isfinite (cloud_in.points[i].y) || 
00186           !pcl_isfinite (cloud_in.points[i].z))
00187         continue;
00188 
00189       //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
00190       Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
00191       cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
00192       cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
00193       cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
00194 
00195       // Rotate normals
00196       //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
00197       Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[i].normal_x, cloud_in[i].normal_y, cloud_in[i].normal_z);
00198       cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
00199       cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
00200       cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
00201     }
00202   }
00203 }
00204 
00206 template <typename PointT, typename Scalar> void
00207 pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00208                                      const std::vector<int> &indices, 
00209                                      pcl::PointCloud<PointT> &cloud_out,
00210                                      const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
00211 {
00212   size_t npts = indices.size ();
00213   // In order to transform the data, we need to remove NaNs
00214   cloud_out.is_dense = cloud_in.is_dense;
00215   cloud_out.header   = cloud_in.header;
00216   cloud_out.width    = static_cast<int> (npts);
00217   cloud_out.height   = 1;
00218   cloud_out.points.resize (npts);
00219   cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
00220   cloud_out.sensor_origin_      = cloud_in.sensor_origin_;
00221 
00222   // If the data is dense, we don't need to check for NaN
00223   if (cloud_in.is_dense)
00224   {
00225     for (size_t i = 0; i < cloud_out.points.size (); ++i)
00226     {
00227       //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
00228       Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
00229       cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
00230       cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
00231       cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
00232 
00233       // Rotate normals
00234       //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
00235       Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[indices[i]].normal_x, cloud_in[indices[i]].normal_y, cloud_in[indices[i]].normal_z);
00236       cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
00237       cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
00238       cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
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[indices[i]].x) || 
00247           !pcl_isfinite (cloud_in.points[indices[i]].y) || 
00248           !pcl_isfinite (cloud_in.points[indices[i]].z))
00249         continue;
00250 
00251       //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
00252       Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
00253       cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
00254       cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
00255       cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
00256 
00257       // Rotate normals
00258       //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
00259       Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[indices[i]].normal_x, cloud_in[indices[i]].normal_y, cloud_in[indices[i]].normal_z);
00260       cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
00261       cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
00262       cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
00263     }
00264   }
00265 }
00266 
00268 template <typename PointT, typename Scalar> inline void
00269 pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00270                           pcl::PointCloud<PointT> &cloud_out,
00271                           const Eigen::Matrix<Scalar, 3, 1> &offset, 
00272                           const Eigen::Quaternion<Scalar> &rotation)
00273 {
00274   Eigen::Translation<Scalar, 3> translation (offset);
00275   // Assemble an Eigen Transform
00276   Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
00277   transformPointCloud (cloud_in, cloud_out, t);
00278 }
00279 
00281 template <typename PointT, typename Scalar> inline void
00282 pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
00283                                      pcl::PointCloud<PointT> &cloud_out,
00284                                      const Eigen::Matrix<Scalar, 3, 1> &offset, 
00285                                      const Eigen::Quaternion<Scalar> &rotation)
00286 {
00287   Eigen::Translation<Scalar, 3> translation (offset);
00288   // Assemble an Eigen Transform
00289   Eigen::Transform<Scalar, 3, Eigen::Affine> t (translation * rotation);
00290   transformPointCloudWithNormals (cloud_in, cloud_out, t);
00291 }
00292 
00294 template <typename PointT, typename Scalar> inline PointT
00295 pcl::transformPoint (const PointT &point, 
00296                      const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
00297 {
00298   PointT ret = point;
00299   //ret.getVector3fMap () = transform * point.getVector3fMap ();
00300   ret.x = static_cast<float> (transform (0, 0) * point.x + transform (0, 1) * point.y + transform (0, 2) * point.z + transform (0, 3));
00301   ret.y = static_cast<float> (transform (1, 0) * point.x + transform (1, 1) * point.y + transform (1, 2) * point.z + transform (1, 3));
00302   ret.z = static_cast<float> (transform (2, 0) * point.x + transform (2, 1) * point.y + transform (2, 2) * point.z + transform (2, 3));
00303 
00304   return (ret);
00305 }
00306 
00308 template <typename PointT, typename Scalar> double
00309 pcl::getPrincipalTransformation (const pcl::PointCloud<PointT> &cloud, 
00310                                  Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
00311 {
00312   EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
00313   Eigen::Matrix<Scalar, 4, 1> centroid;
00314   
00315   pcl::computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid);
00316 
00317   EIGEN_ALIGN16 Eigen::Matrix<Scalar, 3, 3> eigen_vects;
00318   Eigen::Matrix<Scalar, 3, 1> eigen_vals;
00319   pcl::eigen33 (covariance_matrix, eigen_vects, eigen_vals);
00320 
00321   double rel1 = eigen_vals.coeff (0) / eigen_vals.coeff (1);
00322   double rel2 = eigen_vals.coeff (1) / eigen_vals.coeff (2);
00323   
00324   transform.translation () = centroid.head (3);
00325   transform.linear () = eigen_vects;
00326   
00327   return (std::min (rel1, rel2));
00328 }
00329 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:37:05