Point Cloud Library (PCL)  1.11.1-dev
shot_lrf.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012, Willow Garage, Inc.
6  * Copyright (c) 2012-, 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  * $Id$
38  */
39 
40 #ifndef PCL_FEATURES_IMPL_SHOT_LRF_H_
41 #define PCL_FEATURES_IMPL_SHOT_LRF_H_
42 
43 #include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver
44 #include <utility>
45 #include <pcl/features/shot_lrf.h>
46 
47 //////////////////////////////////////////////////////////////////////////////////////////////
48 // Compute a local Reference Frame for a 3D feature; the output is stored in the "rf" matrix
49 template<typename PointInT, typename PointOutT> float
50 pcl::SHOTLocalReferenceFrameEstimation<PointInT, PointOutT>::getLocalRF (const int& current_point_idx, Eigen::Matrix3f &rf)
51 {
52  const Eigen::Vector4f& central_point = (*input_)[current_point_idx].getVector4fMap ();
53  pcl::Indices n_indices;
54  std::vector<float> n_sqr_distances;
55 
56  this->searchForNeighbors (current_point_idx, search_parameter_, n_indices, n_sqr_distances);
57 
58  Eigen::Matrix<double, Eigen::Dynamic, 4> vij (n_indices.size (), 4);
59 
60  Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();
61 
62  double distance = 0.0;
63  double sum = 0.0;
64 
65  int valid_nn_points = 0;
66 
67  for (std::size_t i_idx = 0; i_idx < n_indices.size (); ++i_idx)
68  {
69  Eigen::Vector4f pt = (*surface_)[n_indices[i_idx]].getVector4fMap ();
70  if (pt.head<3> () == central_point.head<3> ())
71  continue;
72 
73  // Difference between current point and origin
74  vij.row (valid_nn_points).matrix () = (pt - central_point).cast<double> ();
75  vij (valid_nn_points, 3) = 0;
76 
77  distance = search_parameter_ - sqrt (n_sqr_distances[i_idx]);
78 
79  // Multiply vij * vij'
80  cov_m += distance * (vij.row (valid_nn_points).head<3> ().transpose () * vij.row (valid_nn_points).head<3> ());
81 
82  sum += distance;
83  valid_nn_points++;
84  }
85 
86  if (valid_nn_points < 5)
87  {
88  //PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Neighborhood has less than 5 vertexes. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]);
89  rf.setConstant (std::numeric_limits<float>::quiet_NaN ());
90 
91  return (std::numeric_limits<float>::max ());
92  }
93 
94  cov_m /= sum;
95 
96  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);
97 
98  const double& e1c = solver.eigenvalues ()[0];
99  const double& e2c = solver.eigenvalues ()[1];
100  const double& e3c = solver.eigenvalues ()[2];
101 
102  if (!std::isfinite (e1c) || !std::isfinite (e2c) || !std::isfinite (e3c))
103  {
104  //PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Eigenvectors are NaN. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]);
105  rf.setConstant (std::numeric_limits<float>::quiet_NaN ());
106 
107  return (std::numeric_limits<float>::max ());
108  }
109 
110  // Disambiguation
111  Eigen::Vector4d v1 = Eigen::Vector4d::Zero ();
112  Eigen::Vector4d v3 = Eigen::Vector4d::Zero ();
113  v1.head<3> ().matrix () = solver.eigenvectors ().col (2);
114  v3.head<3> ().matrix () = solver.eigenvectors ().col (0);
115 
116  int plusNormal = 0, plusTangentDirection1=0;
117  for (int ne = 0; ne < valid_nn_points; ne++)
118  {
119  double dp = vij.row (ne).dot (v1);
120  if (dp >= 0)
121  plusTangentDirection1++;
122 
123  dp = vij.row (ne).dot (v3);
124  if (dp >= 0)
125  plusNormal++;
126  }
127 
128  //TANGENT
129  plusTangentDirection1 = 2*plusTangentDirection1 - valid_nn_points;
130  if (plusTangentDirection1 == 0)
131  {
132  int points = 5; //std::min(valid_nn_points*2/2+1, 11);
133  int medianIndex = valid_nn_points/2;
134 
135  for (int i = -points/2; i <= points/2; i++)
136  if ( vij.row (medianIndex - i).dot (v1) > 0)
137  plusTangentDirection1 ++;
138 
139  if (plusTangentDirection1 < points/2+1)
140  v1 *= - 1;
141  }
142  else if (plusTangentDirection1 < 0)
143  v1 *= - 1;
144 
145  //Normal
146  plusNormal = 2*plusNormal - valid_nn_points;
147  if (plusNormal == 0)
148  {
149  int points = 5; //std::min(valid_nn_points*2/2+1, 11);
150  int medianIndex = valid_nn_points/2;
151 
152  for (int i = -points/2; i <= points/2; i++)
153  if ( vij.row (medianIndex - i).dot (v3) > 0)
154  plusNormal ++;
155 
156  if (plusNormal < points/2+1)
157  v3 *= - 1;
158  } else if (plusNormal < 0)
159  v3 *= - 1;
160 
161  rf.row (0).matrix () = v1.head<3> ().cast<float> ();
162  rf.row (2).matrix () = v3.head<3> ().cast<float> ();
163  rf.row (1).matrix () = rf.row (2).cross (rf.row (0));
164 
165  return (0.0f);
166 }
167 
168 //////////////////////////////////////////////////////////////////////////////////////////////
169 template <typename PointInT, typename PointOutT> void
171 {
172  //check whether used with search radius or search k-neighbors
173  if (this->getKSearch () != 0)
174  {
175  PCL_ERROR(
176  "[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
177  getClassName().c_str ());
178  return;
179  }
180  tree_->setSortedResults (true);
181 
182  for (std::size_t i = 0; i < indices_->size (); ++i)
183  {
184  // point result
185  Eigen::Matrix3f rf;
186  PointOutT& output_rf = output[i];
187 
188  //output_rf.confidence = getLocalRF ((*indices_)[i], rf);
189  //if (output_rf.confidence == std::numeric_limits<float>::max ())
190  if (getLocalRF ((*indices_)[i], rf) == std::numeric_limits<float>::max ())
191  {
192  output.is_dense = false;
193  }
194 
195  for (int d = 0; d < 3; ++d)
196  {
197  output_rf.x_axis[d] = rf.row (0)[d];
198  output_rf.y_axis[d] = rf.row (1)[d];
199  output_rf.z_axis[d] = rf.row (2)[d];
200  }
201  }
202 }
203 
204 #define PCL_INSTANTIATE_SHOTLocalReferenceFrameEstimation(T,OutT) template class PCL_EXPORTS pcl::SHOTLocalReferenceFrameEstimation<T,OutT>;
205 
206 #endif // PCL_FEATURES_IMPL_SHOT_LRF_H_
207 
pcl::geometry::distance
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::SHOTLocalReferenceFrameEstimation::getLocalRF
float getLocalRF(const int &index, Eigen::Matrix3f &rf)
Computes disambiguated local RF for a point index.
Definition: shot_lrf.hpp:50
pcl::PointCloud::is_dense
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:397
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:131
pcl::SHOTLocalReferenceFrameEstimation::computeFeature
void computeFeature(PointCloudOut &output) override
Feature estimation method.
Definition: shot_lrf.hpp:170