Point Cloud Library (PCL) 1.13.0
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//////////////////////////////////////////////////////////////////////////////////////////////
47template <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//////////////////////////////////////////////////////////////////////////////////////////////
112template <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