sampling_surface_normal.hpp
Go to the documentation of this file.
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 
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 
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 
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 
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 
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 
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 &centroid)
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 
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 
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_


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:32:48