Point Cloud Library (PCL)  1.12.1-dev
lccp_segmentation.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2014-, 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_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_
39 #define PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_
40 
41 #include <pcl/segmentation/lccp_segmentation.h>
42 #include <pcl/common/common.h>
43 
44 
45 //////////////////////////////////////////////////////////
46 //////////////////////////////////////////////////////////
47 /////////////////// Public Functions /////////////////////
48 //////////////////////////////////////////////////////////
49 //////////////////////////////////////////////////////////
50 
51 
52 
53 template <typename PointT>
55  concavity_tolerance_threshold_ (10),
56  grouping_data_valid_ (false),
57  supervoxels_set_ (false),
58  use_smoothness_check_ (false),
59  smoothness_threshold_ (0.1),
60  use_sanity_check_ (false),
61  seed_resolution_ (0),
62  voxel_resolution_ (0),
63  k_factor_ (0),
64  min_segment_size_ (0)
65 {
66 }
67 
68 template <typename PointT>
70 
71 template <typename PointT> void
73 {
74  sv_adjacency_list_.clear ();
75  processed_.clear ();
76  sv_label_to_supervoxel_map_.clear ();
77  sv_label_to_seg_label_map_.clear ();
78  seg_label_to_sv_list_map_.clear ();
79  seg_label_to_neighbor_set_map_.clear ();
80  grouping_data_valid_ = false;
81  supervoxels_set_ = false;
82 }
83 
84 template <typename PointT> void
86 {
87  if (supervoxels_set_)
88  {
89  // Calculate for every Edge if the connection is convex or invalid
90  // This effectively performs the segmentation.
91  calculateConvexConnections (sv_adjacency_list_);
92 
93  // Correct edge relations using extended convexity definition if k>0
94  applyKconvexity (k_factor_);
95 
96  // group supervoxels
97  doGrouping ();
98 
99  grouping_data_valid_ = true;
100 
101  // merge small segments
102  mergeSmallSegments ();
103  }
104  else
105  PCL_WARN ("[pcl::LCCPSegmentation::segment] WARNING: Call function setInputSupervoxels first. Nothing has been done. \n");
106 }
107 
108 
109 template <typename PointT> void
111 {
112  if (grouping_data_valid_)
113  {
114  // Relabel all Points in cloud with new labels
115  for (auto &voxel : labeled_cloud_arg)
116  {
117  voxel.label = sv_label_to_seg_label_map_[voxel.label];
118  }
119  }
120  else
121  {
122  PCL_WARN ("[pcl::LCCPSegmentation::relabelCloud] WARNING: Call function segment first. Nothing has been done. \n");
123  }
124 }
125 
126 
127 
128 //////////////////////////////////////////////////////////
129 //////////////////////////////////////////////////////////
130 /////////////////// Protected Functions //////////////////
131 //////////////////////////////////////////////////////////
132 //////////////////////////////////////////////////////////
133 
134 template <typename PointT> void
136 {
137  seg_label_to_neighbor_set_map_.clear ();
138 
139  std::uint32_t current_segLabel;
140  std::uint32_t neigh_segLabel;
141 
142  VertexIterator sv_itr, sv_itr_end;
143  //The vertices in the supervoxel adjacency list are the supervoxel centroids
144  // For every Supervoxel..
145  for(std::tie(sv_itr, sv_itr_end) = boost::vertices(sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr) // For all supervoxels
146  {
147  const std::uint32_t& sv_label = sv_adjacency_list_[*sv_itr];
148  current_segLabel = sv_label_to_seg_label_map_[sv_label];
149 
150  AdjacencyIterator itr_neighbor, itr_neighbor_end;
151  // ..look at all neighbors and insert their labels into the neighbor set
152  for (std::tie(itr_neighbor, itr_neighbor_end) = boost::adjacent_vertices (*sv_itr, sv_adjacency_list_); itr_neighbor != itr_neighbor_end; ++itr_neighbor)
153  {
154  const std::uint32_t& neigh_label = sv_adjacency_list_[*itr_neighbor];
155  neigh_segLabel = sv_label_to_seg_label_map_[neigh_label];
156 
157  if (current_segLabel != neigh_segLabel)
158  {
159  seg_label_to_neighbor_set_map_[current_segLabel].insert (neigh_segLabel);
160  }
161  }
162  }
163 }
164 
165 template <typename PointT> void
167 {
168  if (min_segment_size_ == 0)
169  return;
170 
171  computeSegmentAdjacency ();
172 
173  std::set<std::uint32_t> filteredSegLabels;
174 
175  bool continue_filtering = true;
176 
177  while (continue_filtering)
178  {
179  continue_filtering = false;
180  unsigned int nr_filtered = 0;
181 
182  VertexIterator sv_itr, sv_itr_end;
183  // Iterate through all supervoxels, check if they are in a "small" segment -> change label to largest neighborID
184  for (std::tie(sv_itr, sv_itr_end) = boost::vertices (sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr) // For all supervoxels
185  {
186  const std::uint32_t& sv_label = sv_adjacency_list_[*sv_itr];
187  std::uint32_t current_seg_label = sv_label_to_seg_label_map_[sv_label];
188  std::uint32_t largest_neigh_seg_label = current_seg_label;
189  std::uint32_t largest_neigh_size = seg_label_to_sv_list_map_[current_seg_label].size ();
190 
191  const std::uint32_t& nr_neighbors = seg_label_to_neighbor_set_map_[current_seg_label].size ();
192  if (nr_neighbors == 0)
193  continue;
194 
195  if (seg_label_to_sv_list_map_[current_seg_label].size () <= min_segment_size_)
196  {
197  continue_filtering = true;
198  nr_filtered++;
199 
200  // Find largest neighbor
201  for (auto neighbors_itr = seg_label_to_neighbor_set_map_[current_seg_label].cbegin (); neighbors_itr != seg_label_to_neighbor_set_map_[current_seg_label].cend (); ++neighbors_itr)
202  {
203  if (seg_label_to_sv_list_map_[*neighbors_itr].size () >= largest_neigh_size)
204  {
205  largest_neigh_seg_label = *neighbors_itr;
206  largest_neigh_size = seg_label_to_sv_list_map_[*neighbors_itr].size ();
207  }
208  }
209 
210  // Add to largest neighbor
211  if (largest_neigh_seg_label != current_seg_label)
212  {
213  if (filteredSegLabels.count (largest_neigh_seg_label) > 0)
214  continue; // If neighbor was already assigned to someone else
215 
216  sv_label_to_seg_label_map_[sv_label] = largest_neigh_seg_label;
217  filteredSegLabels.insert (current_seg_label);
218 
219  // Assign supervoxel labels of filtered segment to new owner
220  for (auto sv_ID_itr = seg_label_to_sv_list_map_[current_seg_label].cbegin (); sv_ID_itr != seg_label_to_sv_list_map_[current_seg_label].cend (); ++sv_ID_itr)
221  {
222  seg_label_to_sv_list_map_[largest_neigh_seg_label].insert (*sv_ID_itr);
223  }
224  }
225  }
226  }
227 
228  // Erase filtered Segments from segment map
229  for (const unsigned int &filteredSegLabel : filteredSegLabels)
230  {
231  seg_label_to_sv_list_map_.erase (filteredSegLabel);
232  }
233 
234  // After filtered Segments are deleted, compute completely new adjacency map
235  // NOTE Recomputing the adjacency of every segment in every iteration is an easy but inefficient solution.
236  // Because the number of segments in an average scene is usually well below 1000, the time spend for noise filtering is still negligible in most cases
237  computeSegmentAdjacency ();
238  } // End while (Filtering)
239 }
240 
241 template <typename PointT> void
242 pcl::LCCPSegmentation<PointT>::prepareSegmentation (const std::map<std::uint32_t, typename pcl::Supervoxel<PointT>::Ptr>& supervoxel_clusters_arg,
243  const std::multimap<std::uint32_t, std::uint32_t>& label_adjaceny_arg)
244 {
245  // Clear internal data
246  reset ();
247 
248  // Copy map with supervoxel pointers
249  sv_label_to_supervoxel_map_ = supervoxel_clusters_arg;
250 
251  // Build a boost adjacency list from the adjacency multimap
252  std::map<std::uint32_t, VertexID> label_ID_map;
253 
254  // Add all supervoxel labels as vertices
255  for (auto svlabel_itr = sv_label_to_supervoxel_map_.begin ();
256  svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
257  {
258  const std::uint32_t& sv_label = svlabel_itr->first;
259  VertexID node_id = boost::add_vertex (sv_adjacency_list_);
260  sv_adjacency_list_[node_id] = sv_label;
261  label_ID_map[sv_label] = node_id;
262  }
263 
264  // Add all edges
265  for (const auto &sv_neighbors_itr : label_adjaceny_arg)
266  {
267  const std::uint32_t& sv_label = sv_neighbors_itr.first;
268  const std::uint32_t& neighbor_label = sv_neighbors_itr.second;
269 
270  VertexID u = label_ID_map[sv_label];
271  VertexID v = label_ID_map[neighbor_label];
272 
273  boost::add_edge (u, v, sv_adjacency_list_);
274  }
275 
276  // Initialization
277  // clear the processed_ map
278  seg_label_to_sv_list_map_.clear ();
279  for (auto svlabel_itr = sv_label_to_supervoxel_map_.begin ();
280  svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
281  {
282  const std::uint32_t& sv_label = svlabel_itr->first;
283  processed_[sv_label] = false;
284  sv_label_to_seg_label_map_[sv_label] = 0;
285  }
286 }
287 
288 
289 
290 
291 template <typename PointT> void
293 {
294  // clear the processed_ map
295  seg_label_to_sv_list_map_.clear ();
296  for (auto svlabel_itr = sv_label_to_supervoxel_map_.begin ();
297  svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
298  {
299  const std::uint32_t& sv_label = svlabel_itr->first;
300  processed_[sv_label] = false;
301  sv_label_to_seg_label_map_[sv_label] = 0;
302  }
303 
304  VertexIterator sv_itr, sv_itr_end;
305  // Perform depth search on the graph and recursively group all supervoxels with convex connections
306  //The vertices in the supervoxel adjacency list are the supervoxel centroids
307  // Note: *sv_itr is of type " boost::graph_traits<VoxelAdjacencyList>::vertex_descriptor " which it nothing but a typedef of std::size_t..
308  unsigned int segment_label = 1; // This starts at 1, because 0 is reserved for errors
309  for (std::tie(sv_itr, sv_itr_end) = boost::vertices (sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr) // For all supervoxels
310  {
311  const VertexID sv_vertex_id = *sv_itr;
312  const std::uint32_t& sv_label = sv_adjacency_list_[sv_vertex_id];
313  if (!processed_[sv_label])
314  {
315  // Add neighbors (and their neighbors etc.) to group if similarity constraint is met
316  recursiveSegmentGrowing (sv_vertex_id, segment_label);
317  ++segment_label; // After recursive grouping ended (no more neighbors to consider) -> go to next group
318  }
319  }
320 }
321 
322 template <typename PointT> void
324  const unsigned int segment_label)
325 {
326  const std::uint32_t& sv_label = sv_adjacency_list_[query_point_id];
327 
328  processed_[sv_label] = true;
329 
330  // The next two lines add the supervoxel to the segment
331  sv_label_to_seg_label_map_[sv_label] = segment_label;
332  seg_label_to_sv_list_map_[segment_label].insert (sv_label);
333 
334  OutEdgeIterator out_Edge_itr, out_Edge_itr_end;
335  // Iterate through all neighbors of this supervoxel and check whether they should be merged with the current supervoxel
336  // boost::out_edges (query_point_id, sv_adjacency_list_): adjacent vertices to node (*itr) in graph sv_adjacency_list_
337  for (std::tie(out_Edge_itr, out_Edge_itr_end) = boost::out_edges (query_point_id, sv_adjacency_list_); out_Edge_itr != out_Edge_itr_end; ++out_Edge_itr)
338  {
339  const VertexID neighbor_ID = boost::target (*out_Edge_itr, sv_adjacency_list_);
340  const std::uint32_t& neighbor_label = sv_adjacency_list_[neighbor_ID];
341 
342  if (!processed_[neighbor_label]) // If neighbor was not already processed
343  {
344  if (sv_adjacency_list_[*out_Edge_itr].is_valid)
345  {
346  recursiveSegmentGrowing (neighbor_ID, segment_label);
347  }
348  }
349  } // End neighbor loop
350 }
351 
352 template <typename PointT> void
354 {
355  if (k_arg == 0)
356  return;
357 
358  EdgeIterator edge_itr, edge_itr_end, next_edge;
359  // Check all edges in the graph for k-convexity
360  for (std::tie (edge_itr, edge_itr_end) = boost::edges (sv_adjacency_list_), next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
361  {
362  ++next_edge; // next_edge iterator is necessary, because removing an edge invalidates the iterator to the current edge
363 
364  bool is_convex = sv_adjacency_list_[*edge_itr].is_convex;
365 
366  if (is_convex) // If edge is (0-)convex
367  {
368  unsigned int kcount = 0;
369 
370  const VertexID source = boost::source (*edge_itr, sv_adjacency_list_);
371  const VertexID target = boost::target (*edge_itr, sv_adjacency_list_);
372 
373  OutEdgeIterator source_neighbors_itr, source_neighbors_itr_end;
374  // Find common neighbors, check their connection
375  for (std::tie(source_neighbors_itr, source_neighbors_itr_end) = boost::out_edges (source, sv_adjacency_list_); source_neighbors_itr != source_neighbors_itr_end; ++source_neighbors_itr) // For all supervoxels
376  {
377  VertexID source_neighbor_ID = boost::target (*source_neighbors_itr, sv_adjacency_list_);
378 
379  OutEdgeIterator target_neighbors_itr, target_neighbors_itr_end;
380  for (std::tie(target_neighbors_itr, target_neighbors_itr_end) = boost::out_edges (target, sv_adjacency_list_); target_neighbors_itr != target_neighbors_itr_end; ++target_neighbors_itr) // For all supervoxels
381  {
382  VertexID target_neighbor_ID = boost::target (*target_neighbors_itr, sv_adjacency_list_);
383  if (source_neighbor_ID == target_neighbor_ID) // Common neighbor
384  {
385  EdgeID src_edge = boost::edge (source, source_neighbor_ID, sv_adjacency_list_).first;
386  EdgeID tar_edge = boost::edge (target, source_neighbor_ID, sv_adjacency_list_).first;
387 
388  bool src_is_convex = (sv_adjacency_list_)[src_edge].is_convex;
389  bool tar_is_convex = (sv_adjacency_list_)[tar_edge].is_convex;
390 
391  if (src_is_convex && tar_is_convex)
392  ++kcount;
393 
394  break;
395  }
396  }
397 
398  if (kcount >= k_arg) // Connection is k-convex, stop search
399  break;
400  }
401 
402  // Check k convexity
403  if (kcount < k_arg)
404  (sv_adjacency_list_)[*edge_itr].is_valid = false;
405  }
406  }
407 }
408 
409 template <typename PointT> void
411 {
412 
413  EdgeIterator edge_itr, edge_itr_end, next_edge;
414  for (std::tie(edge_itr, edge_itr_end) = boost::edges (adjacency_list_arg), next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
415  {
416  ++next_edge; // next_edge iterator is necessary, because removing an edge invalidates the iterator to the current edge
417 
418  std::uint32_t source_sv_label = adjacency_list_arg[boost::source (*edge_itr, adjacency_list_arg)];
419  std::uint32_t target_sv_label = adjacency_list_arg[boost::target (*edge_itr, adjacency_list_arg)];
420 
421  float normal_difference;
422  bool is_convex = connIsConvex (source_sv_label, target_sv_label, normal_difference);
423  adjacency_list_arg[*edge_itr].is_convex = is_convex;
424  adjacency_list_arg[*edge_itr].is_valid = is_convex;
425  adjacency_list_arg[*edge_itr].normal_difference = normal_difference;
426  }
427 }
428 
429 template <typename PointT> bool
430 pcl::LCCPSegmentation<PointT>::connIsConvex (const std::uint32_t source_label_arg,
431  const std::uint32_t target_label_arg,
432  float &normal_angle)
433 {
434  typename pcl::Supervoxel<PointT>::Ptr& sv_source = sv_label_to_supervoxel_map_[source_label_arg];
435  typename pcl::Supervoxel<PointT>::Ptr& sv_target = sv_label_to_supervoxel_map_[target_label_arg];
436 
437  const Eigen::Vector3f& source_centroid = sv_source->centroid_.getVector3fMap ();
438  const Eigen::Vector3f& target_centroid = sv_target->centroid_.getVector3fMap ();
439 
440  const Eigen::Vector3f& source_normal = sv_source->normal_.getNormalVector3fMap (). normalized ();
441  const Eigen::Vector3f& target_normal = sv_target->normal_.getNormalVector3fMap (). normalized ();
442 
443  //NOTE For angles below 0 nothing will be merged
444  if (concavity_tolerance_threshold_ < 0)
445  {
446  return (false);
447  }
448 
449  bool is_convex = true;
450  bool is_smooth = true;
451 
452  normal_angle = getAngle3D (source_normal, target_normal, true);
453  // Geometric comparisons
454  Eigen::Vector3f vec_t_to_s, vec_s_to_t;
455 
456  vec_t_to_s = source_centroid - target_centroid;
457  vec_s_to_t = -vec_t_to_s;
458 
459  Eigen::Vector3f ncross;
460  ncross = source_normal.cross (target_normal);
461 
462  // Smoothness Check: Check if there is a step between adjacent patches
463  if (use_smoothness_check_)
464  {
465  float expected_distance = ncross.norm () * seed_resolution_;
466  float dot_p_1 = vec_t_to_s.dot (source_normal);
467  float dot_p_2 = vec_s_to_t.dot (target_normal);
468  float point_dist = (std::fabs (dot_p_1) < std::fabs (dot_p_2)) ? std::fabs (dot_p_1) : std::fabs (dot_p_2);
469  const float dist_smoothing = smoothness_threshold_ * voxel_resolution_; // This is a slacking variable especially important for patches with very similar normals
470 
471  if (point_dist > (expected_distance + dist_smoothing))
472  {
473  is_smooth &= false;
474  }
475  }
476  // ----------------
477 
478  // Sanity Criterion: Check if definition convexity/concavity makes sense for connection of given patches
479  float intersection_angle = getAngle3D (ncross, vec_t_to_s, true);
480  float min_intersect_angle = (intersection_angle < 90.) ? intersection_angle : 180. - intersection_angle;
481 
482  float intersect_thresh = 60. * 1. / (1. + std::exp (-0.25 * (normal_angle - 25.)));
483  if (min_intersect_angle < intersect_thresh && use_sanity_check_)
484  {
485  // std::cout << "Concave/Convex not defined for given case!" << std::endl;
486  is_convex &= false;
487  }
488 
489 
490  // vec_t_to_s is the reference direction for angle measurements
491  // Convexity Criterion: Check if connection of patches is convex. If this is the case the two supervoxels should be merged.
492  if ((getAngle3D (vec_t_to_s, source_normal) - getAngle3D (vec_t_to_s, target_normal)) <= 0)
493  {
494  is_convex &= true; // connection convex
495  }
496  else
497  {
498  is_convex &= (normal_angle < concavity_tolerance_threshold_); // concave connections will be accepted if difference of normals is small
499  }
500  return (is_convex && is_smooth);
501 }
502 
503 #endif // PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_
virtual ~LCCPSegmentation()
typename boost::graph_traits< SupervoxelAdjacencyList >::vertex_iterator VertexIterator
typename boost::graph_traits< SupervoxelAdjacencyList >::edge_iterator EdgeIterator
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, std::uint32_t, EdgeProperties > SupervoxelAdjacencyList
void recursiveSegmentGrowing(const VertexID &queryPointID, const unsigned int group_label)
Assigns neighbors of the query point to the same group as the query point.
void calculateConvexConnections(SupervoxelAdjacencyList &adjacency_list_arg)
Calculates convexity of edges and saves this to the adjacency graph.
void computeSegmentAdjacency()
Compute the adjacency of the segments.
void relabelCloud(pcl::PointCloud< pcl::PointXYZL > &labeled_cloud_arg)
Relabels cloud with supervoxel labels with the computed segment labels.
void mergeSmallSegments()
Segments smaller than min_segment_size_ are merged to the label of largest neighbor.
void prepareSegmentation(const std::map< std::uint32_t, typename pcl::Supervoxel< PointT >::Ptr > &supervoxel_clusters_arg, const std::multimap< std::uint32_t, std::uint32_t > &label_adjacency_arg)
Is called within setInputSupervoxels mainly to reserve required memory.
void segment()
Merge supervoxels using local convexity.
typename boost::graph_traits< SupervoxelAdjacencyList >::out_edge_iterator OutEdgeIterator
typename boost::graph_traits< SupervoxelAdjacencyList >::adjacency_iterator AdjacencyIterator
bool connIsConvex(const std::uint32_t source_label_arg, const std::uint32_t target_label_arg, float &normal_angle)
Returns true if the connection between source and target is convex.
void doGrouping()
Perform depth search on the graph and recursively group all supervoxels with convex connections.
void applyKconvexity(const unsigned int k_arg)
Connections are only convex if this is true for at least k_arg common neighbors of the two patches.
typename boost::graph_traits< SupervoxelAdjacencyList >::edge_descriptor EdgeID
typename boost::graph_traits< SupervoxelAdjacencyList >::vertex_descriptor VertexID
void reset()
Reset internal memory.
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
pcl::PointXYZRGBA centroid_
The centroid of the supervoxel - average voxel.
shared_ptr< Supervoxel< PointT > > Ptr
pcl::Normal normal_
The normal calculated for the voxels contained in the supervoxel.
Define standard C methods and C++ classes that are common to all methods.
double getAngle3D(const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree=false)
Compute the smallest angle between two 3D vectors in radians (default) or degree.
Definition: common.hpp:47