Point Cloud Library (PCL)  1.7.1
normal_based_signature.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2011, Alexandru-Eugen Ichim
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  */
39 
40 #ifndef PCL_FEATURES_IMPL_NORMAL_BASED_SIGNATURE_H_
41 #define PCL_FEATURES_IMPL_NORMAL_BASED_SIGNATURE_H_
42 
43 #include <pcl/features/normal_based_signature.h>
44 
45 template <typename PointT, typename PointNT, typename PointFeature> void
47 {
48  // do a few checks before starting the computations
49 
50  PointFeature test_feature;
51  (void)test_feature;
52  if (N_prime_ * M_prime_ != sizeof (test_feature.values) / sizeof (float))
53  {
54  PCL_ERROR ("NormalBasedSignatureEstimation: not using the proper signature size: %u vs %u\n", N_prime_ * M_prime_, sizeof (test_feature.values) / sizeof (float));
55  return;
56  }
57 
58  std::vector<int> k_indices;
59  std::vector<float> k_sqr_distances;
60 
61  tree_->setInputCloud (input_);
62  output.points.resize (indices_->size ());
63 
64  for (size_t index_i = 0; index_i < indices_->size (); ++index_i)
65  {
66  size_t point_i = (*indices_)[index_i];
67  Eigen::MatrixXf s_matrix (N_, M_);
68 
69  Eigen::Vector4f center_point = input_->points[point_i].getVector4fMap ();
70 
71  for (size_t k = 0; k < N_; ++k)
72  {
73  Eigen::VectorXf s_row (M_);
74 
75  for (size_t l = 0; l < M_; ++l)
76  {
77  Eigen::Vector4f normal = normals_->points[point_i].getNormalVector4fMap ();
78  Eigen::Vector4f normal_u = Eigen::Vector4f::Zero ();
79  Eigen::Vector4f normal_v = Eigen::Vector4f::Zero ();
80 
81  if (fabs (normal.x ()) > 0.0001f)
82  {
83  normal_u.x () = - normal.y () / normal.x ();
84  normal_u.y () = 1.0f;
85  normal_u.z () = 0.0f;
86  normal_u.normalize ();
87 
88  }
89  else if (fabs (normal.y ()) > 0.0001f)
90  {
91  normal_u.x () = 1.0f;
92  normal_u.y () = - normal.x () / normal.y ();
93  normal_u.z () = 0.0f;
94  normal_u.normalize ();
95  }
96  else
97  {
98  normal_u.x () = 0.0f;
99  normal_u.y () = 1.0f;
100  normal_u.z () = - normal.y () / normal.z ();
101  }
102  normal_v = normal.cross3 (normal_u);
103 
104  Eigen::Vector4f zeta_point = 2.0f * static_cast<float> (l + 1) * scale_h_ / static_cast<float> (M_) *
105  (cosf (2.0f * static_cast<float> (M_PI) * static_cast<float> ((k + 1) / N_)) * normal_u +
106  sinf (2.0f * static_cast<float> (M_PI) * static_cast<float> ((k + 1) / N_)) * normal_v);
107 
108  // Compute normal by using the neighbors
109  Eigen::Vector4f zeta_point_plus_center = zeta_point + center_point;
110  PointT zeta_point_pcl;
111  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 ();
112 
113  tree_->radiusSearch (zeta_point_pcl, search_radius_, k_indices, k_sqr_distances);
114 
115  // Do k nearest search if there are no neighbors nearby
116  if (k_indices.size () == 0)
117  {
118  k_indices.resize (5);
119  k_sqr_distances.resize (5);
120  tree_->nearestKSearch (zeta_point_pcl, 5, k_indices, k_sqr_distances);
121  }
122 
123  Eigen::Vector4f average_normal = Eigen::Vector4f::Zero ();
124 
125  float average_normalization_factor = 0.0f;
126 
127  // Normals weighted by 1/squared_distances
128  for (size_t nn_i = 0; nn_i < k_indices.size (); ++nn_i)
129  {
130  if (k_sqr_distances[nn_i] < 1e-7f)
131  {
132  average_normal = normals_->points[k_indices[nn_i]].getNormalVector4fMap ();
133  average_normalization_factor = 1.0f;
134  break;
135  }
136  average_normal += normals_->points[k_indices[nn_i]].getNormalVector4fMap () / k_sqr_distances[nn_i];
137  average_normalization_factor += 1.0f / k_sqr_distances[nn_i];
138  }
139  average_normal /= average_normalization_factor;
140  float s = zeta_point.dot (average_normal) / zeta_point.norm ();
141  s_row[l] = s;
142  }
143 
144  // do DCT on the s_matrix row-wise
145  Eigen::VectorXf dct_row (M_);
146  for (int m = 0; m < s_row.size (); ++m)
147  {
148  float Xk = 0.0f;
149  for (int n = 0; n < s_row.size (); ++n)
150  Xk += static_cast<float> (s_row[n] * cos (M_PI / (static_cast<double> (M_ * n) + 0.5) * static_cast<double> (k)));
151  dct_row[m] = Xk;
152  }
153  s_row = dct_row;
154  s_matrix.row (k).matrix () = dct_row;
155  }
156 
157  // do DFT on the s_matrix column-wise
158  Eigen::MatrixXf dft_matrix (N_, M_);
159  for (size_t column_i = 0; column_i < M_; ++column_i)
160  {
161  Eigen::VectorXf dft_col (N_);
162  for (size_t k = 0; k < N_; ++k)
163  {
164  float Xk_real = 0.0f, Xk_imag = 0.0f;
165  for (size_t n = 0; n < N_; ++n)
166  {
167  Xk_real += static_cast<float> (s_matrix (n, column_i) * cos (2.0f * M_PI / static_cast<double> (N_ * k * n)));
168  Xk_imag += static_cast<float> (s_matrix (n, column_i) * sin (2.0f * M_PI / static_cast<double> (N_ * k * n)));
169  }
170  dft_col[k] = sqrtf (Xk_real*Xk_real + Xk_imag*Xk_imag);
171  }
172  dft_matrix.col (column_i).matrix () = dft_col;
173  }
174 
175  Eigen::MatrixXf final_matrix = dft_matrix.block (0, 0, N_prime_, M_prime_);
176 
177  PointFeature feature_point;
178  for (size_t i = 0; i < N_prime_; ++i)
179  for (size_t j = 0; j < M_prime_; ++j)
180  feature_point.values[i*M_prime_ + j] = final_matrix (i, j);
181 
182  output.points[index_i] = feature_point;
183  }
184 }
185 
186 
187 
188 #define PCL_INSTANTIATE_NormalBasedSignatureEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::NormalBasedSignatureEstimation<T,NT,OutT>;
189 
190 
191 #endif /* PCL_FEATURES_IMPL_NORMAL_BASED_SIGNATURE_H_ */