Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009, 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 * $Id: normal_3d.h 2631 2011-10-05 00:27:56Z mdixon $ 00035 * 00036 */ 00037 00038 #ifndef PCL_NORMAL_3D_H_ 00039 #define PCL_NORMAL_3D_H_ 00040 00041 #include <pcl/features/feature.h> 00042 00043 namespace pcl 00044 { 00055 template <typename PointT> inline void 00056 computePointNormal (const pcl::PointCloud<PointT> &cloud, 00057 Eigen::Vector4f &plane_parameters, float &curvature) 00058 { 00059 if (cloud.points.empty ()) 00060 { 00061 plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ()); 00062 curvature = std::numeric_limits<float>::quiet_NaN (); 00063 return; 00064 } 00065 // Placeholder for the 3x3 covariance matrix at each surface patch 00066 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 00067 // 16-bytes aligned placeholder for the XYZ centroid of a surface patch 00068 Eigen::Vector4f xyz_centroid; 00069 00070 // Estimate the XYZ centroid 00071 compute3DCentroid (cloud, xyz_centroid); 00072 00073 // Compute the 3x3 covariance matrix 00074 computeCovarianceMatrix (cloud, xyz_centroid, covariance_matrix); 00075 00076 // Get the plane normal and surface curvature 00077 solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature); 00078 } 00079 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 if (indices.empty ()) 00096 { 00097 plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ()); 00098 curvature = std::numeric_limits<float>::quiet_NaN (); 00099 return; 00100 } 00101 // Placeholder for the 3x3 covariance matrix at each surface patch 00102 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 00103 // 16-bytes aligned placeholder for the XYZ centroid of a surface patch 00104 Eigen::Vector4f xyz_centroid; 00105 00106 // Estimate the XYZ centroid 00107 compute3DCentroid (cloud, indices, xyz_centroid); 00108 00109 // Compute the 3x3 covariance matrix 00110 computeCovarianceMatrix (cloud, indices, xyz_centroid, covariance_matrix); 00111 00112 // Get the plane normal and surface curvature 00113 solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature); 00114 } 00115 00124 template <typename PointT> inline void 00125 flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, 00126 Eigen::Vector4f &normal) 00127 { 00128 Eigen::Vector4f vp (vp_x, vp_y, vp_z, 0); 00129 // See if we need to flip any plane normals 00130 vp -= point.getVector4fMap (); 00131 vp[3] = 0; // enforce the last coordinate 00132 00133 // Dot product between the (viewpoint - point) and the plane normal 00134 float cos_theta = vp.dot (normal); 00135 00136 // Flip the plane normal 00137 if (cos_theta < 0) 00138 { 00139 normal *= -1; 00140 normal[3] = 0; // enforce the last coordinate; 00141 // Hessian form (D = nc . p_plane (centroid here) + p) 00142 normal[3] = -1 * normal.dot (point.getVector4fMap ()); 00143 } 00144 } 00145 00156 template <typename PointT> inline void 00157 flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, 00158 float &nx, float &ny, float &nz) 00159 { 00160 // See if we need to flip any plane normals 00161 vp_x -= point.x; 00162 vp_y -= point.y; 00163 vp_z -= point.z; 00164 00165 // Dot product between the (viewpoint - point) and the plane normal 00166 float cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz); 00167 00168 // Flip the plane normal 00169 if (cos_theta < 0) 00170 { 00171 nx *= -1; 00172 ny *= -1; 00173 nz *= -1; 00174 } 00175 } 00176 00185 template <typename PointInT, typename PointOutT> 00186 class NormalEstimation: public Feature<PointInT, PointOutT> 00187 { 00188 public: 00189 using Feature<PointInT, PointOutT>::feature_name_; 00190 using Feature<PointInT, PointOutT>::getClassName; 00191 using Feature<PointInT, PointOutT>::indices_; 00192 using Feature<PointInT, PointOutT>::input_; 00193 using Feature<PointInT, PointOutT>::surface_; 00194 using Feature<PointInT, PointOutT>::k_; 00195 using Feature<PointInT, PointOutT>::search_radius_; 00196 using Feature<PointInT, PointOutT>::search_parameter_; 00197 00198 typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut; 00199 00201 NormalEstimation () : vpx_ (0), vpy_ (0), vpz_ (0) 00202 { 00203 feature_name_ = "NormalEstimation"; 00204 }; 00205 00216 inline void 00217 computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, Eigen::Vector4f &plane_parameters, float &curvature) 00218 { 00219 if (indices.empty ()) 00220 { 00221 plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ()); 00222 curvature = std::numeric_limits<float>::quiet_NaN (); 00223 return; 00224 } 00225 // Estimate the XYZ centroid 00226 compute3DCentroid (cloud, indices, xyz_centroid_); 00227 00228 // Compute the 3x3 covariance matrix 00229 computeCovarianceMatrix (cloud, indices, xyz_centroid_, covariance_matrix_); 00230 00231 // Get the plane normal and surface curvature 00232 solvePlaneParameters (covariance_matrix_, xyz_centroid_, plane_parameters, curvature); 00233 } 00234 00247 inline void 00248 computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, float &nx, float &ny, float &nz, float &curvature) 00249 { 00250 if (indices.empty ()) 00251 { 00252 nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN (); 00253 return; 00254 } 00255 // Estimate the XYZ centroid 00256 compute3DCentroid (cloud, indices, xyz_centroid_); 00257 00258 // Compute the 3x3 covariance matrix 00259 computeCovarianceMatrix (cloud, indices, xyz_centroid_, covariance_matrix_); 00260 00261 // Get the plane normal and surface curvature 00262 solvePlaneParameters (covariance_matrix_, nx, ny, nz, curvature); 00263 } 00264 00270 inline void 00271 setViewPoint (float vpx, float vpy, float vpz) 00272 { 00273 vpx_ = vpx; 00274 vpy_ = vpy; 00275 vpz_ = vpz; 00276 } 00277 00279 inline void 00280 getViewPoint (float &vpx, float &vpy, float &vpz) 00281 { 00282 vpx = vpx_; 00283 vpy = vpy_; 00284 vpz = vpz_; 00285 } 00286 00287 protected: 00293 void computeFeature (PointCloudOut &output); 00294 00295 private: 00298 float vpx_, vpy_, vpz_; 00299 00301 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix_; 00302 00304 Eigen::Vector4f xyz_centroid_; 00305 }; 00306 } 00307 00308 #endif //#ifndef PCL_NORMAL_3D_H_ 00309 00310