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