Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Alexandru-Eugen Ichim 00005 * Willow Garage, Inc 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of Willow Garage, Inc. nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * $Id: normal_based_signature.hpp 3023 2011-11-01 03:42:32Z svn $ 00036 */ 00037 00038 #ifndef PCL_FEATURES_IMPL_NORMAL_BASED_SIGNATURE_H_ 00039 #define PCL_FEATURES_IMPL_NORMAL_BASED_SIGNATURE_H_ 00040 00041 #include "pcl/features/normal_based_signature.h" 00042 00043 template <typename PointT, typename PointNT, typename PointFeature> void 00044 pcl::NormalBasedSignatureEstimation<PointT, PointNT, PointFeature>::computeFeature (FeatureCloud &output) 00045 { 00046 // do a few checks before starting the computations 00047 00048 PointFeature test_feature; 00049 if (N_prime_ * M_prime_ != sizeof (test_feature.values) / sizeof (float)) 00050 { 00051 PCL_ERROR ("NormalBasedSignatureEstimation: not using the proper signature size: %u vs %u\n", N_prime_ * M_prime_, sizeof (test_feature.values) / sizeof (float)); 00052 return; 00053 } 00054 00055 std::vector<int> k_indices; 00056 std::vector<float> k_sqr_distances; 00057 00058 tree_->setInputCloud (input_); 00059 output.points.resize (indices_->size ()); 00060 00061 for (size_t index_i = 0; index_i < indices_->size (); ++index_i) 00062 { 00063 size_t point_i = (*indices_)[index_i]; 00064 Eigen::MatrixXf s_matrix (N_, M_); 00065 00066 Eigen::Vector4f center_point = input_->points[point_i].getVector4fMap (); 00067 00068 for (size_t k = 0; k < N_; ++k) 00069 { 00070 Eigen::VectorXf s_row (M_); 00071 00072 for (size_t l = 0; l < M_; ++l) 00073 { 00074 Eigen::Vector4f normal = normals_->points[point_i].getNormalVector4fMap (); 00075 Eigen::Vector4f normal_u = Eigen::Vector4f::Zero (); 00076 Eigen::Vector4f normal_v = Eigen::Vector4f::Zero (); 00077 00078 if (fabs (normal.x ()) > 0.0001f) 00079 { 00080 normal_u.x () = - normal.y () / normal.x (); 00081 normal_u.y () = 1.0f; 00082 normal_u.z () = 0.0f; 00083 normal_u.normalize (); 00084 00085 } 00086 else if (fabs (normal.y ()) > 0.0001f) 00087 { 00088 normal_u.x () = 1.0f; 00089 normal_u.y () = - normal.x () / normal.y (); 00090 normal_u.z () = 0.0f; 00091 normal_u.normalize (); 00092 } 00093 else 00094 { 00095 normal_u.x () = 0.0f; 00096 normal_u.y () = 1.0f; 00097 normal_u.z () = - normal.y () / normal.z (); 00098 } 00099 normal_v = normal.cross3 (normal_u); 00100 00101 Eigen::Vector4f zeta_point = 2.0f * (l+1) * scale_h_ / M_ * (cos (2.0f * M_PI * (k+1) / N_) * normal_u + sin (2.0f * M_PI * (k+1) / N_) * normal_v); 00102 00103 // Compute normal by using the neighbors 00104 Eigen::Vector4f zeta_point_plus_center = zeta_point + center_point; 00105 PointT zeta_point_pcl; 00106 zeta_point_pcl.x = zeta_point_plus_center.x (); zeta_point_pcl.y = zeta_point_plus_center.y (); zeta_point_pcl.z = zeta_point_plus_center.z (); 00107 00108 tree_->radiusSearch (zeta_point_pcl, search_radius_, k_indices, k_sqr_distances); 00109 00110 // Do k nearest search if there are no neighbors nearby 00111 if (k_indices.size () == 0) 00112 { 00113 k_indices.resize (5); 00114 k_sqr_distances.resize (5); 00115 tree_->nearestKSearch (zeta_point_pcl, 5, k_indices, k_sqr_distances); 00116 } 00117 00118 Eigen::Vector4f average_normal = Eigen::Vector4f::Zero (); 00119 00120 float average_normalization_factor = 0.0f; 00121 00122 // Normals weighted by 1/squared_distances 00123 for (size_t nn_i = 0; nn_i < k_indices.size (); ++nn_i) 00124 { 00125 if (k_sqr_distances[nn_i] < 1e-7f) 00126 { 00127 average_normal = normals_->points[k_indices[nn_i]].getNormalVector4fMap (); 00128 average_normalization_factor = 1.0f; 00129 break; 00130 } 00131 average_normal += normals_->points[k_indices[nn_i]].getNormalVector4fMap () / k_sqr_distances[nn_i]; 00132 average_normalization_factor += 1.0f / k_sqr_distances[nn_i]; 00133 } 00134 average_normal /= average_normalization_factor; 00135 float s = zeta_point.dot (average_normal) / zeta_point.norm (); 00136 s_row[l] = s; 00137 } 00138 00139 // do DCT on the s_matrix row-wise 00140 Eigen::VectorXf dct_row (M_); 00141 for (int m = 0; m < s_row.size (); ++m) 00142 { 00143 float Xk = 0.0f; 00144 for (int n = 0; n < s_row.size (); ++n) 00145 Xk += s_row[n] * cos (M_PI / M_ * (n + 0.5f) * k); 00146 dct_row[m] = Xk; 00147 } 00148 s_row = dct_row; 00149 s_matrix.row (k) = dct_row; 00150 } 00151 00152 // do DFT on the s_matrix column-wise 00153 Eigen::MatrixXf dft_matrix (N_, M_); 00154 for (size_t column_i = 0; column_i < M_; ++column_i) 00155 { 00156 Eigen::VectorXf dft_col (N_); 00157 for (size_t k = 0; k < N_; ++k) 00158 { 00159 float Xk_real = 0.0f, Xk_imag = 0.0f; 00160 for (size_t n = 0; n < N_; ++n) 00161 { 00162 Xk_real += s_matrix(n, column_i) * cos (2.0f * M_PI / N_ * k * n); 00163 Xk_imag += s_matrix(n, column_i) * sin (2.0f * M_PI / N_ * k * n); 00164 } 00165 dft_col[k] = sqrt (Xk_real*Xk_real + Xk_imag*Xk_imag); 00166 } 00167 dft_matrix.col (column_i) = dft_col; 00168 } 00169 00170 Eigen::MatrixXf final_matrix = dft_matrix.block (0, 0, N_prime_, M_prime_); 00171 00172 PointFeature feature_point; 00173 for (size_t i = 0; i < N_prime_; ++i) 00174 for (size_t j = 0; j < M_prime_; ++j) 00175 feature_point.values[i*M_prime_ + j] = final_matrix (i, j); 00176 00177 output.points[index_i] = feature_point; 00178 } 00179 } 00180 00181 00182 00183 #define PCL_INSTANTIATE_NormalBasedSignatureEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::NormalBasedSignatureEstimation<T,NT,OutT>; 00184 00185 00186 #endif /* PCL_FEATURES_IMPL_NORMAL_BASED_SIGNATURE_H_ */