Point Cloud Library (PCL)
1.7.0
|
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 00040 /////////////////////////////////////////////////////////////////////////////////////////// 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 00090 /////////////////////////////////////////////////////////////////////////////////////////// 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 00141 /////////////////////////////////////////////////////////////////////////////////////////// 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 00205 /////////////////////////////////////////////////////////////////////////////////////////// 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 00267 /////////////////////////////////////////////////////////////////////////////////////////// 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 00280 /////////////////////////////////////////////////////////////////////////////////////////// 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 00293 /////////////////////////////////////////////////////////////////////////////////////////// 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 00307 /////////////////////////////////////////////////////////////////////////////////////////// 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