Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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#include <pcl/search/kdtree.h> // for KdTree
44
45//////////////////////////////////////////////////////////////////////////////////////////////
46template <typename PointInT, typename PointNT, typename PointOutT>
48: FeatureFromNormals <PointInT, PointNT, PointOutT> ()
49{
50 feature_name_ = "PPFRGBEstimation";
51 // Slight hack in order to pass the check for the presence of a search method in Feature::initCompute ()
52 Feature<PointInT, PointOutT>::tree_.reset (new pcl::search::KdTree <PointInT> ());
54}
55
56
57//////////////////////////////////////////////////////////////////////////////////////////////
58template <typename PointInT, typename PointNT, typename PointOutT> void
60{
61 // Initialize output container - overwrite the sizes done by Feature::initCompute ()
62 output.resize (indices_->size () * input_->size ());
63 output.height = 1;
64 output.width = output.size ();
65
66 // Compute point pair features for every pair of points in the cloud
67 for (std::size_t index_i = 0; index_i < indices_->size (); ++index_i)
68 {
69 std::size_t i = (*indices_)[index_i];
70 for (std::size_t j = 0 ; j < input_->size (); ++j)
71 {
72 PointOutT p;
73 if (i != j)
74 {
76 ((*input_)[i].getVector4fMap (), (*normals_)[i].getNormalVector4fMap (), (*input_)[i].getRGBVector4i (),
77 (*input_)[j].getVector4fMap (), (*normals_)[j].getNormalVector4fMap (), (*input_)[j].getRGBVector4i (),
78 p.f1, p.f2, p.f3, p.f4, p.r_ratio, p.g_ratio, p.b_ratio))
79 {
80 // Calculate alpha_m angle
81 Eigen::Vector3f model_reference_point = (*input_)[i].getVector3fMap (),
82 model_reference_normal = (*normals_)[i].getNormalVector3fMap (),
83 model_point = (*input_)[j].getVector3fMap ();
84 Eigen::AngleAxisf rotation_mg (std::acos (model_reference_normal.dot (Eigen::Vector3f::UnitX ())),
85 model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ());
86 Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg;
87
88 Eigen::Vector3f model_point_transformed = transform_mg * model_point;
89 float angle = std::atan2 ( -model_point_transformed(2), model_point_transformed(1));
90 if (std::sin (angle) * model_point_transformed(2) < 0.0f)
91 angle *= (-1);
92 p.alpha_m = -angle;
93 }
94 else
95 {
96 PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %lu and %lu went wrong.\n", getClassName ().c_str (), i, j);
97 p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = p.r_ratio = p.g_ratio = p.b_ratio = 0.f;
98 }
99 }
100 // Do not calculate the feature for identity pairs (i, i) as they are not used
101 // in the following computations
102 else
103 p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = p.r_ratio = p.g_ratio = p.b_ratio = 0.f;
104
105 output[index_i*input_->size () + j] = p;
106 }
107 }
108}
109
110
111
112//////////////////////////////////////////////////////////////////////////////////////////////
113//////////////////////////////////////////////////////////////////////////////////////////////
114template <typename PointInT, typename PointNT, typename PointOutT>
116: FeatureFromNormals <PointInT, PointNT, PointOutT> ()
117{
118 feature_name_ = "PPFRGBEstimation";
119}
120
121//////////////////////////////////////////////////////////////////////////////////////////////
122template <typename PointInT, typename PointNT, typename PointOutT> void
124{
125 PCL_INFO ("before computing output size: %u\n", output.size ());
126 output.resize (indices_->size ());
127 for (std::size_t index_i = 0; index_i < indices_->size (); ++index_i)
128 {
129 auto i = (*indices_)[index_i];
130 pcl::Indices nn_indices;
131 std::vector<float> nn_distances;
132 tree_->radiusSearch (i, static_cast<float> (search_radius_), nn_indices, nn_distances);
133
134 PointOutT average_feature_nn;
135 average_feature_nn.alpha_m = 0;
136 average_feature_nn.f1 = average_feature_nn.f2 = average_feature_nn.f3 = average_feature_nn.f4 =
137 average_feature_nn.r_ratio = average_feature_nn.g_ratio = average_feature_nn.b_ratio = 0.0f;
138
139 for (const auto &j : nn_indices)
140 {
141 if (i != j)
142 {
143 float f1, f2, f3, f4, r_ratio, g_ratio, b_ratio;
145 ((*input_)[i].getVector4fMap (), (*normals_)[i].getNormalVector4fMap (), (*input_)[i].getRGBVector4i (),
146 (*input_)[j].getVector4fMap (), (*normals_)[j].getNormalVector4fMap (), (*input_)[j].getRGBVector4i (),
147 f1, f2, f3, f4, r_ratio, g_ratio, b_ratio))
148 {
149 average_feature_nn.f1 += f1;
150 average_feature_nn.f2 += f2;
151 average_feature_nn.f3 += f3;
152 average_feature_nn.f4 += f4;
153 average_feature_nn.r_ratio += r_ratio;
154 average_feature_nn.g_ratio += g_ratio;
155 average_feature_nn.b_ratio += b_ratio;
156 }
157 else
158 {
159 PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %lu and %lu went wrong.\n", getClassName ().c_str (), i, j);
160 }
161 }
162 }
163
164 float normalization_factor = static_cast<float> (nn_indices.size ());
165 average_feature_nn.f1 /= normalization_factor;
166 average_feature_nn.f2 /= normalization_factor;
167 average_feature_nn.f3 /= normalization_factor;
168 average_feature_nn.f4 /= normalization_factor;
169 average_feature_nn.r_ratio /= normalization_factor;
170 average_feature_nn.g_ratio /= normalization_factor;
171 average_feature_nn.b_ratio /= normalization_factor;
172 output[index_i] = average_feature_nn;
173 }
174 PCL_INFO ("Output size: %zu\n", static_cast<std::size_t>(output.size ()));
175}
176
177
178#define PCL_INSTANTIATE_PPFRGBEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PPFRGBEstimation<T,NT,OutT>;
179#define PCL_INSTANTIATE_PPFRGBRegionEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PPFRGBRegionEstimation<T,NT,OutT>;
180
181#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:220
PPFRGBEstimation()
Empty Constructor.
Definition ppfrgb.hpp:47
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