Point Cloud Library (PCL)  1.14.1-dev
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  if (N_prime_ * M_prime_ != sizeof (test_feature.values) / sizeof (float))
52  {
53  PCL_ERROR ("NormalBasedSignatureEstimation: not using the proper signature size: %u vs %u\n", N_prime_ * M_prime_, sizeof (test_feature.values) / sizeof (float));
54  return;
55  }
56 
57  pcl::Indices k_indices;
58  std::vector<float> k_sqr_distances;
59 
60  tree_->setInputCloud (input_);
61  output.resize (indices_->size ());
62 
63  for (std::size_t index_i = 0; index_i < indices_->size (); ++index_i)
64  {
65  std::size_t point_i = (*indices_)[index_i];
66  Eigen::MatrixXf s_matrix (N_, M_);
67 
68  Eigen::Vector4f center_point = (*input_)[point_i].getVector4fMap ();
69 
70  for (std::size_t k = 0; k < N_; ++k)
71  {
72  Eigen::VectorXf s_row (M_);
73 
74  for (std::size_t l = 0; l < M_; ++l)
75  {
76  Eigen::Vector4f normal = (*normals_)[point_i].getNormalVector4fMap ();
77  Eigen::Vector4f normal_u = Eigen::Vector4f::Zero ();
78  Eigen::Vector4f normal_v = Eigen::Vector4f::Zero ();
79 
80  if (std::abs (normal.x ()) > 0.0001f)
81  {
82  normal_u.x () = - normal.y () / normal.x ();
83  normal_u.y () = 1.0f;
84  normal_u.z () = 0.0f;
85  normal_u.normalize ();
86 
87  }
88  else if (std::abs (normal.y ()) > 0.0001f)
89  {
90  normal_u.x () = 1.0f;
91  normal_u.y () = - normal.x () / normal.y ();
92  normal_u.z () = 0.0f;
93  normal_u.normalize ();
94  }
95  else
96  {
97  normal_u.x () = 0.0f;
98  normal_u.y () = 1.0f;
99  normal_u.z () = - normal.y () / normal.z ();
100  }
101  normal_v = normal.cross3 (normal_u);
102 
103  Eigen::Vector4f zeta_point = 2.0f * static_cast<float> (l + 1) * scale_h_ / static_cast<float> (M_) *
104  (std::cos (2.0f * static_cast<float> (M_PI) * static_cast<float> ((k + 1) / N_)) * normal_u +
105  sinf (2.0f * static_cast<float> (M_PI) * static_cast<float> ((k + 1) / N_)) * normal_v);
106 
107  // Compute normal by using the neighbors
108  Eigen::Vector4f zeta_point_plus_center = zeta_point + center_point;
109  PointT zeta_point_pcl;
110  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 ();
111 
112  tree_->radiusSearch (zeta_point_pcl, search_radius_, k_indices, k_sqr_distances);
113 
114  // Do k nearest search if there are no neighbors nearby
115  if (k_indices.empty ())
116  {
117  k_indices.resize (5);
118  k_sqr_distances.resize (5);
119  tree_->nearestKSearch (zeta_point_pcl, 5, k_indices, k_sqr_distances);
120  }
121 
122  Eigen::Vector4f average_normal = Eigen::Vector4f::Zero ();
123 
124  float average_normalization_factor = 0.0f;
125 
126  // Normals weighted by 1/squared_distances
127  for (std::size_t nn_i = 0; nn_i < k_indices.size (); ++nn_i)
128  {
129  if (k_sqr_distances[nn_i] < 1e-7f)
130  {
131  average_normal = (*normals_)[k_indices[nn_i]].getNormalVector4fMap ();
132  average_normalization_factor = 1.0f;
133  break;
134  }
135  average_normal += (*normals_)[k_indices[nn_i]].getNormalVector4fMap () / k_sqr_distances[nn_i];
136  average_normalization_factor += 1.0f / k_sqr_distances[nn_i];
137  }
138  average_normal /= average_normalization_factor;
139  float s = zeta_point.dot (average_normal) / zeta_point.norm ();
140  s_row[l] = s;
141  }
142 
143  // do DCT on the s_matrix row-wise
144  Eigen::VectorXf dct_row (M_);
145  for (Eigen::Index m = 0; m < s_row.size (); ++m)
146  {
147  float Xk = 0.0f;
148  for (Eigen::Index n = 0; n < s_row.size (); ++n)
149  Xk += static_cast<float> (s_row[n] * std::cos (M_PI / (static_cast<double> (M_ * n) + 0.5) * static_cast<double> (k)));
150  dct_row[m] = Xk;
151  }
152  s_row = dct_row;
153  s_matrix.row (k).matrix () = dct_row;
154  }
155 
156  // do DFT on the s_matrix column-wise
157  Eigen::MatrixXf dft_matrix (N_, M_);
158  for (std::size_t column_i = 0; column_i < M_; ++column_i)
159  {
160  Eigen::VectorXf dft_col (N_);
161  for (std::size_t k = 0; k < N_; ++k)
162  {
163  float Xk_real = 0.0f, Xk_imag = 0.0f;
164  for (std::size_t n = 0; n < N_; ++n)
165  {
166  Xk_real += static_cast<float> (s_matrix (n, column_i) * std::cos (2.0f * M_PI / static_cast<double> (N_ * k * n)));
167  Xk_imag += static_cast<float> (s_matrix (n, column_i) * sin (2.0f * M_PI / static_cast<double> (N_ * k * n)));
168  }
169  dft_col[k] = std::sqrt (Xk_real*Xk_real + Xk_imag*Xk_imag);
170  }
171  dft_matrix.col (column_i).matrix () = dft_col;
172  }
173 
174  Eigen::MatrixXf final_matrix = dft_matrix.block (0, 0, N_prime_, M_prime_);
175 
176  PointFeature feature_point;
177  for (std::size_t i = 0; i < N_prime_; ++i)
178  for (std::size_t j = 0; j < M_prime_; ++j)
179  feature_point.values[i*M_prime_ + j] = final_matrix (i, j);
180 
181  output[index_i] = feature_point;
182  }
183 }
184 
185 
186 
187 #define PCL_INSTANTIATE_NormalBasedSignatureEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::NormalBasedSignatureEstimation<T,NT,OutT>;
188 
189 
190 #endif /* PCL_FEATURES_IMPL_NORMAL_BASED_SIGNATURE_H_ */
void computeFeature(FeatureCloud &output) override
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
#define M_PI
Definition: pcl_macros.h:201
A point structure representing Euclidean xyz coordinates, and the RGB color.