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 * $Id$
38 *
39 */
40
41 #pragma once
42
43 #include <numeric> // for partial_sum
44 #include <pcl/features/usc.h>
45 #include <pcl/features/shot_lrf.h>
46 #include <pcl/common/angles.h>
47 #include <pcl/common/geometry.h>
48 #include <pcl/common/point_tests.h> // for pcl::isFinite
49 #include <pcl/common/utils.h>
50
51
52 //////////////////////////////////////////////////////////////////////////////////////////////
53 template <typename PointInT, typename PointOutT, typename PointRFT> bool
initCompute()54 pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::initCompute ()
55 {
56 if (!Feature<PointInT, PointOutT>::initCompute ())
57 {
58 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
59 return (false);
60 }
61
62 // Default LRF estimation alg: SHOTLocalReferenceFrameEstimation
63 typename SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>::Ptr lrf_estimator(new SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>());
64 lrf_estimator->setRadiusSearch (local_radius_);
65 lrf_estimator->setInputCloud (input_);
66 lrf_estimator->setIndices (indices_);
67 if (!fake_surface_)
68 lrf_estimator->setSearchSurface(surface_);
69
70 if (!FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames (indices_->size (), lrf_estimator))
71 {
72 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
73 return (false);
74 }
75
76 if (search_radius_< min_radius_)
77 {
78 PCL_ERROR ("[pcl::%s::initCompute] search_radius_ must be GREATER than min_radius_.\n", getClassName ().c_str ());
79 return (false);
80 }
81
82 // Update descriptor length
83 descriptor_length_ = elevation_bins_ * azimuth_bins_ * radius_bins_;
84
85 // Compute radial, elevation and azimuth divisions
86 float azimuth_interval = 360.0f / static_cast<float> (azimuth_bins_);
87 float elevation_interval = 180.0f / static_cast<float> (elevation_bins_);
88
89 // Reallocate divisions and volume lut
90 radii_interval_.clear ();
91 phi_divisions_.clear ();
92 theta_divisions_.clear ();
93 volume_lut_.clear ();
94
95 // Fills radii interval based on formula (1) in section 2.1 of Frome's paper
96 radii_interval_.resize (radius_bins_ + 1);
97 for (std::size_t j = 0; j < radius_bins_ + 1; j++)
98 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_))));
99
100 // Fill theta divisions of elevation
101 theta_divisions_.resize (elevation_bins_ + 1, elevation_interval);
102 theta_divisions_[0] = 0;
103 std::partial_sum(theta_divisions_.begin (), theta_divisions_.end (), theta_divisions_.begin ());
104
105 // Fill phi divisions of elevation
106 phi_divisions_.resize (azimuth_bins_ + 1, azimuth_interval);
107 phi_divisions_[0] = 0;
108 std::partial_sum(phi_divisions_.begin (), phi_divisions_.end (), phi_divisions_.begin ());
109
110 // LookUp Table that contains the volume of all the bins
111 // "phi" term of the volume integral
112 // "integr_phi" has always the same value so we compute it only one time
113 float integr_phi = pcl::deg2rad (phi_divisions_[1]) - pcl::deg2rad (phi_divisions_[0]);
114 // exponential to compute the cube root using pow
115 float e = 1.0f / 3.0f;
116 // Resize volume look up table
117 volume_lut_.resize (radius_bins_ * elevation_bins_ * azimuth_bins_);
118 // Fill volumes look up table
119 for (std::size_t j = 0; j < radius_bins_; j++)
120 {
121 // "r" term of the volume integral
122 float integr_r = (radii_interval_[j+1]*radii_interval_[j+1]*radii_interval_[j+1] / 3) - (radii_interval_[j]*radii_interval_[j]*radii_interval_[j]/ 3);
123
124 for (std::size_t k = 0; k < elevation_bins_; k++)
125 {
126 // "theta" term of the volume integral
127 float integr_theta = std::cos (deg2rad (theta_divisions_[k])) - std::cos (deg2rad (theta_divisions_[k+1]));
128 // Volume
129 float V = integr_phi * integr_theta * integr_r;
130 // Compute cube root of the computed volume commented for performance but left
131 // here for clarity
132 // float cbrt = pow(V, e);
133 // cbrt = 1 / cbrt;
134
135 for (std::size_t l = 0; l < azimuth_bins_; l++)
136 // Store in lut 1/cbrt
137 //volume_lut_[ (l*elevation_bins_*radius_bins_) + k*radius_bins_ + j ] = cbrt;
138 volume_lut_[(l*elevation_bins_*radius_bins_) + k*radius_bins_ + j] = 1.0f / powf (V, e);
139 }
140 }
141 return (true);
142 }
143
144 //////////////////////////////////////////////////////////////////////////////////////////////
145 template <typename PointInT, typename PointOutT, typename PointRFT> void
computePointDescriptor(std::size_t index,std::vector<float> & desc)146 pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computePointDescriptor (std::size_t index, /*float rf[9],*/ std::vector<float> &desc)
147 {
148 pcl::Vector3fMapConst origin = (*input_)[(*indices_)[index]].getVector3fMap ();
149
150 const Eigen::Vector3f x_axis ((*frames_)[index].x_axis[0],
151 (*frames_)[index].x_axis[1],
152 (*frames_)[index].x_axis[2]);
153 //const Eigen::Vector3f& y_axis = (*frames_)[index].y_axis.getNormalVector3fMap ();
154 const Eigen::Vector3f normal ((*frames_)[index].z_axis[0],
155 (*frames_)[index].z_axis[1],
156 (*frames_)[index].z_axis[2]);
157
158 // Find every point within specified search_radius_
159 pcl::Indices nn_indices;
160 std::vector<float> nn_dists;
161 const std::size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
162 // For each point within radius
163 for (std::size_t ne = 0; ne < neighb_cnt; ne++)
164 {
165 if (pcl::utils::equal(nn_dists[ne], 0.0f))
166 continue;
167 // Get neighbours coordinates
168 Eigen::Vector3f neighbour = (*surface_)[nn_indices[ne]].getVector3fMap ();
169
170 // ----- Compute current neighbour polar coordinates -----
171
172 // Get distance between the neighbour and the origin
173 float r = ::sqrt (nn_dists[ne]);
174
175 // Project point into the tangent plane
176 Eigen::Vector3f proj;
177 pcl::geometry::project (neighbour, origin, normal, proj);
178 proj -= origin;
179
180 // Normalize to compute the dot product
181 proj.normalize ();
182
183 // Compute the angle between the projection and the x axis in the interval [0,360]
184 Eigen::Vector3f cross = x_axis.cross (proj);
185 float phi = rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
186 phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
187 /// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
188 Eigen::Vector3f no = neighbour - origin;
189 no.normalize ();
190 float theta = normal.dot (no);
191 theta = pcl::rad2deg (std::acos (std::min (1.0f, std::max (-1.0f, theta))));
192
193 /// Compute the Bin(j, k, l) coordinates of current neighbour
194 const auto rad_min = std::lower_bound(std::next (radii_interval_.cbegin ()), radii_interval_.cend (), r);
195 const auto theta_min = std::lower_bound(std::next (theta_divisions_.cbegin ()), theta_divisions_.cend (), theta);
196 const auto phi_min = std::lower_bound(std::next (phi_divisions_.cbegin ()), phi_divisions_.cend (), phi);
197
198 /// Bin (j, k, l)
199 const auto j = std::distance(radii_interval_.cbegin (), std::prev(rad_min));
200 const auto k = std::distance(theta_divisions_.cbegin (), std::prev(theta_min));
201 const auto l = std::distance(phi_divisions_.cbegin (), std::prev(phi_min));
202
203 /// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
204 pcl::Indices neighbour_indices;
205 std::vector<float> neighbour_didtances;
206 float point_density = static_cast<float> (searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances));
207 /// point_density is always bigger than 0 because FindPointsWithinRadius returns at least the point itself
208 float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) +
209 (k*radius_bins_) +
210 j];
211
212 assert (w >= 0.0);
213 if (w == std::numeric_limits<float>::infinity ())
214 PCL_ERROR ("Shape Context Error INF!\n");
215 if (std::isnan(w))
216 PCL_ERROR ("Shape Context Error IND!\n");
217 /// Accumulate w into correspondent Bin(j,k,l)
218 desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
219
220 assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
221 } // end for each neighbour
222 }
223
224 //////////////////////////////////////////////////////////////////////////////////////////////
225 template <typename PointInT, typename PointOutT, typename PointRFT> void
computeFeature(PointCloudOut & output)226 pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computeFeature (PointCloudOut &output)
227 {
228 assert (descriptor_length_ == 1960);
229
230 output.is_dense = true;
231
232 for (std::size_t point_index = 0; point_index < indices_->size (); ++point_index)
233 {
234 //output[point_index].descriptor.resize (descriptor_length_);
235
236 // If the point is not finite, set the descriptor to NaN and continue
237 const PointRFT& current_frame = (*frames_)[point_index];
238 if (!isFinite ((*input_)[(*indices_)[point_index]]) ||
239 !std::isfinite (current_frame.x_axis[0]) ||
240 !std::isfinite (current_frame.y_axis[0]) ||
241 !std::isfinite (current_frame.z_axis[0]) )
242 {
243 std::fill (output[point_index].descriptor, output[point_index].descriptor + descriptor_length_,
244 std::numeric_limits<float>::quiet_NaN ());
245 std::fill (output[point_index].rf, output[point_index].rf + 9, 0);
246 output.is_dense = false;
247 continue;
248 }
249
250 for (int d = 0; d < 3; ++d)
251 {
252 output[point_index].rf[0 + d] = current_frame.x_axis[d];
253 output[point_index].rf[3 + d] = current_frame.y_axis[d];
254 output[point_index].rf[6 + d] = current_frame.z_axis[d];
255 }
256
257 std::vector<float> descriptor (descriptor_length_);
258 computePointDescriptor (point_index, descriptor);
259 std::copy (descriptor.begin (), descriptor.end (), output[point_index].descriptor);
260 }
261 }
262
263 #define PCL_INSTANTIATE_UniqueShapeContext(T,OutT,RFT) template class PCL_EXPORTS pcl::UniqueShapeContext<T,OutT,RFT>;
264
265