Point Cloud Library (PCL) 1.12.1
ppfrgb.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2012-, Open Perception, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the copyright holder(s) nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 */
37
38#ifndef PCL_FEATURES_IMPL_PPFRGB_H_
39#define PCL_FEATURES_IMPL_PPFRGB_H_
40
41#include <pcl/features/ppfrgb.h>
42#include <pcl/features/pfhrgb.h>
43
44//////////////////////////////////////////////////////////////////////////////////////////////
45template <typename PointInT, typename PointNT, typename PointOutT>
47: FeatureFromNormals <PointInT, PointNT, PointOutT> ()
48{
49 feature_name_ = "PPFRGBEstimation";
50 // Slight hack in order to pass the check for the presence of a search method in Feature::initCompute ()
53}
54
55
56//////////////////////////////////////////////////////////////////////////////////////////////
57template <typename PointInT, typename PointNT, typename PointOutT> void
59{
60 // Initialize output container - overwrite the sizes done by Feature::initCompute ()
61 output.resize (indices_->size () * input_->size ());
62 output.height = 1;
63 output.width = output.size ();
64
65 // Compute point pair features for every pair of points in the cloud
66 for (std::size_t index_i = 0; index_i < indices_->size (); ++index_i)
67 {
68 std::size_t i = (*indices_)[index_i];
69 for (std::size_t j = 0 ; j < input_->size (); ++j)
70 {
71 PointOutT p;
72 if (i != j)
73 {
75 ((*input_)[i].getVector4fMap (), (*normals_)[i].getNormalVector4fMap (), (*input_)[i].getRGBVector4i (),
76 (*input_)[j].getVector4fMap (), (*normals_)[j].getNormalVector4fMap (), (*input_)[j].getRGBVector4i (),
77 p.f1, p.f2, p.f3, p.f4, p.r_ratio, p.g_ratio, p.b_ratio))
78 {
79 // Calculate alpha_m angle
80 Eigen::Vector3f model_reference_point = (*input_)[i].getVector3fMap (),
81 model_reference_normal = (*normals_)[i].getNormalVector3fMap (),
82 model_point = (*input_)[j].getVector3fMap ();
83 Eigen::AngleAxisf rotation_mg (std::acos (model_reference_normal.dot (Eigen::Vector3f::UnitX ())),
84 model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ());
85 Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg;
86
87 Eigen::Vector3f model_point_transformed = transform_mg * model_point;
88 float angle = std::atan2 ( -model_point_transformed(2), model_point_transformed(1));
89 if (std::sin (angle) * model_point_transformed(2) < 0.0f)
90 angle *= (-1);
91 p.alpha_m = -angle;
92 }
93 else
94 {
95 PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %lu and %lu went wrong.\n", getClassName ().c_str (), i, j);
96 p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = p.r_ratio = p.g_ratio = p.b_ratio = 0.f;
97 }
98 }
99 // Do not calculate the feature for identity pairs (i, i) as they are not used
100 // in the following computations
101 else
102 p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = p.r_ratio = p.g_ratio = p.b_ratio = 0.f;
103
104 output[index_i*input_->size () + j] = p;
105 }
106 }
107}
108
109
110
111//////////////////////////////////////////////////////////////////////////////////////////////
112//////////////////////////////////////////////////////////////////////////////////////////////
113template <typename PointInT, typename PointNT, typename PointOutT>
115: FeatureFromNormals <PointInT, PointNT, PointOutT> ()
116{
117 feature_name_ = "PPFRGBEstimation";
118}
119
120//////////////////////////////////////////////////////////////////////////////////////////////
121template <typename PointInT, typename PointNT, typename PointOutT> void
123{
124 PCL_INFO ("before computing output size: %u\n", output.size ());
125 output.resize (indices_->size ());
126 for (std::size_t index_i = 0; index_i < indices_->size (); ++index_i)
127 {
128 auto i = (*indices_)[index_i];
129 pcl::Indices nn_indices;
130 std::vector<float> nn_distances;
131 tree_->radiusSearch (i, static_cast<float> (search_radius_), nn_indices, nn_distances);
132
133 PointOutT average_feature_nn;
134 average_feature_nn.alpha_m = 0;
135 average_feature_nn.f1 = average_feature_nn.f2 = average_feature_nn.f3 = average_feature_nn.f4 =
136 average_feature_nn.r_ratio = average_feature_nn.g_ratio = average_feature_nn.b_ratio = 0.0f;
137
138 for (const auto &j : nn_indices)
139 {
140 if (i != j)
141 {
142 float f1, f2, f3, f4, r_ratio, g_ratio, b_ratio;
144 ((*input_)[i].getVector4fMap (), (*normals_)[i].getNormalVector4fMap (), (*input_)[i].getRGBVector4i (),
145 (*input_)[j].getVector4fMap (), (*normals_)[j].getNormalVector4fMap (), (*input_)[j].getRGBVector4i (),
146 f1, f2, f3, f4, r_ratio, g_ratio, b_ratio))
147 {
148 average_feature_nn.f1 += f1;
149 average_feature_nn.f2 += f2;
150 average_feature_nn.f3 += f3;
151 average_feature_nn.f4 += f4;
152 average_feature_nn.r_ratio += r_ratio;
153 average_feature_nn.g_ratio += g_ratio;
154 average_feature_nn.b_ratio += b_ratio;
155 }
156 else
157 {
158 PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %lu and %lu went wrong.\n", getClassName ().c_str (), i, j);
159 }
160 }
161 }
162
163 float normalization_factor = static_cast<float> (nn_indices.size ());
164 average_feature_nn.f1 /= normalization_factor;
165 average_feature_nn.f2 /= normalization_factor;
166 average_feature_nn.f3 /= normalization_factor;
167 average_feature_nn.f4 /= normalization_factor;
168 average_feature_nn.r_ratio /= normalization_factor;
169 average_feature_nn.g_ratio /= normalization_factor;
170 average_feature_nn.b_ratio /= normalization_factor;
171 output[index_i] = average_feature_nn;
172 }
173 PCL_INFO ("Output size: %zu\n", static_cast<std::size_t>(output.size ()));
174}
175
176
177#define PCL_INSTANTIATE_PPFRGBEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PPFRGBEstimation<T,NT,OutT>;
178#define PCL_INSTANTIATE_PPFRGBRegionEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PPFRGBRegionEstimation<T,NT,OutT>;
179
180#endif // PCL_FEATURES_IMPL_PPFRGB_H_
Feature represents the base feature class.
Definition: feature.h:107
std::string feature_name_
The feature name.
Definition: feature.h:223
PPFRGBEstimation()
Empty Constructor.
Definition: ppfrgb.hpp:46
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
PCL_EXPORTS bool computeRGBPairFeatures(const Eigen::Vector4f &p1, const Eigen::Vector4f &n1, const Eigen::Vector4i &colors1, const Eigen::Vector4f &p2, const Eigen::Vector4f &n2, const Eigen::Vector4i &colors2, float &f1, float &f2, float &f3, float &f4, float &f5, float &f6, float &f7)
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133