Point Cloud Library (PCL)
1.3.1
|
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 }