Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
grsd.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2009-2014, Willow Garage, Inc.
6 * Copyright (c) 2014-, 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#ifndef PCL_FEATURES_IMPL_GRSD_H_
40#define PCL_FEATURES_IMPL_GRSD_H_
41
42#include <pcl/features/grsd.h>
43#include <pcl/features/rsd.h> // for RSDEstimation
44///////// STATIC /////////
45template <typename PointInT, typename PointNT, typename PointOutT> int
47 double min_radius_plane,
48 double max_radius_noise,
51{
53 return (1); // plane
55 return (2); // cylinder (rim)
57 return (0); // noise/corner
59 return (3); // sphere/corner
60 return (4); // edge
61}
62
63//////////////////////////////////////////////////////////////////////////////////////////////
64template <typename PointInT, typename PointNT, typename PointOutT> void
66{
67 // Check if search_radius_ was set
68 if (width_ < 0)
69 {
70 PCL_ERROR ("[pcl::%s::computeFeature] A voxel cell width needs to be set!\n", getClassName ().c_str ());
71 output.width = output.height = 0;
72 output.clear ();
73 return;
74 }
75
76 // Create the voxel grid
79 grid.setLeafSize (width_, width_, width_);
80 grid.setInputCloud (input_);
81 grid.setSaveLeafLayout (true); // TODO maybe avoid this using nearest neighbor search
82 grid.filter (*cloud_downsampled);
83
84 // Compute RSD
87 rsd.setInputCloud (cloud_downsampled);
88 rsd.setSearchSurface (input_);
89 rsd.setInputNormals (normals_);
90 rsd.setRadiusSearch (std::max (search_radius_, std::sqrt (3.0) * width_ / 2));
91 rsd.compute (*radii);
92
93 // Save the type of each point
94 int NR_CLASS = 5; // TODO make this nicer
95 std::vector<int> types (radii->size ());
96 std::transform(radii->points.cbegin (), radii->points.cend (), types.begin (),
97 [](const auto& point) {
98 // GCC 5.4 can't find unqualified getSimpleType
99 return GRSDEstimation<PointInT, PointNT, PointOutT>::getSimpleType(point.r_min, point.r_max); });
100
101 // Get the transitions between surface types between neighbors of occupied cells
102 Eigen::MatrixXi transition_matrix = Eigen::MatrixXi::Zero (NR_CLASS + 1, NR_CLASS + 1);
103 for (std::size_t idx = 0; idx < cloud_downsampled->size (); ++idx)
104 {
105 const int source_type = types[idx];
106 std::vector<int> neighbors = grid.getNeighborCentroidIndices ((*cloud_downsampled)[idx], relative_coordinates_all_);
107 for (const int &neighbor : neighbors)
108 {
110 if (neighbor != -1) // not empty
111 neighbor_type = types[neighbor];
113 }
114 }
115
116 // Save feature values
117 output.resize (1);
118 output.height = output.width = 1;
119 int nrf = 0;
120 for (int i = 0; i < NR_CLASS + 1; i++)
121 for (int j = i; j < NR_CLASS + 1; j++)
122 output[0].histogram[nrf++] = transition_matrix (i, j) + transition_matrix (j, i);
123}
124
125#define PCL_INSTANTIATE_GRSDEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::GRSDEstimation<T,NT,OutT>;
126
127#endif /* PCL_FEATURES_IMPL_GRSD_H_ */
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
typename Feature< PointInT, PointOutT >::PointCloudIn PointCloudIn
Definition grsd.h:85
static int getSimpleType(float min_radius, float max_radius, double min_radius_plane=0.100, double max_radius_noise=0.015, double min_radius_cylinder=0.175, double max_min_radius_diff=0.050)
Get the type of the local surface based on the min and max radius computed.
Definition grsd.hpp:46
void computeFeature(PointCloudOut &output) override
Estimate the Global Radius-based Surface Descriptor (GRSD) for a set of points given by <setInputClou...
Definition grsd.hpp:65
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition grsd.h:84
typename Feature< PointInT, PointOutT >::PointCloudInPtr PointCloudInPtr
Definition grsd.h:86
shared_ptr< PointCloud< PointT > > Ptr