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: principal_curvatures.hpp 3176 2011-11-17 14:32:07Z rusu $ 00035 * 00036 */ 00037 00038 #ifndef PCL_FEATURES_IMPL_PRINCIPAL_CURVATURES_H_ 00039 #define PCL_FEATURES_IMPL_PRINCIPAL_CURVATURES_H_ 00040 00041 #include "pcl/features/principal_curvatures.h" 00042 00044 template <typename PointInT, typename PointNT, typename PointOutT> void 00045 pcl::PrincipalCurvaturesEstimation<PointInT, PointNT, PointOutT>::computePointPrincipalCurvatures ( 00046 const pcl::PointCloud<PointNT> &normals, int p_idx, const std::vector<int> &indices, 00047 float &pcx, float &pcy, float &pcz, float &pc1, float &pc2) 00048 { 00049 EIGEN_ALIGN16 Eigen::Matrix3f I = Eigen::Matrix3f::Identity (); 00050 Eigen::Vector3f n_idx (normals.points[p_idx].normal[0], normals.points[p_idx].normal[1], normals.points[p_idx].normal[2]); 00051 EIGEN_ALIGN16 Eigen::Matrix3f M = I - n_idx * n_idx.transpose (); // projection matrix (into tangent plane) 00052 00053 // Project normals into the tangent plane 00054 Eigen::Vector3f normal; 00055 projected_normals_.resize (indices.size ()); 00056 xyz_centroid_.setZero (); 00057 for (size_t idx = 0; idx < indices.size(); ++idx) 00058 { 00059 normal[0] = normals.points[indices[idx]].normal[0]; 00060 normal[1] = normals.points[indices[idx]].normal[1]; 00061 normal[2] = normals.points[indices[idx]].normal[2]; 00062 00063 projected_normals_[idx] = M * normal; 00064 xyz_centroid_ += projected_normals_[idx]; 00065 } 00066 00067 // Estimate the XYZ centroid 00068 xyz_centroid_ /= indices.size (); 00069 00070 // Initialize to 0 00071 covariance_matrix_.setZero (); 00072 00073 double demean_xy, demean_xz, demean_yz; 00074 // For each point in the cloud 00075 for (size_t idx = 0; idx < indices.size (); ++idx) 00076 { 00077 demean_ = projected_normals_[idx] - xyz_centroid_; 00078 00079 demean_xy = demean_[0] * demean_[1]; 00080 demean_xz = demean_[0] * demean_[2]; 00081 demean_yz = demean_[1] * demean_[2]; 00082 00083 covariance_matrix_(0, 0) += demean_[0] * demean_[0]; 00084 covariance_matrix_(0, 1) += demean_xy; 00085 covariance_matrix_(0, 2) += demean_xz; 00086 00087 covariance_matrix_(1, 0) += demean_xy; 00088 covariance_matrix_(1, 1) += demean_[1] * demean_[1]; 00089 covariance_matrix_(1, 2) += demean_yz; 00090 00091 covariance_matrix_(2, 0) += demean_xz; 00092 covariance_matrix_(2, 1) += demean_yz; 00093 covariance_matrix_(2, 2) += demean_[2] * demean_[2]; 00094 } 00095 00096 // Extract the eigenvalues and eigenvectors 00097 //Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> ei_symm (covariance_matrix_); 00098 //eigenvalues_ = ei_symm.eigenvalues (); 00099 //eigenvectors_ = ei_symm.eigenvectors (); 00100 pcl::eigen33 (covariance_matrix_, eigenvectors_, eigenvalues_); 00101 00102 pcx = eigenvectors_ (0, 2); 00103 pcy = eigenvectors_ (1, 2); 00104 pcz = eigenvectors_ (2, 2); 00105 float indices_size = 1.0f / indices.size (); 00106 pc1 = eigenvalues_ (2) * indices_size; 00107 pc2 = eigenvalues_ (1) * indices_size; 00108 } 00109 00110 00112 template <typename PointInT, typename PointNT, typename PointOutT> void 00113 pcl::PrincipalCurvaturesEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) 00114 { 00115 // Allocate enough space to hold the results 00116 // \note This resize is irrelevant for a radiusSearch (). 00117 std::vector<int> nn_indices (k_); 00118 std::vector<float> nn_dists (k_); 00119 00120 // Iterating over the entire index vector 00121 for (size_t idx = 0; idx < indices_->size (); ++idx) 00122 { 00123 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists); 00124 00125 // Estimate the principal curvatures at each patch 00126 computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices, 00127 output.points[idx].principal_curvature[0], output.points[idx].principal_curvature[1], output.points[idx].principal_curvature[2], 00128 output.points[idx].pc1, output.points[idx].pc2); 00129 } 00130 } 00131 00132 #define PCL_INSTANTIATE_PrincipalCurvaturesEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PrincipalCurvaturesEstimation<T,NT,OutT>; 00133 00134 #endif // PCL_FEATURES_IMPL_PRINCIPAL_CURVATURES_H_