CARLA
Rtree.h
Go to the documentation of this file.
1 // Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
7 #pragma once
8 
9 #include <vector>
10 
11 #include <boost/geometry.hpp>
12 #include <boost/geometry/geometries/point.hpp>
13 #include <boost/geometry/index/rtree.hpp>
14 
15 namespace carla {
16 namespace geom {
17 
18  /// Rtree class working with 3D point clouds.
19  /// Asociates a T element with a 3D point
20  /// Useful to perform fast k-NN searches
21  template <typename T, size_t Dimension = 3>
23  public:
24 
25  typedef boost::geometry::model::point<float, Dimension, boost::geometry::cs::cartesian> BPoint;
26  typedef std::pair<BPoint, T> TreeElement;
27 
28  void InsertElement(const BPoint &point, const T &element) {
29  _rtree.insert(std::make_pair(point, element));
30  }
31 
32  void InsertElement(const TreeElement &element) {
33  _rtree.insert(element);
34  }
35 
36  void InsertElements(const std::vector<TreeElement> &elements) {
37  _rtree.insert(elements.begin(), elements.end());
38  }
39 
40  /// Return nearest neighbors with a user defined filter.
41  /// The filter reveices as an argument a TreeElement value and needs to
42  /// return a bool to accept or reject the value
43  /// [&](Rtree::TreeElement const &element){if (IsOk(element)) return true;
44  /// else return false;}
45  template <typename Filter>
46  std::vector<TreeElement> GetNearestNeighboursWithFilter(const BPoint &point, Filter filter,
47  size_t number_neighbours = 1) const {
48  std::vector<TreeElement> query_result;
49  auto nearest = boost::geometry::index::nearest(point, static_cast<unsigned int>(number_neighbours));
50  auto satisfies = boost::geometry::index::satisfies(filter);
51  // Using explicit operator&& to workaround Bullseye coverage issue
52  // https://www.bullseye.com/help/trouble-logicalOverload.html.
53  _rtree.query(operator&&(nearest, satisfies), std::back_inserter(query_result));
54  return query_result;
55  }
56 
57  std::vector<TreeElement> GetNearestNeighbours(const BPoint &point, size_t number_neighbours = 1) const {
58  std::vector<TreeElement> query_result;
59  _rtree.query(boost::geometry::index::nearest(point, static_cast<unsigned int>(number_neighbours)),
60  std::back_inserter(query_result));
61  return query_result;
62  }
63 
64  size_t GetTreeSize() const {
65  return _rtree.size();
66  }
67 
68  private:
69 
70  boost::geometry::index::rtree<TreeElement, boost::geometry::index::linear<16>> _rtree;
71 
72  };
73 
74  /// Rtree class working with 3D segment clouds.
75  /// Stores a pair of T elements (one for each end of the segment)
76  /// Useful to perform fast k-NN searches.
77  template <typename T, size_t Dimension = 3>
79  public:
80 
81  typedef boost::geometry::model::point<float, Dimension, boost::geometry::cs::cartesian> BPoint;
82  typedef boost::geometry::model::segment<BPoint> BSegment;
83  typedef std::pair<BSegment, std::pair<T, T>> TreeElement;
84 
85  void InsertElement(const BSegment &segment, const T &element_start, const T &element_end) {
86  _rtree.insert(std::make_pair(segment, std::make_pair(element_start, element_end)));
87  }
88 
89  void InsertElement(const TreeElement &element) {
90  _rtree.insert(element);
91  }
92 
93  void InsertElements(const std::vector<TreeElement> &elements) {
94  _rtree.insert(elements.begin(), elements.end());
95  }
96 
97  /// Return nearest neighbors with a user defined filter.
98  /// The filter reveices as an argument a TreeElement value and needs to
99  /// return a bool to accept or reject the value
100  /// [&](Rtree::TreeElement const &element){if (IsOk(element)) return true;
101  /// else return false;}
102  template <typename Geometry, typename Filter>
103  std::vector<TreeElement> GetNearestNeighboursWithFilter(
104  const Geometry &geometry,
105  Filter filter,
106  size_t number_neighbours = 1) const {
107  std::vector<TreeElement> query_result;
108  _rtree.query(
109  boost::geometry::index::nearest(geometry, static_cast<unsigned int>(number_neighbours)) &&
110  boost::geometry::index::satisfies(filter),
111  std::back_inserter(query_result));
112  return query_result;
113  }
114 
115  template<typename Geometry>
116  std::vector<TreeElement> GetNearestNeighbours(const Geometry &geometry, size_t number_neighbours = 1) const {
117  std::vector<TreeElement> query_result;
118  _rtree.query(
119  boost::geometry::index::nearest(geometry, static_cast<unsigned int>(number_neighbours)),
120  std::back_inserter(query_result));
121  return query_result;
122  }
123 
124  /// Returns segments that intersec the specified geometry
125  /// Warning: intersection between 3D segments is not implemented by boost
126  template<typename Geometry>
127  std::vector<TreeElement> GetIntersections(const Geometry &geometry) const {
128  std::vector<TreeElement> query_result;
129  _rtree.query(
130  boost::geometry::index::intersects(geometry),
131  std::back_inserter(query_result));
132  return query_result;
133  }
134 
135  size_t GetTreeSize() const {
136  return _rtree.size();
137  }
138 
139  private:
140 
141  boost::geometry::index::rtree<TreeElement, boost::geometry::index::linear<16>> _rtree;
142 
143  };
144 
145 } // namespace geom
146 } // namespace carla
std::vector< TreeElement > GetNearestNeighbours(const BPoint &point, size_t number_neighbours=1) const
Definition: Rtree.h:57
boost::geometry::index::rtree< TreeElement, boost::geometry::index::linear< 16 > > _rtree
Definition: Rtree.h:70
std::vector< TreeElement > GetNearestNeighbours(const Geometry &geometry, size_t number_neighbours=1) const
Definition: Rtree.h:116
std::pair< BPoint, T > TreeElement
Definition: Rtree.h:26
size_t GetTreeSize() const
Definition: Rtree.h:64
void InsertElements(const std::vector< TreeElement > &elements)
Definition: Rtree.h:93
std::vector< TreeElement > GetIntersections(const Geometry &geometry) const
Returns segments that intersec the specified geometry Warning: intersection between 3D segments is no...
Definition: Rtree.h:127
boost::geometry::model::point< float, Dimension, boost::geometry::cs::cartesian > BPoint
Definition: Rtree.h:25
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
void InsertElement(const BPoint &point, const T &element)
Definition: Rtree.h:28
void InsertElement(const TreeElement &element)
Definition: Rtree.h:32
std::vector< TreeElement > GetNearestNeighboursWithFilter(const BPoint &point, Filter filter, size_t number_neighbours=1) const
Return nearest neighbors with a user defined filter.
Definition: Rtree.h:46
Rtree class working with 3D segment clouds.
Definition: Rtree.h:78
boost::geometry::model::point< float, Dimension, boost::geometry::cs::cartesian > BPoint
Definition: Rtree.h:81
void InsertElements(const std::vector< TreeElement > &elements)
Definition: Rtree.h:36
boost::geometry::index::rtree< TreeElement, boost::geometry::index::linear< 16 > > _rtree
Definition: Rtree.h:141
void InsertElement(const TreeElement &element)
Definition: Rtree.h:89
void InsertElement(const BSegment &segment, const T &element_start, const T &element_end)
Definition: Rtree.h:85
boost::geometry::model::segment< BPoint > BSegment
Definition: Rtree.h:82
Rtree class working with 3D point clouds.
Definition: Rtree.h:22
size_t GetTreeSize() const
Definition: Rtree.h:135
std::vector< TreeElement > GetNearestNeighboursWithFilter(const Geometry &geometry, Filter filter, size_t number_neighbours=1) const
Return nearest neighbors with a user defined filter.
Definition: Rtree.h:103
std::pair< BSegment, std::pair< T, T > > TreeElement
Definition: Rtree.h:83