Point Cloud Library (PCL)  1.14.1-dev
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 //////////////////////////////////////////////////////////////////////////////////////////////
45 template <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 //////////////////////////////////////////////////////////////////////////////////////////////
57 template <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 //////////////////////////////////////////////////////////////////////////////////////////////
113 template <typename PointInT, typename PointNT, typename PointOutT>
115 : FeatureFromNormals <PointInT, PointNT, PointOutT> ()
116 {
117  feature_name_ = "PPFRGBEstimation";
118 }
119 
120 //////////////////////////////////////////////////////////////////////////////////////////////
121 template <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:220
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