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-2011, 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 * $Id$ 00038 * 00039 */ 00040 00041 #ifndef PCL_NORMAL_3D_H_ 00042 #define PCL_NORMAL_3D_H_ 00043 00044 #include <pcl/features/feature.h> 00045 #include <pcl/common/centroid.h> 00046 00047 namespace pcl 00048 { 00049 /** \brief Compute the Least-Squares plane fit for a given set of points, and return the estimated plane 00050 * parameters together with the surface curvature. 00051 * \param cloud the input point cloud 00052 * \param plane_parameters the plane parameters as: a, b, c, d (ax + by + cz + d = 0) 00053 * \param curvature the estimated surface curvature as a measure of 00054 * \f[ 00055 * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2) 00056 * \f] 00057 * \ingroup features 00058 */ 00059 template <typename PointT> inline void 00060 computePointNormal (const pcl::PointCloud<PointT> &cloud, 00061 Eigen::Vector4f &plane_parameters, float &curvature) 00062 { 00063 // Placeholder for the 3x3 covariance matrix at each surface patch 00064 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 00065 // 16-bytes aligned placeholder for the XYZ centroid of a surface patch 00066 Eigen::Vector4f xyz_centroid; 00067 00068 if (cloud.size () < 3 || 00069 computeMeanAndCovarianceMatrix (cloud, covariance_matrix, xyz_centroid) == 0) 00070 { 00071 plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ()); 00072 curvature = std::numeric_limits<float>::quiet_NaN (); 00073 return; 00074 } 00075 00076 // Get the plane normal and surface curvature 00077 solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature); 00078 } 00079 00080 /** \brief Compute the Least-Squares plane fit for a given set of points, using their indices, 00081 * and return the estimated plane parameters together with the surface curvature. 00082 * \param cloud the input point cloud 00083 * \param indices the point cloud indices that need to be used 00084 * \param plane_parameters the plane parameters as: a, b, c, d (ax + by + cz + d = 0) 00085 * \param curvature the estimated surface curvature as a measure of 00086 * \f[ 00087 * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2) 00088 * \f] 00089 * \ingroup features 00090 */ 00091 template <typename PointT> inline void 00092 computePointNormal (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices, 00093 Eigen::Vector4f &plane_parameters, float &curvature) 00094 { 00095 // Placeholder for the 3x3 covariance matrix at each surface patch 00096 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 00097 // 16-bytes aligned placeholder for the XYZ centroid of a surface patch 00098 Eigen::Vector4f xyz_centroid; 00099 if (indices.size () < 3 || 00100 computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, xyz_centroid) == 0) 00101 { 00102 plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ()); 00103 curvature = std::numeric_limits<float>::quiet_NaN (); 00104 return; 00105 } 00106 // Get the plane normal and surface curvature 00107 solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature); 00108 } 00109 00110 /** \brief Flip (in place) the estimated normal of a point towards a given viewpoint 00111 * \param point a given point 00112 * \param vp_x the X coordinate of the viewpoint 00113 * \param vp_y the X coordinate of the viewpoint 00114 * \param vp_z the X coordinate of the viewpoint 00115 * \param normal the plane normal to be flipped 00116 * \ingroup features 00117 */ 00118 template <typename PointT, typename Scalar> inline void 00119 flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, 00120 Eigen::Matrix<Scalar, 4, 1>& normal) 00121 { 00122 Eigen::Matrix <Scalar, 4, 1> vp (vp_x - point.x, vp_y - point.y, vp_z - point.z, 0); 00123 00124 // Dot product between the (viewpoint - point) and the plane normal 00125 float cos_theta = vp.dot (normal); 00126 00127 // Flip the plane normal 00128 if (cos_theta < 0) 00129 { 00130 normal *= -1; 00131 normal[3] = 0.0f; 00132 // Hessian form (D = nc . p_plane (centroid here) + p) 00133 normal[3] = -1 * normal.dot (point.getVector4fMap ()); 00134 } 00135 } 00136 00137 /** \brief Flip (in place) the estimated normal of a point towards a given viewpoint 00138 * \param point a given point 00139 * \param vp_x the X coordinate of the viewpoint 00140 * \param vp_y the X coordinate of the viewpoint 00141 * \param vp_z the X coordinate of the viewpoint 00142 * \param normal the plane normal to be flipped 00143 * \ingroup features 00144 */ 00145 template <typename PointT, typename Scalar> inline void 00146 flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, 00147 Eigen::Matrix<Scalar, 3, 1>& normal) 00148 { 00149 Eigen::Matrix <Scalar, 3, 1> vp (vp_x - point.x, vp_y - point.y, vp_z - point.z); 00150 00151 // Flip the plane normal 00152 if (vp.dot (normal) < 0) 00153 normal *= -1; 00154 } 00155 00156 /** \brief Flip (in place) the estimated normal of a point towards a given viewpoint 00157 * \param point a given point 00158 * \param vp_x the X coordinate of the viewpoint 00159 * \param vp_y the X coordinate of the viewpoint 00160 * \param vp_z the X coordinate of the viewpoint 00161 * \param nx the resultant X component of the plane normal 00162 * \param ny the resultant Y component of the plane normal 00163 * \param nz the resultant Z component of the plane normal 00164 * \ingroup features 00165 */ 00166 template <typename PointT> inline void 00167 flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, 00168 float &nx, float &ny, float &nz) 00169 { 00170 // See if we need to flip any plane normals 00171 vp_x -= point.x; 00172 vp_y -= point.y; 00173 vp_z -= point.z; 00174 00175 // Dot product between the (viewpoint - point) and the plane normal 00176 float cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz); 00177 00178 // Flip the plane normal 00179 if (cos_theta < 0) 00180 { 00181 nx *= -1; 00182 ny *= -1; 00183 nz *= -1; 00184 } 00185 } 00186 00187 /** \brief NormalEstimation estimates local surface properties (surface normals and curvatures)at each 00188 * 3D point. If PointOutT is specified as pcl::Normal, the normal is stored in the first 3 components (0-2), 00189 * and the curvature is stored in component 3. 00190 * 00191 * \note The code is stateful as we do not expect this class to be multicore parallelized. Please look at 00192 * \ref NormalEstimationOMP for a parallel implementation. 00193 * \author Radu B. Rusu 00194 * \ingroup features 00195 */ 00196 template <typename PointInT, typename PointOutT> 00197 class NormalEstimation: public Feature<PointInT, PointOutT> 00198 { 00199 public: 00200 typedef boost::shared_ptr<NormalEstimation<PointInT, PointOutT> > Ptr; 00201 typedef boost::shared_ptr<const NormalEstimation<PointInT, PointOutT> > ConstPtr; 00202 using Feature<PointInT, PointOutT>::feature_name_; 00203 using Feature<PointInT, PointOutT>::getClassName; 00204 using Feature<PointInT, PointOutT>::indices_; 00205 using Feature<PointInT, PointOutT>::input_; 00206 using Feature<PointInT, PointOutT>::surface_; 00207 using Feature<PointInT, PointOutT>::k_; 00208 using Feature<PointInT, PointOutT>::search_radius_; 00209 using Feature<PointInT, PointOutT>::search_parameter_; 00210 00211 typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut; 00212 typedef typename Feature<PointInT, PointOutT>::PointCloudConstPtr PointCloudConstPtr; 00213 00214 /** \brief Empty constructor. */ 00215 NormalEstimation () 00216 : vpx_ (0) 00217 , vpy_ (0) 00218 , vpz_ (0) 00219 , covariance_matrix_ () 00220 , xyz_centroid_ () 00221 , use_sensor_origin_ (true) 00222 { 00223 feature_name_ = "NormalEstimation"; 00224 }; 00225 00226 /** \brief Empty destructor */ 00227 virtual ~NormalEstimation () {} 00228 00229 /** \brief Compute the Least-Squares plane fit for a given set of points, using their indices, 00230 * and return the estimated plane parameters together with the surface curvature. 00231 * \param cloud the input point cloud 00232 * \param indices the point cloud indices that need to be used 00233 * \param plane_parameters the plane parameters as: a, b, c, d (ax + by + cz + d = 0) 00234 * \param curvature the estimated surface curvature as a measure of 00235 * \f[ 00236 * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2) 00237 * \f] 00238 */ 00239 inline void 00240 computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, 00241 Eigen::Vector4f &plane_parameters, float &curvature) 00242 { 00243 if (indices.size () < 3 || 00244 computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix_, xyz_centroid_) == 0) 00245 { 00246 plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ()); 00247 curvature = std::numeric_limits<float>::quiet_NaN (); 00248 return; 00249 } 00250 00251 // Get the plane normal and surface curvature 00252 solvePlaneParameters (covariance_matrix_, xyz_centroid_, plane_parameters, curvature); 00253 } 00254 00255 /** \brief Compute the Least-Squares plane fit for a given set of points, using their indices, 00256 * and return the estimated plane parameters together with the surface curvature. 00257 * \param cloud the input point cloud 00258 * \param indices the point cloud indices that need to be used 00259 * \param nx the resultant X component of the plane normal 00260 * \param ny the resultant Y component of the plane normal 00261 * \param nz the resultant Z component of the plane normal 00262 * \param curvature the estimated surface curvature as a measure of 00263 * \f[ 00264 * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2) 00265 * \f] 00266 */ 00267 inline void 00268 computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, 00269 float &nx, float &ny, float &nz, float &curvature) 00270 { 00271 if (indices.size () < 3 || 00272 computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix_, xyz_centroid_) == 0) 00273 { 00274 nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN (); 00275 return; 00276 } 00277 00278 // Get the plane normal and surface curvature 00279 solvePlaneParameters (covariance_matrix_, nx, ny, nz, curvature); 00280 } 00281 00282 /** \brief Provide a pointer to the input dataset 00283 * \param cloud the const boost shared pointer to a PointCloud message 00284 */ 00285 virtual inline void 00286 setInputCloud (const PointCloudConstPtr &cloud) 00287 { 00288 input_ = cloud; 00289 if (use_sensor_origin_) 00290 { 00291 vpx_ = input_->sensor_origin_.coeff (0); 00292 vpy_ = input_->sensor_origin_.coeff (1); 00293 vpz_ = input_->sensor_origin_.coeff (2); 00294 } 00295 } 00296 00297 /** \brief Set the viewpoint. 00298 * \param vpx the X coordinate of the viewpoint 00299 * \param vpy the Y coordinate of the viewpoint 00300 * \param vpz the Z coordinate of the viewpoint 00301 */ 00302 inline void 00303 setViewPoint (float vpx, float vpy, float vpz) 00304 { 00305 vpx_ = vpx; 00306 vpy_ = vpy; 00307 vpz_ = vpz; 00308 use_sensor_origin_ = false; 00309 } 00310 00311 /** \brief Get the viewpoint. 00312 * \param [out] vpx x-coordinate of the view point 00313 * \param [out] vpy y-coordinate of the view point 00314 * \param [out] vpz z-coordinate of the view point 00315 * \note this method returns the currently used viewpoint for normal flipping. 00316 * If the viewpoint is set manually using the setViewPoint method, this method will return the set view point coordinates. 00317 * If an input cloud is set, it will return the sensor origin otherwise it will return the origin (0, 0, 0) 00318 */ 00319 inline void 00320 getViewPoint (float &vpx, float &vpy, float &vpz) 00321 { 00322 vpx = vpx_; 00323 vpy = vpy_; 00324 vpz = vpz_; 00325 } 00326 00327 /** \brief sets whether the sensor origin or a user given viewpoint should be used. After this method, the 00328 * normal estimation method uses the sensor origin of the input cloud. 00329 * to use a user defined view point, use the method setViewPoint 00330 */ 00331 inline void 00332 useSensorOriginAsViewPoint () 00333 { 00334 use_sensor_origin_ = true; 00335 if (input_) 00336 { 00337 vpx_ = input_->sensor_origin_.coeff (0); 00338 vpy_ = input_->sensor_origin_.coeff (1); 00339 vpz_ = input_->sensor_origin_.coeff (2); 00340 } 00341 else 00342 { 00343 vpx_ = 0; 00344 vpy_ = 0; 00345 vpz_ = 0; 00346 } 00347 } 00348 00349 protected: 00350 /** \brief Estimate normals for all points given in <setInputCloud (), setIndices ()> using the surface in 00351 * setSearchSurface () and the spatial locator in setSearchMethod () 00352 * \note In situations where not enough neighbors are found, the normal and curvature values are set to -1. 00353 * \param output the resultant point cloud model dataset that contains surface normals and curvatures 00354 */ 00355 void 00356 computeFeature (PointCloudOut &output); 00357 00358 /** \brief Values describing the viewpoint ("pinhole" camera model assumed). For per point viewpoints, inherit 00359 * from NormalEstimation and provide your own computeFeature (). By default, the viewpoint is set to 0,0,0. */ 00360 float vpx_, vpy_, vpz_; 00361 00362 /** \brief Placeholder for the 3x3 covariance matrix at each surface patch. */ 00363 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix_; 00364 00365 /** \brief 16-bytes aligned placeholder for the XYZ centroid of a surface patch. */ 00366 Eigen::Vector4f xyz_centroid_; 00367 00368 /** whether the sensor origin of the input cloud or a user given viewpoint should be used.*/ 00369 bool use_sensor_origin_; 00370 00371 public: 00372 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00373 }; 00374 } 00375 00376 #ifdef PCL_NO_PRECOMPILE 00377 #include <pcl/features/impl/normal_3d.hpp> 00378 #endif 00379 00380 #endif //#ifndef PCL_NORMAL_3D_H_ 00381