Point Cloud Library (PCL)  1.9.1-dev
flare.hpp
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2016-, 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 
39 #ifndef PCL_FEATURES_IMPL_FLARE_H_
40 #define PCL_FEATURES_IMPL_FLARE_H_
41 
42 #include <pcl/features/flare.h>
43 #include <pcl/common/geometry.h>
44 
45 //////////////////////////////////////////////////////////////////////////////////////////////
46 template<typename PointInT, typename PointNT, typename PointOutT, typename SignedDistanceT> bool
48 {
50  {
51  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
52  return (false);
53  }
54 
55  if (tangent_radius_ == 0.0f)
56  {
57  PCL_ERROR ("[pcl::%s::initCompute] tangent_radius_ not set.\n", getClassName ().c_str ());
58  return (false);
59  }
60 
61  // If no search sampled_surface_ has been defined, use the surface_ dataset as the search sampled_surface_ itself
62  if (!sampled_surface_)
63  {
64  fake_sampled_surface_ = true;
65  sampled_surface_ = surface_;
66 
67  if (sampled_tree_)
68  {
69  PCL_WARN ("[pcl::%s::initCompute] sampled_surface_ is not set even if sampled_tree_ is already set.", getClassName ().c_str ());
70  PCL_WARN ("sampled_tree_ will be rebuilt from surface_. Use sampled_surface_.\n");
71  }
72  }
73 
74  // Check if a space search locator was given for sampled_surface_
75  if (!sampled_tree_)
76  {
77  if (sampled_surface_->isOrganized () && surface_->isOrganized () && input_->isOrganized ())
78  sampled_tree_.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
79  else
80  sampled_tree_.reset (new pcl::search::KdTree<PointInT> (false));
81  }
82 
83  if (sampled_tree_->getInputCloud () != sampled_surface_) // Make sure the tree searches the sampled surface
84  sampled_tree_->setInputCloud (sampled_surface_);
85 
86  return (true);
87 }
88 
89 //////////////////////////////////////////////////////////////////////////////////////////////
90 template<typename PointInT, typename PointNT, typename PointOutT, typename SignedDistanceT> bool
92 {
93  // Reset the surface
94  if (fake_surface_)
95  {
96  surface_.reset ();
97  fake_surface_ = false;
98  }
99  // Reset the sampled surface
100  if (fake_sampled_surface_)
101  {
102  sampled_surface_.reset ();
103  fake_sampled_surface_ = false;
104  }
105  return (true);
106 }
107 
108 //////////////////////////////////////////////////////////////////////////////////////////////
109 template<typename PointInT, typename PointNT, typename PointOutT, typename SignedDistanceT> SignedDistanceT
111  Eigen::Matrix3f &lrf)
112 {
113  Eigen::Vector3f x_axis, y_axis;
114  Eigen::Vector3f fitted_normal; //z_axis
115 
116  //find Z axis
117 
118  //extract support points for the computation of Z axis
119  std::vector<int> neighbours_indices;
120  std::vector<float> neighbours_distances;
121 
122  const std::size_t n_normal_neighbours =
123  this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances);
124  if (n_normal_neighbours < min_neighbors_for_normal_axis_)
125  {
126  if (!pcl::isFinite ((*normals_)[index]))
127  {
128  //normal is invalid
129  //setting lrf to NaN
130  lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
131  return (std::numeric_limits<SignedDistanceT>::max ());
132  }
133 
134  //set z_axis as the normal of index point
135  fitted_normal = (*normals_)[index].getNormalVector3fMap ();
136  }
137  else
138  {
139  float plane_curvature;
140  normal_estimation_.computePointNormal (*surface_, neighbours_indices, fitted_normal (0), fitted_normal (1), fitted_normal (2), plane_curvature);
141 
142  //disambiguate Z axis with normal mean
143  if (!pcl::flipNormalTowardsNormalsMean<PointNT> (*normals_, neighbours_indices, fitted_normal))
144  {
145  //all normals in the neighbourood are invalid
146  //setting lrf to NaN
147  lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
148  return (std::numeric_limits<SignedDistanceT>::max ());
149  }
150  }
151 
152  //setting LRF Z axis
153  lrf.row (2).matrix () = fitted_normal;
154 
155  //find X axis
156 
157  //extract support points for Rx radius
158  const std::size_t n_tangent_neighbours =
159  sampled_tree_->radiusSearch ((*input_)[index], tangent_radius_, neighbours_indices, neighbours_distances);
160 
161  if (n_tangent_neighbours < min_neighbors_for_tangent_axis_)
162  {
163  //set X axis as a random axis
164  x_axis = pcl::geometry::randomOrthogonalAxis (fitted_normal);
165  y_axis = fitted_normal.cross (x_axis);
166 
167  lrf.row (0).matrix () = x_axis;
168  lrf.row (1).matrix () = y_axis;
169 
170  return (std::numeric_limits<SignedDistanceT>::max ());
171  }
172 
173  //find point with the largest signed distance from tangent plane
174 
175  SignedDistanceT shape_score;
176  SignedDistanceT best_shape_score = -std::numeric_limits<SignedDistanceT>::max ();
177  int best_shape_index = -1;
178 
179  Eigen::Vector3f best_margin_point;
180 
181  const float radius2 = tangent_radius_ * tangent_radius_;
182  const float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2;
183 
184  Vector3fMapConst feature_point = (*input_)[index].getVector3fMap ();
185 
186  for (std::size_t curr_neigh = 0; curr_neigh < n_tangent_neighbours; ++curr_neigh)
187  {
188  const int& curr_neigh_idx = neighbours_indices[curr_neigh];
189  const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
190 
191  if (neigh_distance_sqr <= margin_distance2)
192  {
193  continue;
194  }
195 
196  //point curr_neigh_idx is inside the ring between marginThresh and Radius
197 
198  shape_score = fitted_normal.dot ((*sampled_surface_)[curr_neigh_idx].getVector3fMap ());
199 
200  if (shape_score > best_shape_score)
201  {
202  best_shape_index = curr_neigh_idx;
203  best_shape_score = shape_score;
204  }
205  } //for each neighbor
206 
207  if (best_shape_index == -1)
208  {
209  x_axis = pcl::geometry::randomOrthogonalAxis (fitted_normal);
210  y_axis = fitted_normal.cross (x_axis);
211 
212  lrf.row (0).matrix () = x_axis;
213  lrf.row (1).matrix () = y_axis;
214 
215  return (std::numeric_limits<SignedDistanceT>::max ());
216  }
217 
218  //find orthogonal axis directed to best_shape_index point projection on plane with fittedNormal as axis
219  x_axis = pcl::geometry::projectedAsUnitVector (sampled_surface_->at (best_shape_index).getVector3fMap (), feature_point, fitted_normal);
220 
221  y_axis = fitted_normal.cross (x_axis);
222 
223  lrf.row (0).matrix () = x_axis;
224  lrf.row (1).matrix () = y_axis;
225  //z axis already set
226 
227  best_shape_score -= fitted_normal.dot (feature_point);
228  return (best_shape_score);
229 }
230 
231 //////////////////////////////////////////////////////////////////////////////////////////////
232 template<typename PointInT, typename PointNT, typename PointOutT, typename SignedDistanceT> void
234 {
235  //check whether used with search radius or search k-neighbors
236  if (this->getKSearch () != 0)
237  {
238  PCL_ERROR (
239  "[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch (0) and setRadiusSearch (radius) to use this class.\n",
240  getClassName ().c_str ());
241  return;
242  }
243 
244  signed_distances_from_highest_points_.resize (indices_->size ());
245 
246  for (std::size_t point_idx = 0; point_idx < indices_->size (); ++point_idx)
247  {
248  Eigen::Matrix3f currentLrf;
249  PointOutT &rf = output[point_idx];
250 
251  signed_distances_from_highest_points_[point_idx] = computePointLRF ((*indices_)[point_idx], currentLrf);
252  if (signed_distances_from_highest_points_[point_idx] == std::numeric_limits<SignedDistanceT>::max ())
253  {
254  output.is_dense = false;
255  }
256 
257  rf.getXAxisVector3fMap () = currentLrf.row (0);
258  rf.getYAxisVector3fMap () = currentLrf.row (1);
259  rf.getZAxisVector3fMap () = currentLrf.row (2);
260  }
261 }
262 
263 #define PCL_INSTANTIATE_FLARELocalReferenceFrameEstimation(T,NT,OutT,SdT) template class PCL_EXPORTS pcl::FLARELocalReferenceFrameEstimation<T,NT,OutT,SdT>;
264 
265 #endif // PCL_FEATURES_IMPL_FLARE_H_
Eigen::Vector3f randomOrthogonalAxis(Eigen::Vector3f const &axis)
Define a random unit vector orthogonal to axis.
Definition: geometry.h:134
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:61
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:55
Defines some geometrical functions and utility functions.
SignedDistanceT computePointLRF(const int index, Eigen::Matrix3f &lrf)
Estimate the LRF descriptor for a given point based on its spatial neighborhood of 3D points with nor...
Definition: flare.hpp:110
void computeFeature(PointCloudOut &output) override
Abstract feature estimation method.
Definition: flare.hpp:233
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
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:405
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
Definition: organized.h:62
Eigen::Vector3f projectedAsUnitVector(Eigen::Vector3f const &point, Eigen::Vector3f const &plane_origin, Eigen::Vector3f const &plane_normal)
Given a plane defined by plane_origin and plane_normal, find the unit vector pointing from plane_orig...
Definition: geometry.h:115
bool initCompute() override
This method should get called before starting the actual computation.
Definition: flare.hpp:47
bool deinitCompute() override
This method should get called after the actual computation is ended.
Definition: flare.hpp:91