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 #ifndef PCL_TRANSFORMS_H_ 00040 #define PCL_TRANSFORMS_H_ 00041 00042 #include <pcl/point_cloud.h> 00043 #include <pcl/point_types.h> 00044 #include <pcl/common/centroid.h> 00045 #include <pcl/common/eigen.h> 00046 #include <pcl/PointIndices.h> 00047 00048 namespace pcl 00049 { 00050 /** \brief Apply an affine transform defined by an Eigen Transform 00051 * \param[in] cloud_in the input point cloud 00052 * \param[out] cloud_out the resultant output point cloud 00053 * \param[in] transform an affine transformation (typically a rigid transformation) 00054 * \note Can be used with cloud_in equal to cloud_out 00055 * \ingroup common 00056 */ 00057 template <typename PointT, typename Scalar> void 00058 transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00059 pcl::PointCloud<PointT> &cloud_out, 00060 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform); 00061 00062 template <typename PointT> void 00063 transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00064 pcl::PointCloud<PointT> &cloud_out, 00065 const Eigen::Affine3f &transform) 00066 { 00067 return (transformPointCloud<PointT, float> (cloud_in, cloud_out, transform)); 00068 } 00069 00070 /** \brief Apply an affine transform defined by an Eigen Transform 00071 * \param[in] cloud_in the input point cloud 00072 * \param[in] indices the set of point indices to use from the input point cloud 00073 * \param[out] cloud_out the resultant output point cloud 00074 * \param[in] transform an affine transformation (typically a rigid transformation) 00075 * \ingroup common 00076 */ 00077 template <typename PointT, typename Scalar> void 00078 transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00079 const std::vector<int> &indices, 00080 pcl::PointCloud<PointT> &cloud_out, 00081 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform); 00082 00083 template <typename PointT> void 00084 transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00085 const std::vector<int> &indices, 00086 pcl::PointCloud<PointT> &cloud_out, 00087 const Eigen::Affine3f &transform) 00088 { 00089 return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform)); 00090 } 00091 00092 /** \brief Apply an affine transform defined by an Eigen Transform 00093 * \param[in] cloud_in the input point cloud 00094 * \param[in] indices the set of point indices to use from the input point cloud 00095 * \param[out] cloud_out the resultant output point cloud 00096 * \param[in] transform an affine transformation (typically a rigid transformation) 00097 * \ingroup common 00098 */ 00099 template <typename PointT, typename Scalar> void 00100 transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00101 const pcl::PointIndices &indices, 00102 pcl::PointCloud<PointT> &cloud_out, 00103 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform) 00104 { 00105 return (transformPointCloud<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform)); 00106 } 00107 00108 template <typename PointT> void 00109 transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00110 const pcl::PointIndices &indices, 00111 pcl::PointCloud<PointT> &cloud_out, 00112 const Eigen::Affine3f &transform) 00113 { 00114 return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform)); 00115 } 00116 00117 /** \brief Transform a point cloud and rotate its normals using an Eigen transform. 00118 * \param[in] cloud_in the input point cloud 00119 * \param[out] cloud_out the resultant output point cloud 00120 * \param[in] transform an affine transformation (typically a rigid transformation) 00121 * \note Can be used with cloud_in equal to cloud_out 00122 */ 00123 template <typename PointT, typename Scalar> void 00124 transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 00125 pcl::PointCloud<PointT> &cloud_out, 00126 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform); 00127 00128 template <typename PointT> void 00129 transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 00130 pcl::PointCloud<PointT> &cloud_out, 00131 const Eigen::Affine3f &transform) 00132 { 00133 return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, transform)); 00134 } 00135 00136 /** \brief Transform a point cloud and rotate its normals using an Eigen transform. 00137 * \param[in] cloud_in the input point cloud 00138 * \param[in] indices the set of point indices to use from the input point cloud 00139 * \param[out] cloud_out the resultant output point cloud 00140 * \param[in] transform an affine transformation (typically a rigid transformation) 00141 */ 00142 template <typename PointT, typename Scalar> void 00143 transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 00144 const std::vector<int> &indices, 00145 pcl::PointCloud<PointT> &cloud_out, 00146 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform); 00147 00148 template <typename PointT> void 00149 transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 00150 const std::vector<int> &indices, 00151 pcl::PointCloud<PointT> &cloud_out, 00152 const Eigen::Affine3f &transform) 00153 { 00154 return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform)); 00155 } 00156 00157 /** \brief Transform a point cloud and rotate its normals using an Eigen transform. 00158 * \param[in] cloud_in the input point cloud 00159 * \param[in] indices the set of point indices to use from the input point cloud 00160 * \param[out] cloud_out the resultant output point cloud 00161 * \param[in] transform an affine transformation (typically a rigid transformation) 00162 */ 00163 template <typename PointT, typename Scalar> void 00164 transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 00165 const pcl::PointIndices &indices, 00166 pcl::PointCloud<PointT> &cloud_out, 00167 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform) 00168 { 00169 return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform)); 00170 } 00171 00172 00173 template <typename PointT> void 00174 transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 00175 const pcl::PointIndices &indices, 00176 pcl::PointCloud<PointT> &cloud_out, 00177 const Eigen::Affine3f &transform) 00178 { 00179 return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform)); 00180 } 00181 00182 /** \brief Apply a rigid transform defined by a 4x4 matrix 00183 * \param[in] cloud_in the input point cloud 00184 * \param[out] cloud_out the resultant output point cloud 00185 * \param[in] transform a rigid transformation 00186 * \note Can be used with cloud_in equal to cloud_out 00187 * \ingroup common 00188 */ 00189 template <typename PointT, typename Scalar> void 00190 transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00191 pcl::PointCloud<PointT> &cloud_out, 00192 const Eigen::Matrix<Scalar, 4, 4> &transform) 00193 { 00194 Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform); 00195 return (transformPointCloud<PointT, Scalar> (cloud_in, cloud_out, t)); 00196 } 00197 00198 template <typename PointT> void 00199 transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00200 pcl::PointCloud<PointT> &cloud_out, 00201 const Eigen::Matrix4f &transform) 00202 { 00203 return (transformPointCloud<PointT, float> (cloud_in, cloud_out, transform)); 00204 } 00205 00206 /** \brief Apply a rigid transform defined by a 4x4 matrix 00207 * \param[in] cloud_in the input point cloud 00208 * \param[in] indices the set of point indices to use from the input point cloud 00209 * \param[out] cloud_out the resultant output point cloud 00210 * \param[in] transform a rigid transformation 00211 * \ingroup common 00212 */ 00213 template <typename PointT, typename Scalar> void 00214 transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00215 const std::vector<int> &indices, 00216 pcl::PointCloud<PointT> &cloud_out, 00217 const Eigen::Matrix<Scalar, 4, 4> &transform) 00218 { 00219 Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform); 00220 return (transformPointCloud<PointT, Scalar> (cloud_in, indices, cloud_out, t)); 00221 } 00222 00223 template <typename PointT> void 00224 transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00225 const std::vector<int> &indices, 00226 pcl::PointCloud<PointT> &cloud_out, 00227 const Eigen::Matrix4f &transform) 00228 { 00229 return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform)); 00230 } 00231 00232 /** \brief Apply a rigid transform defined by a 4x4 matrix 00233 * \param[in] cloud_in the input point cloud 00234 * \param[in] indices the set of point indices to use from the input point cloud 00235 * \param[out] cloud_out the resultant output point cloud 00236 * \param[in] transform a rigid transformation 00237 * \ingroup common 00238 */ 00239 template <typename PointT, typename Scalar> void 00240 transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00241 const pcl::PointIndices &indices, 00242 pcl::PointCloud<PointT> &cloud_out, 00243 const Eigen::Matrix<Scalar, 4, 4> &transform) 00244 { 00245 return (transformPointCloud<PointT, Scalar> (cloud_in, indices.indices, cloud_out, transform)); 00246 } 00247 00248 template <typename PointT> void 00249 transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00250 const pcl::PointIndices &indices, 00251 pcl::PointCloud<PointT> &cloud_out, 00252 const Eigen::Matrix4f &transform) 00253 { 00254 return (transformPointCloud<PointT, float> (cloud_in, indices, cloud_out, transform)); 00255 } 00256 00257 /** \brief Transform a point cloud and rotate its normals using an Eigen transform. 00258 * \param[in] cloud_in the input point cloud 00259 * \param[out] cloud_out the resultant output point cloud 00260 * \param[in] transform an affine transformation (typically a rigid transformation) 00261 * \note Can be used with cloud_in equal to cloud_out 00262 * \ingroup common 00263 */ 00264 template <typename PointT, typename Scalar> void 00265 transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 00266 pcl::PointCloud<PointT> &cloud_out, 00267 const Eigen::Matrix<Scalar, 4, 4> &transform) 00268 { 00269 Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform); 00270 return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, cloud_out, t)); 00271 } 00272 00273 00274 template <typename PointT> void 00275 transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 00276 pcl::PointCloud<PointT> &cloud_out, 00277 const Eigen::Matrix4f &transform) 00278 { 00279 return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, transform)); 00280 } 00281 00282 /** \brief Transform a point cloud and rotate its normals using an Eigen transform. 00283 * \param[in] cloud_in the input point cloud 00284 * \param[in] indices the set of point indices to use from the input point cloud 00285 * \param[out] cloud_out the resultant output point cloud 00286 * \param[in] transform an affine transformation (typically a rigid transformation) 00287 * \note Can be used with cloud_in equal to cloud_out 00288 * \ingroup common 00289 */ 00290 template <typename PointT, typename Scalar> void 00291 transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 00292 const std::vector<int> &indices, 00293 pcl::PointCloud<PointT> &cloud_out, 00294 const Eigen::Matrix<Scalar, 4, 4> &transform) 00295 { 00296 Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform); 00297 return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices, cloud_out, t)); 00298 } 00299 00300 00301 template <typename PointT> void 00302 transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 00303 const std::vector<int> &indices, 00304 pcl::PointCloud<PointT> &cloud_out, 00305 const Eigen::Matrix4f &transform) 00306 { 00307 return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform)); 00308 } 00309 00310 /** \brief Transform a point cloud and rotate its normals using an Eigen transform. 00311 * \param[in] cloud_in the input point cloud 00312 * \param[in] indices the set of point indices to use from the input point cloud 00313 * \param[out] cloud_out the resultant output point cloud 00314 * \param[in] transform an affine transformation (typically a rigid transformation) 00315 * \note Can be used with cloud_in equal to cloud_out 00316 * \ingroup common 00317 */ 00318 template <typename PointT, typename Scalar> void 00319 transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 00320 const pcl::PointIndices &indices, 00321 pcl::PointCloud<PointT> &cloud_out, 00322 const Eigen::Matrix<Scalar, 4, 4> &transform) 00323 { 00324 Eigen::Transform<Scalar, 3, Eigen::Affine> t (transform); 00325 return (transformPointCloudWithNormals<PointT, Scalar> (cloud_in, indices, cloud_out, t)); 00326 } 00327 00328 00329 template <typename PointT> void 00330 transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 00331 const pcl::PointIndices &indices, 00332 pcl::PointCloud<PointT> &cloud_out, 00333 const Eigen::Matrix4f &transform) 00334 { 00335 return (transformPointCloudWithNormals<PointT, float> (cloud_in, indices, cloud_out, transform)); 00336 } 00337 00338 /** \brief Apply a rigid transform defined by a 3D offset and a quaternion 00339 * \param[in] cloud_in the input point cloud 00340 * \param[out] cloud_out the resultant output point cloud 00341 * \param[in] offset the translation component of the rigid transformation 00342 * \param[in] rotation the rotation component of the rigid transformation 00343 * \ingroup common 00344 */ 00345 template <typename PointT, typename Scalar> inline void 00346 transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00347 pcl::PointCloud<PointT> &cloud_out, 00348 const Eigen::Matrix<Scalar, 3, 1> &offset, 00349 const Eigen::Quaternion<Scalar> &rotation); 00350 00351 template <typename PointT> inline void 00352 transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00353 pcl::PointCloud<PointT> &cloud_out, 00354 const Eigen::Vector3f &offset, 00355 const Eigen::Quaternionf &rotation) 00356 { 00357 return (transformPointCloud<PointT, float> (cloud_in, cloud_out, offset, rotation)); 00358 } 00359 00360 /** \brief Transform a point cloud and rotate its normals using an Eigen transform. 00361 * \param[in] cloud_in the input point cloud 00362 * \param[out] cloud_out the resultant output point cloud 00363 * \param[in] offset the translation component of the rigid transformation 00364 * \param[in] rotation the rotation component of the rigid transformation 00365 * \ingroup common 00366 */ 00367 template <typename PointT, typename Scalar> inline void 00368 transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 00369 pcl::PointCloud<PointT> &cloud_out, 00370 const Eigen::Matrix<Scalar, 3, 1> &offset, 00371 const Eigen::Quaternion<Scalar> &rotation); 00372 00373 template <typename PointT> void 00374 transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 00375 pcl::PointCloud<PointT> &cloud_out, 00376 const Eigen::Vector3f &offset, 00377 const Eigen::Quaternionf &rotation) 00378 { 00379 return (transformPointCloudWithNormals<PointT, float> (cloud_in, cloud_out, offset, rotation)); 00380 } 00381 00382 /** \brief Transform a point with members x,y,z 00383 * \param[in] point the point to transform 00384 * \param[out] transform the transformation to apply 00385 * \return the transformed point 00386 * \ingroup common 00387 */ 00388 template <typename PointT, typename Scalar> inline PointT 00389 transformPoint (const PointT &point, 00390 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform); 00391 00392 template <typename PointT> inline PointT 00393 transformPoint (const PointT &point, 00394 const Eigen::Affine3f &transform) 00395 { 00396 return (transformPoint<PointT, float> (point, transform)); 00397 } 00398 00399 /** \brief Calculates the principal (PCA-based) alignment of the point cloud 00400 * \param[in] cloud the input point cloud 00401 * \param[out] transform the resultant transform 00402 * \return the ratio lambda1/lambda2 or lambda2/lambda3, whatever is closer to 1. 00403 * \note If the return value is close to one then the transformation might be not unique -> two principal directions have 00404 * almost same variance (extend) 00405 */ 00406 template <typename PointT, typename Scalar> inline double 00407 getPrincipalTransformation (const pcl::PointCloud<PointT> &cloud, 00408 Eigen::Transform<Scalar, 3, Eigen::Affine> &transform); 00409 00410 template <typename PointT> inline double 00411 getPrincipalTransformation (const pcl::PointCloud<PointT> &cloud, 00412 Eigen::Affine3f &transform) 00413 { 00414 return (getPrincipalTransformation<PointT, float> (cloud, transform)); 00415 } 00416 } 00417 00418 #include <pcl/common/impl/transforms.hpp> 00419 00420 #endif // PCL_TRANSFORMS_H_