Point Cloud Library (PCL)  1.14.1-dev
rift.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  * $Id$
38  *
39  */
40 
41 #ifndef PCL_FEATURES_IMPL_RIFT_H_
42 #define PCL_FEATURES_IMPL_RIFT_H_
43 
44 #include <pcl/features/rift.h>
45 
46 //////////////////////////////////////////////////////////////////////////////////////////////
47 template <typename PointInT, typename GradientT, typename PointOutT> void
49  const PointCloudIn &cloud, const PointCloudGradient &gradient,
50  int p_idx, float radius, const pcl::Indices &indices,
51  const std::vector<float> &sqr_distances, Eigen::MatrixXf &rift_descriptor)
52 {
53  if (indices.empty ())
54  {
55  PCL_ERROR ("[pcl::RIFTEstimation] Null indices points passed!\n");
56  return;
57  }
58 
59  // Determine the number of bins to use based on the size of rift_descriptor
60  int nr_distance_bins = static_cast<int> (rift_descriptor.rows ());
61  int nr_gradient_bins = static_cast<int> (rift_descriptor.cols ());
62 
63  // Get the center point
64  pcl::Vector3fMapConst p0 = cloud[p_idx].getVector3fMap ();
65 
66  // Compute the RIFT descriptor
67  rift_descriptor.setZero ();
68  for (std::size_t idx = 0; idx < indices.size (); ++idx)
69  {
70  // Compute the gradient magnitude and orientation (relative to the center point)
71  pcl::Vector3fMapConst point = cloud[indices[idx]].getVector3fMap ();
72  Eigen::Map<const Eigen::Vector3f> gradient_vector (& (gradient[indices[idx]].gradient[0]));
73 
74  float gradient_magnitude = gradient_vector.norm ();
75  float gradient_angle_from_center = std::acos (gradient_vector.dot ((point - p0).normalized ()) / gradient_magnitude);
76  if (!std::isfinite (gradient_angle_from_center))
77  gradient_angle_from_center = 0.0;
78 
79  // Normalize distance and angle values to: 0.0 <= d,g < nr_distances_bins,nr_gradient_bins
80  const float eps = std::numeric_limits<float>::epsilon ();
81  float d = static_cast<float> (nr_distance_bins) * std::sqrt (sqr_distances[idx]) / (radius + eps);
82  float g = static_cast<float> (nr_gradient_bins) * gradient_angle_from_center / (static_cast<float> (M_PI) + eps);
83 
84  // Compute the bin indices that need to be updated
85  int d_idx_min = (std::max)(static_cast<int> (std::ceil (d - 1)), 0);
86  int d_idx_max = (std::min)(static_cast<int> (std::floor (d + 1)), nr_distance_bins - 1);
87  int g_idx_min = static_cast<int> (std::ceil (g - 1));
88  int g_idx_max = static_cast<int> (std::floor (g + 1));
89 
90  // Update the appropriate bins of the histogram
91  for (int g_idx = g_idx_min; g_idx <= g_idx_max; ++g_idx)
92  {
93  // Because gradient orientation is cyclical, out-of-bounds values must wrap around
94  int g_idx_wrapped = ((g_idx + nr_gradient_bins) % nr_gradient_bins);
95 
96  for (int d_idx = d_idx_min; d_idx <= d_idx_max; ++d_idx)
97  {
98  // To avoid boundary effects, use linear interpolation when updating each bin
99  float w = (1.0f - std::abs (d - static_cast<float> (d_idx))) * (1.0f - std::abs (g - static_cast<float> (g_idx)));
100 
101  rift_descriptor (d_idx, g_idx_wrapped) += w * gradient_magnitude;
102  }
103  }
104  }
105 
106  // Normalize the RIFT descriptor to unit magnitude
107  rift_descriptor.normalize ();
108 }
109 
110 
111 //////////////////////////////////////////////////////////////////////////////////////////////
112 template <typename PointInT, typename GradientT, typename PointOutT> void
114 {
115  // Make sure a search radius is set
116  if (search_radius_ == 0.0)
117  {
118  PCL_ERROR ("[pcl::%s::computeFeature] The search radius must be set before computing the feature!\n",
119  getClassName ().c_str ());
120  output.width = output.height = 0;
121  output.clear ();
122  return;
123  }
124 
125  // Make sure the RIFT descriptor has valid dimensions
126  if (nr_gradient_bins_ <= 0)
127  {
128  PCL_ERROR ("[pcl::%s::computeFeature] The number of gradient bins must be greater than zero!\n",
129  getClassName ().c_str ());
130  output.width = output.height = 0;
131  output.clear ();
132  return;
133  }
134  if (nr_distance_bins_ <= 0)
135  {
136  PCL_ERROR ("[pcl::%s::computeFeature] The number of distance bins must be greater than zero!\n",
137  getClassName ().c_str ());
138  output.width = output.height = 0;
139  output.clear ();
140  return;
141  }
142 
143  // Check for valid input gradient
144  if (!gradient_)
145  {
146  PCL_ERROR ("[pcl::%s::computeFeature] No input gradient was given!\n", getClassName ().c_str ());
147  output.width = output.height = 0;
148  output.clear ();
149  return;
150  }
151  if (gradient_->size () != surface_->size ())
152  {
153  PCL_ERROR ("[pcl::%s::computeFeature] ", getClassName ().c_str ());
154  PCL_ERROR ("The number of points in the input dataset differs from the number of points in the gradient!\n");
155  output.width = output.height = 0;
156  output.clear ();
157  return;
158  }
159 
160  Eigen::MatrixXf rift_descriptor (nr_distance_bins_, nr_gradient_bins_);
161  pcl::Indices nn_indices;
162  std::vector<float> nn_dist_sqr;
163 
164  // Iterating over the entire index vector
165  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
166  {
167  // Find neighbors within the search radius
168  tree_->radiusSearch ((*indices_)[idx], search_radius_, nn_indices, nn_dist_sqr);
169 
170  // Compute the RIFT descriptor
171  computeRIFT (*surface_, *gradient_, (*indices_)[idx], static_cast<float> (search_radius_), nn_indices, nn_dist_sqr, rift_descriptor);
172 
173  // Default layout is column major, copy elementwise
174  std::copy (rift_descriptor.data (), rift_descriptor.data () + rift_descriptor.size (), output[idx].histogram);
175  }
176 }
177 
178 #define PCL_INSTANTIATE_RIFTEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::RIFTEstimation<T,NT,OutT>;
179 
180 #endif // PCL_FEATURES_IMPL_RIFT_H_
181 
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:885
void computeFeature(PointCloudOut &output) override
Estimate the Rotation Invariant Feature Transform (RIFT) descriptors at a set of points given by <set...
Definition: rift.hpp:113
void computeRIFT(const PointCloudIn &cloud, const PointCloudGradient &gradient, int p_idx, float radius, const pcl::Indices &indices, const std::vector< float > &squared_distances, Eigen::MatrixXf &rift_descriptor)
Estimate the Rotation Invariant Feature Transform (RIFT) descriptor for a given point based on its sp...
Definition: rift.hpp:48
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
#define M_PI
Definition: pcl_macros.h:201