Point Cloud Library (PCL) 1.12.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
45template <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
Abstract feature estimation method.
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.