Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
ppf.hpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2011, Alexandru-Eugen Ichim
6  * Willow Garage, Inc
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 Willow Garage, Inc. 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  * $Id: ppf.hpp 5036 2012-03-12 08:54:15Z rusu $
37  */
38 
39 #ifndef PCL_FEATURES_IMPL_PPF_H_
40 #define PCL_FEATURES_IMPL_PPF_H_
41 
42 #include <pcl/features/ppf.h>
43 #include <pcl/features/pfh.h>
44 
46 template <typename PointInT, typename PointNT, typename PointOutT>
47 pcl::PPFEstimation<PointInT, PointNT, PointOutT>::PPFEstimation ()
48  : FeatureFromNormals <PointInT, PointNT, PointOutT> ()
49 {
50  feature_name_ = "PPFEstimation";
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> ());
53  Feature<PointInT, PointOutT>::search_radius_ = 1.0f;
54 }
55 
56 
58 template <typename PointInT, typename PointNT, typename PointOutT> void
59 pcl::PPFEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
60 {
61  // Initialize output container - overwrite the sizes done by Feature::initCompute ()
62  output.points.resize (indices_->size () * input_->points.size ());
63  output.height = 1;
64  output.width = static_cast<uint32_t> (output.points.size ());
65  output.is_dense = true;
66 
67  // Compute point pair features for every pair of points in the cloud
68  for (size_t index_i = 0; index_i < indices_->size (); ++index_i)
69  {
70  size_t i = (*indices_)[index_i];
71  for (size_t j = 0 ; j < input_->points.size (); ++j)
72  {
73  PointOutT p;
74  if (i != j)
75  {
76  if (//pcl::computePPFPairFeature
77  pcl::computePairFeatures (input_->points[i].getVector4fMap (),
78  normals_->points[i].getNormalVector4fMap (),
79  input_->points[j].getVector4fMap (),
80  normals_->points[j].getNormalVector4fMap (),
81  p.f1, p.f2, p.f3, p.f4))
82  {
83  // Calculate alpha_m angle
84  Eigen::Vector3f model_reference_point = input_->points[i].getVector3fMap (),
85  model_reference_normal = normals_->points[i].getNormalVector3fMap (),
86  model_point = input_->points[j].getVector3fMap ();
87  Eigen::AngleAxisf rotation_mg (acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ())),
88  model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ());
89  Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg;
90 
91  Eigen::Vector3f model_point_transformed = transform_mg * model_point;
92  float angle = atan2f ( -model_point_transformed(2), model_point_transformed(1));
93  if (sin (angle) * model_point_transformed(2) < 0.0f)
94  angle *= (-1);
95  p.alpha_m = -angle;
96  }
97  else
98  {
99  PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %zu and %zu went wrong.\n", getClassName ().c_str (), i, j);
100  p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = std::numeric_limits<float>::quiet_NaN ();
101  output.is_dense = false;
102  }
103  }
104  // Do not calculate the feature for identity pairs (i, i) as they are not used
105  // in the following computations
106  else
107  {
108  p.f1 = p.f2 = p.f3 = p.f4 = p.alpha_m = std::numeric_limits<float>::quiet_NaN ();
109  output.is_dense = false;
110  }
111 
112  output.points[index_i*input_->points.size () + j] = p;
113  }
114  }
115 }
116 
118 template <typename PointInT, typename PointNT> void
119 pcl::PPFEstimation<PointInT, PointNT, Eigen::MatrixXf>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
120 {
121  // Initialize output container - overwrite the sizes done by Feature::initCompute ()
122  output.points.resize (indices_->size () * input_->points.size (), 5);
123  output.height = 1;
124  output.width = static_cast<uint32_t> (indices_->size () * input_->points.size ());
125 
126  output.is_dense = true;
127  // Compute point pair features for every pair of points in the cloud
128  for (size_t index_i = 0; index_i < indices_->size (); ++index_i)
129  {
130  size_t i = (*indices_)[index_i];
131  for (size_t j = 0 ; j < input_->points.size (); ++j)
132  {
133  Eigen::VectorXf p (5);
134  if (i != j)
135  {
136  if (//pcl::computePPFPairFeature
137  pcl::computePairFeatures (input_->points[i].getVector4fMap (),
138  normals_->points[i].getNormalVector4fMap (),
139  input_->points[j].getVector4fMap (),
140  normals_->points[j].getNormalVector4fMap (),
141  p (0), p (1), p (2), p (3)))
142  {
143  // Calculate alpha_m angle
144  Eigen::Vector3f model_reference_point = input_->points[i].getVector3fMap (),
145  model_reference_normal = normals_->points[i].getNormalVector3fMap (),
146  model_point = input_->points[j].getVector3fMap ();
147  Eigen::AngleAxisf rotation_mg (acosf (model_reference_normal.dot (Eigen::Vector3f::UnitX ())),
148  model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ());
149  Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg;
150 
151  Eigen::Vector3f model_point_transformed = transform_mg * model_point;
152  float angle = atan2f ( -model_point_transformed(2), model_point_transformed(1));
153  if (sin (angle) * model_point_transformed(2) < 0.0f)
154  angle *= (-1);
155  p (4) = -angle;
156  }
157  else
158  {
159  PCL_ERROR ("[pcl::%s::computeFeature] Computing pair feature vector between points %zu and %zu went wrong.\n", getClassName ().c_str (), i, j);
160  p.setConstant (std::numeric_limits<float>::quiet_NaN ());
161  output.is_dense = false;
162  }
163  }
164  // Do not calculate the feature for identity pairs (i, i) as they are not used
165  // in the following computations
166  else
167  {
168  p.setConstant (std::numeric_limits<float>::quiet_NaN ());
169  output.is_dense = false;
170  }
171 
172  output.points.row (index_i*input_->points.size () + j) = p;
173  }
174  }
175 }
176 
177 
178 #define PCL_INSTANTIATE_PPFEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PPFEstimation<T,NT,OutT>;
179 
180 
181 #endif // PCL_FEATURES_IMPL_PPF_H_
Generated on Fri Mar 8 2013 12:50:46 for Point Cloud Library (PCL) by   doxygen 1.8.3.1