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