Point Cloud Library (PCL) 1.12.1
3dsc.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
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 */
38
39#pragma once
40
41#include <pcl/features/3dsc.h>
42
43#include <pcl/common/angles.h>
44#include <pcl/common/geometry.h>
45#include <pcl/common/point_tests.h> // for pcl::isFinite
46#include <pcl/common/utils.h>
47
48#include <cmath>
49#include <numeric> // for partial_sum
50
51//////////////////////////////////////////////////////////////////////////////////////////////
52template <typename PointInT, typename PointNT, typename PointOutT> bool
54{
56 {
57 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
58 return (false);
59 }
60
61 if (search_radius_< min_radius_)
62 {
63 PCL_ERROR ("[pcl::%s::initCompute] search_radius_ must be GREATER than min_radius_.\n", getClassName ().c_str ());
64 return (false);
65 }
66
67 // Update descriptor length
68 descriptor_length_ = elevation_bins_ * azimuth_bins_ * radius_bins_;
69
70 // Compute radial, elevation and azimuth divisions
71 float azimuth_interval = 360.0f / static_cast<float> (azimuth_bins_);
72 float elevation_interval = 180.0f / static_cast<float> (elevation_bins_);
73
74 // Reallocate divisions and volume lut
75 radii_interval_.clear ();
76 phi_divisions_.clear ();
77 theta_divisions_.clear ();
78 volume_lut_.clear ();
79
80 // Fills radii interval based on formula (1) in section 2.1 of Frome's paper
81 radii_interval_.resize (radius_bins_ + 1);
82 for (std::size_t j = 0; j < radius_bins_ + 1; j++)
83 radii_interval_[j] = static_cast<float> (std::exp (std::log (min_radius_) + ((static_cast<float> (j) / static_cast<float> (radius_bins_)) * std::log (search_radius_ / min_radius_))));
84
85 // Fill theta divisions of elevation
86 theta_divisions_.resize (elevation_bins_ + 1, elevation_interval);
87 theta_divisions_[0] = 0.f;
88 std::partial_sum(theta_divisions_.begin (), theta_divisions_.end (), theta_divisions_.begin ());
89
90 // Fill phi didvisions of elevation
91 phi_divisions_.resize (azimuth_bins_ + 1, azimuth_interval);
92 phi_divisions_[0] = 0.f;
93 std::partial_sum(phi_divisions_.begin (), phi_divisions_.end (), phi_divisions_.begin ());
94
95 // LookUp Table that contains the volume of all the bins
96 // "phi" term of the volume integral
97 // "integr_phi" has always the same value so we compute it only one time
98 float integr_phi = pcl::deg2rad (phi_divisions_[1]) - pcl::deg2rad (phi_divisions_[0]);
99 // exponential to compute the cube root using pow
100 float e = 1.0f / 3.0f;
101 // Resize volume look up table
102 volume_lut_.resize (radius_bins_ * elevation_bins_ * azimuth_bins_);
103 // Fill volumes look up table
104 for (std::size_t j = 0; j < radius_bins_; j++)
105 {
106 // "r" term of the volume integral
107 float integr_r = (radii_interval_[j+1] * radii_interval_[j+1] * radii_interval_[j+1] / 3.0f) - (radii_interval_[j] * radii_interval_[j] * radii_interval_[j] / 3.0f);
108
109 for (std::size_t k = 0; k < elevation_bins_; k++)
110 {
111 // "theta" term of the volume integral
112 float integr_theta = std::cos (pcl::deg2rad (theta_divisions_[k])) - std::cos (pcl::deg2rad (theta_divisions_[k+1]));
113 // Volume
114 float V = integr_phi * integr_theta * integr_r;
115 // Compute cube root of the computed volume commented for performance but left
116 // here for clarity
117 // float cbrt = pow(V, e);
118 // cbrt = 1 / cbrt;
119
120 for (std::size_t l = 0; l < azimuth_bins_; l++)
121 {
122 // Store in lut 1/cbrt
123 //volume_lut_[ (l*elevation_bins_*radius_bins_) + k*radius_bins_ + j ] = cbrt;
124 volume_lut_[(l*elevation_bins_*radius_bins_) + k*radius_bins_ + j] = 1.0f / powf (V, e);
125 }
126 }
127 }
128 return (true);
129}
130
131//////////////////////////////////////////////////////////////////////////////////////////////
132template <typename PointInT, typename PointNT, typename PointOutT> bool
134 std::size_t index, const pcl::PointCloud<PointNT> &normals, float rf[9], std::vector<float> &desc)
135{
136 // The RF is formed as this x_axis | y_axis | normal
137 Eigen::Map<Eigen::Vector3f> x_axis (rf);
138 Eigen::Map<Eigen::Vector3f> y_axis (rf + 3);
139 Eigen::Map<Eigen::Vector3f> normal (rf + 6);
140
141 // Find every point within specified search_radius_
142 pcl::Indices nn_indices;
143 std::vector<float> nn_dists;
144 const std::size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
145 if (neighb_cnt == 0)
146 {
147 std::fill (desc.begin (), desc.end (), std::numeric_limits<float>::quiet_NaN ());
148 std::fill (rf, rf + 9, 0.f);
149 return (false);
150 }
151
152 const auto minDistanceIt = std::min_element(nn_dists.begin (), nn_dists.end ());
153 const auto minIndex = nn_indices[std::distance (nn_dists.begin (), minDistanceIt)];
154
155 // Get origin point
156 Vector3fMapConst origin = (*input_)[(*indices_)[index]].getVector3fMap ();
157 // Get origin normal
158 // Use pre-computed normals
159 if (!pcl::isNormalFinite(normals[minIndex]))
160 {
161 std::fill (desc.begin (), desc.end (), std::numeric_limits<float>::quiet_NaN ());
162 std::fill (rf, rf + 9, 0.f);
163 return (false);
164 }
165 normal = normals[minIndex].getNormalVector3fMap ();
166
167 // Compute and store the RF direction
168 x_axis[0] = rnd ();
169 x_axis[1] = rnd ();
170 x_axis[2] = rnd ();
171 if (!pcl::utils::equal (normal[2], 0.0f))
172 x_axis[2] = - (normal[0]*x_axis[0] + normal[1]*x_axis[1]) / normal[2];
173 else if (!pcl::utils::equal (normal[1], 0.0f))
174 x_axis[1] = - (normal[0]*x_axis[0] + normal[2]*x_axis[2]) / normal[1];
175 else if (!pcl::utils::equal (normal[0], 0.0f))
176 x_axis[0] = - (normal[1]*x_axis[1] + normal[2]*x_axis[2]) / normal[0];
177
178 x_axis.normalize ();
179
180 // Check if the computed x axis is orthogonal to the normal
181 assert (pcl::utils::equal (x_axis[0]*normal[0] + x_axis[1]*normal[1] + x_axis[2]*normal[2], 0.0f, 1E-6f));
182
183 // Store the 3rd frame vector
184 y_axis.matrix () = normal.cross (x_axis);
185
186 // For each point within radius
187 for (std::size_t ne = 0; ne < neighb_cnt; ne++)
188 {
189 if (pcl::utils::equal (nn_dists[ne], 0.0f))
190 continue;
191 // Get neighbours coordinates
192 Eigen::Vector3f neighbour = (*surface_)[nn_indices[ne]].getVector3fMap ();
193
194 /// ----- Compute current neighbour polar coordinates -----
195 /// Get distance between the neighbour and the origin
196 float r = std::sqrt (nn_dists[ne]);
197
198 /// Project point into the tangent plane
199 Eigen::Vector3f proj;
200 pcl::geometry::project (neighbour, origin, normal, proj);
201 proj -= origin;
202
203 /// Normalize to compute the dot product
204 proj.normalize ();
205
206 /// Compute the angle between the projection and the x axis in the interval [0,360]
207 Eigen::Vector3f cross = x_axis.cross (proj);
208 float phi = pcl::rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
209 phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
210 /// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
211 Eigen::Vector3f no = neighbour - origin;
212 no.normalize ();
213 float theta = normal.dot (no);
214 theta = pcl::rad2deg (std::acos (std::min (1.0f, std::max (-1.0f, theta))));
215
216 // Compute the Bin(j, k, l) coordinates of current neighbour
217 const auto rad_min = std::lower_bound(std::next (radii_interval_.cbegin ()), radii_interval_.cend (), r);
218 const auto theta_min = std::lower_bound(std::next (theta_divisions_.cbegin ()), theta_divisions_.cend (), theta);
219 const auto phi_min = std::lower_bound(std::next (phi_divisions_.cbegin ()), phi_divisions_.cend (), phi);
220
221 // Bin (j, k, l)
222 const auto j = std::distance(radii_interval_.cbegin (), std::prev(rad_min));
223 const auto k = std::distance(theta_divisions_.cbegin (), std::prev(theta_min));
224 const auto l = std::distance(phi_divisions_.cbegin (), std::prev(phi_min));
225
226 // Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
227 pcl::Indices neighbour_indices;
228 std::vector<float> neighbour_distances;
229 int point_density = searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_distances);
230 // point_density is NOT always bigger than 0 (on error, searchForNeighbors returns 0), so we must check for that
231 if (point_density == 0)
232 continue;
233
234 float w = (1.0f / static_cast<float> (point_density)) *
235 volume_lut_[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j];
236
237 assert (w >= 0.0);
238 if (w == std::numeric_limits<float>::infinity ())
239 PCL_ERROR ("Shape Context Error INF!\n");
240 if (std::isnan(w))
241 PCL_ERROR ("Shape Context Error IND!\n");
242 /// Accumulate w into correspondent Bin(j,k,l)
243 desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
244
245 assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
246 } // end for each neighbour
247
248 // 3DSC does not define a repeatable local RF, we set it to zero to signal it to the user
249 std::fill (rf, rf + 9, 0);
250 return (true);
251}
252
253//////////////////////////////////////////////////////////////////////////////////////////////
254template <typename PointInT, typename PointNT, typename PointOutT> void
256{
257 assert (descriptor_length_ == 1980);
258
259 output.is_dense = true;
260 // Iterate over all points and compute the descriptors
261 for (std::size_t point_index = 0; point_index < indices_->size (); point_index++)
262 {
263 //output[point_index].descriptor.resize (descriptor_length_);
264
265 // If the point is not finite, set the descriptor to NaN and continue
266 if (!isFinite ((*input_)[(*indices_)[point_index]]))
267 {
268 std::fill (output[point_index].descriptor, output[point_index].descriptor + descriptor_length_,
269 std::numeric_limits<float>::quiet_NaN ());
270 std::fill (output[point_index].rf, output[point_index].rf + 9, 0);
271 output.is_dense = false;
272 continue;
273 }
274
275 std::vector<float> descriptor (descriptor_length_);
276 if (!computePoint (point_index, *normals_, output[point_index].rf, descriptor))
277 output.is_dense = false;
278 std::copy (descriptor.begin (), descriptor.end (), output[point_index].descriptor);
279 }
280}
281
282#define PCL_INSTANTIATE_ShapeContext3DEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::ShapeContext3DEstimation<T,NT,OutT>;
283
Define standard C methods to do angle calculations.
bool computePoint(std::size_t index, const pcl::PointCloud< PointNT > &normals, float rf[9], std::vector< float > &desc)
Estimate a descriptor for a given point.
Definition: 3dsc.hpp:133
bool initCompute() override
Initialize computation by allocating all the intervals and the volume lookup table.
Definition: 3dsc.hpp:53
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: 3dsc.h:88
void computeFeature(PointCloudOut &output) override
Estimate the actual feature.
Definition: 3dsc.hpp:255
Defines some geometrical functions and utility functions.
float deg2rad(float alpha)
Convert an angle from degrees to radians.
Definition: angles.hpp:67
float rad2deg(float alpha)
Convert an angle from radians to degrees.
Definition: angles.hpp:61
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
void project(const PointT &point, const PointT &plane_origin, const NormalT &plane_normal, PointT &projected)
Definition: geometry.h:81
bool equal(T val1, T val2, T eps=std::numeric_limits< T >::min())
Check if val1 and val2 are equal to an epsilon extent.
Definition: utils.h:55
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:55
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
constexpr bool isNormalFinite(const PointT &) noexcept
Definition: point_tests.h:131