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  int n_neighbours = this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances);
122 
123  if (n_neighbours < min_neighbors_for_normal_axis_)
124  {
125  if (!pcl::isFinite ((*normals_)[index]))
126  {
127  //normal is invalid
128  //setting lrf to NaN
129  lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
130  return (std::numeric_limits<SignedDistanceT>::max ());
131  }
132 
133  //set z_axis as the normal of index point
134  fitted_normal = (*normals_)[index].getNormalVector3fMap ();
135  }
136  else
137  {
138  float plane_curvature;
139  normal_estimation_.computePointNormal (*surface_, neighbours_indices, fitted_normal (0), fitted_normal (1), fitted_normal (2), plane_curvature);
140 
141  //disambiguate Z axis with normal mean
142  if (!pcl::flipNormalTowardsNormalsMean<PointNT> (*normals_, neighbours_indices, fitted_normal))
143  {
144  //all normals in the neighbourood are invalid
145  //setting lrf to NaN
146  lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
147  return (std::numeric_limits<SignedDistanceT>::max ());
148  }
149  }
150 
151  //setting LRF Z axis
152  lrf.row (2).matrix () = fitted_normal;
153 
154  //find X axis
155 
156  //extract support points for Rx radius
157  n_neighbours = sampled_tree_->radiusSearch ((*input_)[index], tangent_radius_, neighbours_indices, neighbours_distances);
158 
159  if (n_neighbours < min_neighbors_for_tangent_axis_)
160  {
161  //set X axis as a random axis
162  x_axis = pcl::geometry::randomOrthogonalAxis (fitted_normal);
163  y_axis = fitted_normal.cross (x_axis);
164 
165  lrf.row (0).matrix () = x_axis;
166  lrf.row (1).matrix () = y_axis;
167 
168  return (std::numeric_limits<SignedDistanceT>::max ());
169  }
170 
171  //find point with the largest signed distance from tangent plane
172 
173  SignedDistanceT shape_score;
174  SignedDistanceT best_shape_score = -std::numeric_limits<SignedDistanceT>::max ();
175  int best_shape_index = -1;
176 
177  Eigen::Vector3f best_margin_point;
178 
179  const float radius2 = tangent_radius_ * tangent_radius_;
180  const float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2;
181 
182  Vector3fMapConst feature_point = (*input_)[index].getVector3fMap ();
183 
184  for (int curr_neigh = 0; curr_neigh < n_neighbours; ++curr_neigh)
185  {
186  const int& curr_neigh_idx = neighbours_indices[curr_neigh];
187  const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
188 
189  if (neigh_distance_sqr <= margin_distance2)
190  {
191  continue;
192  }
193 
194  //point curr_neigh_idx is inside the ring between marginThresh and Radius
195 
196  shape_score = fitted_normal.dot ((*sampled_surface_)[curr_neigh_idx].getVector3fMap ());
197 
198  if (shape_score > best_shape_score)
199  {
200  best_shape_index = curr_neigh_idx;
201  best_shape_score = shape_score;
202  }
203  } //for each neighbor
204 
205  if (best_shape_index == -1)
206  {
207  x_axis = pcl::geometry::randomOrthogonalAxis (fitted_normal);
208  y_axis = fitted_normal.cross (x_axis);
209 
210  lrf.row (0).matrix () = x_axis;
211  lrf.row (1).matrix () = y_axis;
212 
213  return (std::numeric_limits<SignedDistanceT>::max ());
214  }
215 
216  //find orthogonal axis directed to best_shape_index point projection on plane with fittedNormal as axis
217  x_axis = pcl::geometry::projectedAsUnitVector (sampled_surface_->at (best_shape_index).getVector3fMap (), feature_point, fitted_normal);
218 
219  y_axis = fitted_normal.cross (x_axis);
220 
221  lrf.row (0).matrix () = x_axis;
222  lrf.row (1).matrix () = y_axis;
223  //z axis already set
224 
225  best_shape_score -= fitted_normal.dot (feature_point);
226  return (best_shape_score);
227 }
228 
229 //////////////////////////////////////////////////////////////////////////////////////////////
230 template<typename PointInT, typename PointNT, typename PointOutT, typename SignedDistanceT> void
232 {
233  //check whether used with search radius or search k-neighbors
234  if (this->getKSearch () != 0)
235  {
236  PCL_ERROR (
237  "[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch (0) and setRadiusSearch (radius) to use this class.\n",
238  getClassName ().c_str ());
239  return;
240  }
241 
242  signed_distances_from_highest_points_.resize (indices_->size ());
243 
244  for (size_t point_idx = 0; point_idx < indices_->size (); ++point_idx)
245  {
246  Eigen::Matrix3f currentLrf;
247  PointOutT &rf = output[point_idx];
248 
249  signed_distances_from_highest_points_[point_idx] = computePointLRF ((*indices_)[point_idx], currentLrf);
250  if (signed_distances_from_highest_points_[point_idx] == std::numeric_limits<SignedDistanceT>::max ())
251  {
252  output.is_dense = false;
253  }
254 
255  rf.getXAxisVector3fMap () = currentLrf.row (0);
256  rf.getYAxisVector3fMap () = currentLrf.row (1);
257  rf.getZAxisVector3fMap () = currentLrf.row (2);
258  }
259 }
260 
261 #define PCL_INSTANTIATE_FLARELocalReferenceFrameEstimation(T,NT,OutT,SdT) template class PCL_EXPORTS pcl::FLARELocalReferenceFrameEstimation<T,NT,OutT,SdT>;
262 
263 #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:231
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:434
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