1 /* 2 * Software License Agreement (BSD License) 3 * 4 * Point Cloud Library (PCL) - www.pointclouds.org 5 * Copyright (c) 2010-2011, Willow Garage, Inc. 6 * Copyright (c) 2012-, Open Perception, Inc. 7 * 8 * All rights reserved. 9 * 10 * Redistribution and use in source and binary forms, with or without 11 * modification, are permitted provided that the following conditions 12 * are met: 13 * 14 * * Redistributions of source code must retain the above copyright 15 * notice, this list of conditions and the following disclaimer. 16 * * Redistributions in binary form must reproduce the above 17 * copyright notice, this list of conditions and the following 18 * disclaimer in the documentation and/or other materials provided 19 * with the distribution. 20 * * Neither the name of the copyright holder(s) nor the names of its 21 * contributors may be used to endorse or promote products derived 22 * from this software without specific prior written permission. 23 * 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 * POSSIBILITY OF SUCH DAMAGE. 36 * 37 * $Id$ 38 * 39 */ 40 41 #pragma once 42 43 #include <pcl/memory.h> 44 #include <pcl/pcl_macros.h> 45 #include <pcl/features/feature.h> 46 #include <pcl/common/centroid.h> 47 48 namespace pcl 49 { 50 /** \brief Compute the Least-Squares plane fit for a given set of points, and return the estimated plane 51 * parameters together with the surface curvature. 52 * \param cloud the input point cloud 53 * \param plane_parameters the plane parameters as: a, b, c, d (ax + by + cz + d = 0) 54 * \param curvature the estimated surface curvature as a measure of 55 * \f[ 56 * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2) 57 * \f] 58 * \ingroup features 59 */ 60 template <typename PointT> inline bool computePointNormal(const pcl::PointCloud<PointT> & cloud,Eigen::Vector4f & plane_parameters,float & curvature)61 computePointNormal (const pcl::PointCloud<PointT> &cloud, 62 Eigen::Vector4f &plane_parameters, float &curvature) 63 { 64 // Placeholder for the 3x3 covariance matrix at each surface patch 65 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 66 // 16-bytes aligned placeholder for the XYZ centroid of a surface patch 67 Eigen::Vector4f xyz_centroid; 68 69 if (cloud.size () < 3 || 70 computeMeanAndCovarianceMatrix (cloud, covariance_matrix, xyz_centroid) == 0) 71 { 72 plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ()); 73 curvature = std::numeric_limits<float>::quiet_NaN (); 74 return false; 75 } 76 77 // Get the plane normal and surface curvature 78 solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature); 79 return true; 80 } 81 82 /** \brief Compute the Least-Squares plane fit for a given set of points, using their indices, 83 * and return the estimated plane parameters together with the surface curvature. 84 * \param cloud the input point cloud 85 * \param indices the point cloud indices that need to be used 86 * \param plane_parameters the plane parameters as: a, b, c, d (ax + by + cz + d = 0) 87 * \param curvature the estimated surface curvature as a measure of 88 * \f[ 89 * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2) 90 * \f] 91 * \ingroup features 92 */ 93 template <typename PointT> inline bool computePointNormal(const pcl::PointCloud<PointT> & cloud,const pcl::Indices & indices,Eigen::Vector4f & plane_parameters,float & curvature)94 computePointNormal (const pcl::PointCloud<PointT> &cloud, const pcl::Indices &indices, 95 Eigen::Vector4f &plane_parameters, float &curvature) 96 { 97 // Placeholder for the 3x3 covariance matrix at each surface patch 98 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 99 // 16-bytes aligned placeholder for the XYZ centroid of a surface patch 100 Eigen::Vector4f xyz_centroid; 101 if (indices.size () < 3 || 102 computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, xyz_centroid) == 0) 103 { 104 plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ()); 105 curvature = std::numeric_limits<float>::quiet_NaN (); 106 return false; 107 } 108 // Get the plane normal and surface curvature 109 solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature); 110 return true; 111 } 112 113 /** \brief Flip (in place) the estimated normal of a point towards a given viewpoint 114 * \param point a given point 115 * \param vp_x the X coordinate of the viewpoint 116 * \param vp_y the X coordinate of the viewpoint 117 * \param vp_z the X coordinate of the viewpoint 118 * \param normal the plane normal to be flipped 119 * \ingroup features 120 */ 121 template <typename PointT, typename Scalar> inline void flipNormalTowardsViewpoint(const PointT & point,float vp_x,float vp_y,float vp_z,Eigen::Matrix<Scalar,4,1> & normal)122 flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, 123 Eigen::Matrix<Scalar, 4, 1>& normal) 124 { 125 Eigen::Matrix <Scalar, 4, 1> vp (vp_x - point.x, vp_y - point.y, vp_z - point.z, 0); 126 127 // Dot product between the (viewpoint - point) and the plane normal 128 float cos_theta = vp.dot (normal); 129 130 // Flip the plane normal 131 if (cos_theta < 0) 132 { 133 normal *= -1; 134 normal[3] = 0.0f; 135 // Hessian form (D = nc . p_plane (centroid here) + p) 136 normal[3] = -1 * normal.dot (point.getVector4fMap ()); 137 } 138 } 139 140 /** \brief Flip (in place) the estimated normal of a point towards a given viewpoint 141 * \param point a given point 142 * \param vp_x the X coordinate of the viewpoint 143 * \param vp_y the X coordinate of the viewpoint 144 * \param vp_z the X coordinate of the viewpoint 145 * \param normal the plane normal to be flipped 146 * \ingroup features 147 */ 148 template <typename PointT, typename Scalar> inline void flipNormalTowardsViewpoint(const PointT & point,float vp_x,float vp_y,float vp_z,Eigen::Matrix<Scalar,3,1> & normal)149 flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, 150 Eigen::Matrix<Scalar, 3, 1>& normal) 151 { 152 Eigen::Matrix <Scalar, 3, 1> vp (vp_x - point.x, vp_y - point.y, vp_z - point.z); 153 154 // Flip the plane normal 155 if (vp.dot (normal) < 0) 156 normal *= -1; 157 } 158 159 /** \brief Flip (in place) the estimated normal of a point towards a given viewpoint 160 * \param point a given point 161 * \param vp_x the X coordinate of the viewpoint 162 * \param vp_y the X coordinate of the viewpoint 163 * \param vp_z the X coordinate of the viewpoint 164 * \param nx the resultant X component of the plane normal 165 * \param ny the resultant Y component of the plane normal 166 * \param nz the resultant Z component of the plane normal 167 * \ingroup features 168 */ 169 template <typename PointT> inline void flipNormalTowardsViewpoint(const PointT & point,float vp_x,float vp_y,float vp_z,float & nx,float & ny,float & nz)170 flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, 171 float &nx, float &ny, float &nz) 172 { 173 // See if we need to flip any plane normals 174 vp_x -= point.x; 175 vp_y -= point.y; 176 vp_z -= point.z; 177 178 // Dot product between the (viewpoint - point) and the plane normal 179 float cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz); 180 181 // Flip the plane normal 182 if (cos_theta < 0) 183 { 184 nx *= -1; 185 ny *= -1; 186 nz *= -1; 187 } 188 } 189 190 /** \brief Flip (in place) normal to get the same sign of the mean of the normals specified by normal_indices. 191 * 192 * The method is described in: 193 * A. Petrelli, L. Di Stefano, "A repeatable and efficient canonical reference for surface matching", 3DimPVT, 2012 194 * A. Petrelli, L. Di Stefano, "On the repeatability of the local reference frame for partial shape matching", 13th International Conference on Computer Vision (ICCV), 2011 195 * 196 * Normals should be unit vectors. Otherwise the resulting mean would be weighted by the normal norms. 197 * \param[in] normal_cloud Cloud of normals used to compute the mean 198 * \param[in] normal_indices Indices of normals used to compute the mean 199 * \param[in] normal input Normal to flip. Normal is modified by the function. 200 * \return false if normal_indices does not contain any valid normal. 201 * \ingroup features 202 */ 203 template<typename PointNT> inline bool flipNormalTowardsNormalsMean(pcl::PointCloud<PointNT> const & normal_cloud,pcl::Indices const & normal_indices,Eigen::Vector3f & normal)204 flipNormalTowardsNormalsMean ( pcl::PointCloud<PointNT> const &normal_cloud, 205 pcl::Indices const &normal_indices, 206 Eigen::Vector3f &normal) 207 { 208 Eigen::Vector3f normal_mean = Eigen::Vector3f::Zero (); 209 210 for (const auto &normal_index : normal_indices) 211 { 212 const PointNT& cur_pt = normal_cloud[normal_index]; 213 214 if (pcl::isFinite (cur_pt)) 215 { 216 normal_mean += cur_pt.getNormalVector3fMap (); 217 } 218 } 219 220 if (normal_mean.isZero ()) 221 return false; 222 223 normal_mean.normalize (); 224 225 if (normal.dot (normal_mean) < 0) 226 { 227 normal = -normal; 228 } 229 230 return true; 231 } 232 233 /** \brief NormalEstimation estimates local surface properties (surface normals and curvatures)at each 234 * 3D point. If PointOutT is specified as pcl::Normal, the normal is stored in the first 3 components (0-2), 235 * and the curvature is stored in component 3. 236 * 237 * \note The code is stateful as we do not expect this class to be multicore parallelized. Please look at 238 * \ref NormalEstimationOMP for a parallel implementation. 239 * \author Radu B. Rusu 240 * \ingroup features 241 */ 242 template <typename PointInT, typename PointOutT> 243 class NormalEstimation: public Feature<PointInT, PointOutT> 244 { 245 public: 246 using Ptr = shared_ptr<NormalEstimation<PointInT, PointOutT> >; 247 using ConstPtr = shared_ptr<const NormalEstimation<PointInT, PointOutT> >; 248 using Feature<PointInT, PointOutT>::feature_name_; 249 using Feature<PointInT, PointOutT>::getClassName; 250 using Feature<PointInT, PointOutT>::indices_; 251 using Feature<PointInT, PointOutT>::input_; 252 using Feature<PointInT, PointOutT>::surface_; 253 using Feature<PointInT, PointOutT>::k_; 254 using Feature<PointInT, PointOutT>::search_radius_; 255 using Feature<PointInT, PointOutT>::search_parameter_; 256 257 using PointCloudOut = typename Feature<PointInT, PointOutT>::PointCloudOut; 258 using PointCloudConstPtr = typename Feature<PointInT, PointOutT>::PointCloudConstPtr; 259 260 /** \brief Empty constructor. */ NormalEstimation()261 NormalEstimation () 262 : vpx_ (0) 263 , vpy_ (0) 264 , vpz_ (0) 265 , use_sensor_origin_ (true) 266 { 267 feature_name_ = "NormalEstimation"; 268 }; 269 270 /** \brief Empty destructor */ ~NormalEstimation()271 ~NormalEstimation () {} 272 273 /** \brief Compute the Least-Squares plane fit for a given set of points, using their indices, 274 * and return the estimated plane parameters together with the surface curvature. 275 * \param cloud the input point cloud 276 * \param indices the point cloud indices that need to be used 277 * \param plane_parameters the plane parameters as: a, b, c, d (ax + by + cz + d = 0) 278 * \param curvature the estimated surface curvature as a measure of 279 * \f[ 280 * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2) 281 * \f] 282 */ 283 inline bool computePointNormal(const pcl::PointCloud<PointInT> & cloud,const pcl::Indices & indices,Eigen::Vector4f & plane_parameters,float & curvature)284 computePointNormal (const pcl::PointCloud<PointInT> &cloud, const pcl::Indices &indices, 285 Eigen::Vector4f &plane_parameters, float &curvature) 286 { 287 if (indices.size () < 3 || 288 computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix_, xyz_centroid_) == 0) 289 { 290 plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ()); 291 curvature = std::numeric_limits<float>::quiet_NaN (); 292 return false; 293 } 294 295 // Get the plane normal and surface curvature 296 solvePlaneParameters (covariance_matrix_, xyz_centroid_, plane_parameters, curvature); 297 return true; 298 } 299 300 /** \brief Compute the Least-Squares plane fit for a given set of points, using their indices, 301 * and return the estimated plane parameters together with the surface curvature. 302 * \param cloud the input point cloud 303 * \param indices the point cloud indices that need to be used 304 * \param nx the resultant X component of the plane normal 305 * \param ny the resultant Y component of the plane normal 306 * \param nz the resultant Z component of the plane normal 307 * \param curvature the estimated surface curvature as a measure of 308 * \f[ 309 * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2) 310 * \f] 311 */ 312 inline bool computePointNormal(const pcl::PointCloud<PointInT> & cloud,const pcl::Indices & indices,float & nx,float & ny,float & nz,float & curvature)313 computePointNormal (const pcl::PointCloud<PointInT> &cloud, const pcl::Indices &indices, 314 float &nx, float &ny, float &nz, float &curvature) 315 { 316 if (indices.size () < 3 || 317 computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix_, xyz_centroid_) == 0) 318 { 319 nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN (); 320 return false; 321 } 322 323 // Get the plane normal and surface curvature 324 solvePlaneParameters (covariance_matrix_, nx, ny, nz, curvature); 325 return true; 326 } 327 328 /** \brief Provide a pointer to the input dataset 329 * \param cloud the const boost shared pointer to a PointCloud message 330 */ 331 inline void setInputCloud(const PointCloudConstPtr & cloud)332 setInputCloud (const PointCloudConstPtr &cloud) override 333 { 334 input_ = cloud; 335 if (use_sensor_origin_) 336 { 337 vpx_ = input_->sensor_origin_.coeff (0); 338 vpy_ = input_->sensor_origin_.coeff (1); 339 vpz_ = input_->sensor_origin_.coeff (2); 340 } 341 } 342 343 /** \brief Set the viewpoint. 344 * \param vpx the X coordinate of the viewpoint 345 * \param vpy the Y coordinate of the viewpoint 346 * \param vpz the Z coordinate of the viewpoint 347 */ 348 inline void setViewPoint(float vpx,float vpy,float vpz)349 setViewPoint (float vpx, float vpy, float vpz) 350 { 351 vpx_ = vpx; 352 vpy_ = vpy; 353 vpz_ = vpz; 354 use_sensor_origin_ = false; 355 } 356 357 /** \brief Get the viewpoint. 358 * \param [out] vpx x-coordinate of the view point 359 * \param [out] vpy y-coordinate of the view point 360 * \param [out] vpz z-coordinate of the view point 361 * \note this method returns the currently used viewpoint for normal flipping. 362 * If the viewpoint is set manually using the setViewPoint method, this method will return the set view point coordinates. 363 * If an input cloud is set, it will return the sensor origin otherwise it will return the origin (0, 0, 0) 364 */ 365 inline void getViewPoint(float & vpx,float & vpy,float & vpz)366 getViewPoint (float &vpx, float &vpy, float &vpz) 367 { 368 vpx = vpx_; 369 vpy = vpy_; 370 vpz = vpz_; 371 } 372 373 /** \brief sets whether the sensor origin or a user given viewpoint should be used. After this method, the 374 * normal estimation method uses the sensor origin of the input cloud. 375 * to use a user defined view point, use the method setViewPoint 376 */ 377 inline void useSensorOriginAsViewPoint()378 useSensorOriginAsViewPoint () 379 { 380 use_sensor_origin_ = true; 381 if (input_) 382 { 383 vpx_ = input_->sensor_origin_.coeff (0); 384 vpy_ = input_->sensor_origin_.coeff (1); 385 vpz_ = input_->sensor_origin_.coeff (2); 386 } 387 else 388 { 389 vpx_ = 0; 390 vpy_ = 0; 391 vpz_ = 0; 392 } 393 } 394 395 protected: 396 /** \brief Estimate normals for all points given in <setInputCloud (), setIndices ()> using the surface in 397 * setSearchSurface () and the spatial locator in setSearchMethod () 398 * \note In situations where not enough neighbors are found, the normal and curvature values are set to NaN. 399 * \param output the resultant point cloud model dataset that contains surface normals and curvatures 400 */ 401 void 402 computeFeature (PointCloudOut &output) override; 403 404 /** \brief Values describing the viewpoint ("pinhole" camera model assumed). For per point viewpoints, inherit 405 * from NormalEstimation and provide your own computeFeature (). By default, the viewpoint is set to 0,0,0. */ 406 float vpx_, vpy_, vpz_; 407 408 /** \brief Placeholder for the 3x3 covariance matrix at each surface patch. */ 409 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix_; 410 411 /** \brief 16-bytes aligned placeholder for the XYZ centroid of a surface patch. */ 412 Eigen::Vector4f xyz_centroid_; 413 414 /** whether the sensor origin of the input cloud or a user given viewpoint should be used.*/ 415 bool use_sensor_origin_; 416 417 public: 418 PCL_MAKE_ALIGNED_OPERATOR_NEW 419 }; 420 } 421 422 #ifdef PCL_NO_PRECOMPILE 423 #include <pcl/features/impl/normal_3d.hpp> 424 #endif 425