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) 2009-2011, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 */ 00037 00038 #ifndef PCL_FILTERS_IMPL_SAMPLING_SURFACE_NORMAL_H_ 00039 #define PCL_FILTERS_IMPL_SAMPLING_SURFACE_NORMAL_H_ 00040 00041 #include <iostream> 00042 #include <vector> 00043 #include <pcl/common/eigen.h> 00044 #include <pcl/filters/sampling_surface_normal.h> 00045 00046 /////////////////////////////////////////////////////////////////////////////// 00047 template<typename PointT> void 00048 pcl::SamplingSurfaceNormal<PointT>::applyFilter (PointCloud &output) 00049 { 00050 std::vector <int> indices; 00051 int npts = int (input_->points.size ()); 00052 for (unsigned int i = 0; i < npts; i++) 00053 indices.push_back (i); 00054 00055 Vector max_vec (3, 1); 00056 Vector min_vec (3, 1); 00057 findXYZMaxMin (*input_, max_vec, min_vec); 00058 PointCloud data = *input_; 00059 partition (data, 0, npts, min_vec, max_vec, indices, output); 00060 output.width = 1; 00061 output.height = uint32_t (output.points.size ()); 00062 } 00063 00064 /////////////////////////////////////////////////////////////////////////////// 00065 template<typename PointT> void 00066 pcl::SamplingSurfaceNormal<PointT>::findXYZMaxMin (const PointCloud& cloud, Vector& max_vec, Vector& min_vec) 00067 { 00068 float maxval = cloud.points[0].x; 00069 float minval = cloud.points[0].x; 00070 00071 for (unsigned int i = 0; i < cloud.points.size (); i++) 00072 { 00073 if (cloud.points[i].x > maxval) 00074 { 00075 maxval = cloud.points[i].x; 00076 } 00077 if (cloud.points[i].x < minval) 00078 { 00079 minval = cloud.points[i].x; 00080 } 00081 } 00082 max_vec (0) = maxval; 00083 min_vec (0) = minval; 00084 00085 maxval = cloud.points[0].y; 00086 minval = cloud.points[0].y; 00087 00088 for (unsigned int i = 0; i < cloud.points.size (); i++) 00089 { 00090 if (cloud.points[i].y > maxval) 00091 { 00092 maxval = cloud.points[i].y; 00093 } 00094 if (cloud.points[i].y < minval) 00095 { 00096 minval = cloud.points[i].y; 00097 } 00098 } 00099 max_vec (1) = maxval; 00100 min_vec (1) = minval; 00101 00102 maxval = cloud.points[0].z; 00103 minval = cloud.points[0].z; 00104 00105 for (unsigned int i = 0; i < cloud.points.size (); i++) 00106 { 00107 if (cloud.points[i].z > maxval) 00108 { 00109 maxval = cloud.points[i].z; 00110 } 00111 if (cloud.points[i].z < minval) 00112 { 00113 minval = cloud.points[i].z; 00114 } 00115 } 00116 max_vec (2) = maxval; 00117 min_vec (2) = minval; 00118 } 00119 00120 /////////////////////////////////////////////////////////////////////////////// 00121 template<typename PointT> void 00122 pcl::SamplingSurfaceNormal<PointT>::partition ( 00123 const PointCloud& cloud, const int first, const int last, 00124 const Vector min_values, const Vector max_values, 00125 std::vector<int>& indices, PointCloud& output) 00126 { 00127 const int count (last - first); 00128 if (count <= sample_) 00129 { 00130 samplePartition (cloud, first, last, indices, output); 00131 return; 00132 } 00133 int cutDim = 0; 00134 (max_values - min_values).maxCoeff (&cutDim); 00135 00136 const int rightCount (count / 2); 00137 const int leftCount (count - rightCount); 00138 assert (last - rightCount == first + leftCount); 00139 00140 // sort, hack std::nth_element 00141 std::nth_element (indices.begin () + first, indices.begin () + first + leftCount, 00142 indices.begin () + last, CompareDim (cutDim, cloud)); 00143 00144 const int cutIndex (indices[first+leftCount]); 00145 const float cutVal = findCutVal (cloud, cutDim, cutIndex); 00146 00147 // update bounds for left 00148 Vector leftMaxValues (max_values); 00149 leftMaxValues[cutDim] = cutVal; 00150 // update bounds for right 00151 Vector rightMinValues (min_values); 00152 rightMinValues[cutDim] = cutVal; 00153 00154 // recurse 00155 partition (cloud, first, first + leftCount, min_values, leftMaxValues, indices, output); 00156 partition (cloud, first + leftCount, last, rightMinValues, max_values, indices, output); 00157 } 00158 00159 /////////////////////////////////////////////////////////////////////////////// 00160 template<typename PointT> void 00161 pcl::SamplingSurfaceNormal<PointT>::samplePartition ( 00162 const PointCloud& data, const int first, const int last, 00163 std::vector <int>& indices, PointCloud& output) 00164 { 00165 pcl::PointCloud <PointT> cloud; 00166 00167 for (unsigned int i = first; i < last; i++) 00168 { 00169 PointT pt; 00170 pt.x = data.points[indices[i]].x; 00171 pt.y = data.points[indices[i]].y; 00172 pt.z = data.points[indices[i]].z; 00173 cloud.points.push_back (pt); 00174 } 00175 cloud.width = 1; 00176 cloud.height = uint32_t (cloud.points.size ()); 00177 00178 Eigen::Vector4f normal; 00179 float curvature = 0; 00180 //pcl::computePointNormal<PointT> (cloud, normal, curvature); 00181 00182 computeNormal (cloud, normal, curvature); 00183 00184 for (unsigned int i = 0; i < cloud.points.size (); i++) 00185 { 00186 // TODO: change to Boost random number generators! 00187 const float r = float (std::rand ()) / float (RAND_MAX); 00188 00189 if (r < ratio_) 00190 { 00191 PointT pt = cloud.points[i]; 00192 pt.normal[0] = normal (0); 00193 pt.normal[1] = normal (1); 00194 pt.normal[2] = normal (2); 00195 pt.curvature = curvature; 00196 00197 output.points.push_back (pt); 00198 } 00199 } 00200 } 00201 00202 /////////////////////////////////////////////////////////////////////////////// 00203 template<typename PointT> void 00204 pcl::SamplingSurfaceNormal<PointT>::computeNormal (const PointCloud& cloud, Eigen::Vector4f &normal, float& curvature) 00205 { 00206 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; 00207 Eigen::Vector4f xyz_centroid; 00208 float nx = 0.0; 00209 float ny = 0.0; 00210 float nz = 0.0; 00211 00212 if (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, xyz_centroid) == 0) 00213 { 00214 nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN (); 00215 return; 00216 } 00217 00218 // Get the plane normal and surface curvature 00219 solvePlaneParameters (covariance_matrix, nx, ny, nz, curvature); 00220 normal (0) = nx; 00221 normal (1) = ny; 00222 normal (2) = nz; 00223 00224 normal (3) = 0; 00225 // Hessian form (D = nc . p_plane (centroid here) + p) 00226 normal (3) = -1 * normal.dot (xyz_centroid); 00227 } 00228 00229 ////////////////////////////////////////////////////////////////////////////////////////////// 00230 template <typename PointT> inline unsigned int 00231 pcl::SamplingSurfaceNormal<PointT>::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00232 Eigen::Matrix3f &covariance_matrix, 00233 Eigen::Vector4f ¢roid) 00234 { 00235 // create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer 00236 Eigen::Matrix<float, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 9, Eigen::RowMajor>::Zero (); 00237 unsigned int point_count = 0; 00238 for (unsigned int i = 0; i < cloud.points.size (); i++) 00239 { 00240 if (!isFinite (cloud[i])) 00241 { 00242 continue; 00243 } 00244 00245 ++point_count; 00246 accu [0] += cloud[i].x * cloud[i].x; 00247 accu [1] += cloud[i].x * cloud[i].y; 00248 accu [2] += cloud[i].x * cloud[i].z; 00249 accu [3] += cloud[i].y * cloud[i].y; // 4 00250 accu [4] += cloud[i].y * cloud[i].z; // 5 00251 accu [5] += cloud[i].z * cloud[i].z; // 8 00252 accu [6] += cloud[i].x; 00253 accu [7] += cloud[i].y; 00254 accu [8] += cloud[i].z; 00255 } 00256 00257 accu /= static_cast<float> (point_count); 00258 centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8]; 00259 centroid[3] = 0; 00260 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6]; 00261 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7]; 00262 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8]; 00263 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7]; 00264 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8]; 00265 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8]; 00266 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1); 00267 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2); 00268 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5); 00269 00270 return (static_cast<unsigned int> (point_count)); 00271 } 00272 00273 ////////////////////////////////////////////////////////////////////////////////////////////// 00274 template <typename PointT> void 00275 pcl::SamplingSurfaceNormal<PointT>::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, 00276 float &nx, float &ny, float &nz, float &curvature) 00277 { 00278 // Extract the smallest eigenvalue and its eigenvector 00279 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; 00280 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; 00281 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); 00282 00283 nx = eigen_vector [0]; 00284 ny = eigen_vector [1]; 00285 nz = eigen_vector [2]; 00286 00287 // Compute the curvature surface change 00288 float eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8); 00289 if (eig_sum != 0) 00290 curvature = fabsf (eigen_value / eig_sum); 00291 else 00292 curvature = 0; 00293 } 00294 00295 /////////////////////////////////////////////////////////////////////////////// 00296 template <typename PointT> float 00297 pcl::SamplingSurfaceNormal<PointT>::findCutVal ( 00298 const PointCloud& cloud, const int cut_dim, const int cut_index) 00299 { 00300 if (cut_dim == 0) 00301 return (cloud.points[cut_index].x); 00302 else if (cut_dim == 1) 00303 return (cloud.points[cut_index].y); 00304 else if (cut_dim == 2) 00305 return (cloud.points[cut_index].z); 00306 00307 return (0.0f); 00308 } 00309 00310 00311 #define PCL_INSTANTIATE_SamplingSurfaceNormal(T) template class PCL_EXPORTS pcl::SamplingSurfaceNormal<T>; 00312 00313 #endif // PCL_FILTERS_IMPL_NORMAL_SPACE_SAMPLE_H_