source: rrlib_geometry/space_partitioning/tKDTree.hpp @ 21:c6c1dda3d2e0

Last change on this file since 21:c6c1dda3d2e0 was 21:c6c1dda3d2e0, checked in by Tobias Föhst <foehst@…>, 8 years ago

Some renaming and the default metric is now accessible via cDEFAULT_METRIC

File size: 9.4 KB
Line 
1//
2// You received this file as part of RRLib
3// Robotics Research Library
4//
5// Copyright (C) Finroc GbR (finroc.org)
6//
7// This program is free software; you can redistribute it and/or
8// modify it under the terms of the GNU General Public License
9// as published by the Free Software Foundation; either version 2
10// of the License, or (at your option) any later version.
11//
12// This program is distributed in the hope that it will be useful,
13// but WITHOUT ANY WARRANTY; without even the implied warranty of
14// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15// GNU General Public License for more details.
16//
17// You should have received a copy of the GNU General Public License
18// along with this program; if not, write to the Free Software
19// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
20//
21//----------------------------------------------------------------------
22/*!\file    tKDTree.hpp
23 *
24 * \author  Tobias Foehst
25 *
26 * \date    2008-11-28
27 *
28 */
29//----------------------------------------------------------------------
30
31//----------------------------------------------------------------------
32// External includes (system with <>, local with "")
33//----------------------------------------------------------------------
34#include <algorithm>
35
36//----------------------------------------------------------------------
37// Internal includes with ""
38//----------------------------------------------------------------------
39
40//----------------------------------------------------------------------
41// Debugging
42//----------------------------------------------------------------------
43#include <cassert>
44
45//----------------------------------------------------------------------
46// Namespace declaration
47//----------------------------------------------------------------------
48namespace rrlib
49{
50namespace geometry
51{
52
53//----------------------------------------------------------------------
54// Forward declarations / typedefs / enums
55//----------------------------------------------------------------------
56
57//----------------------------------------------------------------------
58// Const values
59//----------------------------------------------------------------------
60
61template <size_t Tdimension, typename TElement>
62const typename tKDTree<Tdimension, TElement>::tMetric tKDTree<Tdimension, TElement>::cDEFAULT_METRIC = [](const tPoint &a, const tPoint &b)
63{
64  return (a - b).Length();
65};
66
67//----------------------------------------------------------------------
68// Implementation
69//----------------------------------------------------------------------
70
71//----------------------------------------------------------------------
72// tKDTree constructors
73//----------------------------------------------------------------------
74template <size_t Tdimension, typename TElement>
75template <typename TIterator>
76tKDTree<Tdimension, TElement>::tKDTree(TIterator begin_points, TIterator end_points, tMetric metric)
77    : root(new tNode(begin_points, end_points, metric))
78{}
79
80//----------------------------------------------------------------------
81// tKDTree destructor
82//----------------------------------------------------------------------
83template <size_t Tdimension, typename TElement>
84tKDTree<Tdimension, TElement>::~tKDTree()
85{
86  delete this->root;
87}
88
89//----------------------------------------------------------------------
90// tKDTree Root
91//----------------------------------------------------------------------
92template <size_t Tdimension, typename TElement>
93const typename tKDTree<Tdimension, TElement>::tNode &tKDTree<Tdimension, TElement>::Root() const
94{
95  assert(this->root);
96  return *this->root;
97}
98
99//----------------------------------------------------------------------
100// tKDTree::tNode constructors
101//----------------------------------------------------------------------
102template <size_t Tdimension, typename TElement>
103template <typename TIterator>
104tKDTree<Tdimension, TElement>::tNode::tNode(TIterator points_begin, TIterator points_end, tMetric metric)
105    : bounding_box(points_begin, points_end),
106    split_axis(SelectSplitAxis(metric)),
107    split_value(0.5 *(this->bounding_box.Min()[this->split_axis] + this->bounding_box.Max()[this->split_axis])),
108    left_child(0),
109    right_child(0),
110    number_of_points(std::distance(points_begin, points_end))
111{
112  if (this->number_of_points > 1)
113  {
114    std::sort(points_begin, points_end, [this](const tPoint &a, const tPoint &b)
115    {
116      return a[this->split_axis] < b[this->split_axis];
117    });
118    TIterator split = points_begin;
119    while (split != points_end && (*split)[this->split_axis] <= this->split_value)
120    {
121      ++split;
122    }
123    if (split != points_begin && split != points_end)
124    {
125      this->left_child = new tNode(points_begin, split, metric);
126      this->right_child = new tNode(split, points_end, metric);
127      assert(this->left_child && this->right_child);
128    }
129  }
130
131  // additional data
132  if (this->IsLeaf())
133  {
134    // there may be more than one point in a leave
135    for (auto it = points_begin; it != points_end; it++)
136    {
137      for (size_t i = 0; i < Tdimension; i++)
138      {
139        this->center_of_mass[i] += (*it)[i];
140      }
141    }
142    this->center_of_mass *= 1.0 / this->NumberOfPoints();
143  }
144  else
145  {
146    // exploiting the recursive structure
147    this->center_of_mass = this->left_child->CenterOfMass() * this->left_child->NumberOfPoints() + this->right_child->CenterOfMass() * this->right_child->NumberOfPoints();
148    this->center_of_mass *= 1.0 / this->NumberOfPoints();
149  }
150}
151
152//----------------------------------------------------------------------
153// tKDTree::tNode destructor
154//----------------------------------------------------------------------
155template <size_t Tdimension, typename TElement>
156tKDTree<Tdimension, TElement>::tNode::~tNode()
157{
158  delete this->left_child;
159  delete this->right_child;
160}
161
162//----------------------------------------------------------------------
163// tKDTree::tNode SplitAxis
164//----------------------------------------------------------------------
165template <size_t Tdimension, typename TElement>
166size_t tKDTree<Tdimension, TElement>::tNode::SplitAxis() const
167{
168  return this->split_axis;
169}
170
171//----------------------------------------------------------------------
172// tKDTree::tNode SplitValue
173//----------------------------------------------------------------------
174template <size_t Tdimension, typename TElement>
175TElement tKDTree<Tdimension, TElement>::tNode::SplitValue() const
176{
177  return this->split_value;
178}
179
180//----------------------------------------------------------------------
181// tKDTree::tNode NumberOfPoints
182//----------------------------------------------------------------------
183template <size_t Tdimension, typename TElement>
184size_t tKDTree<Tdimension, TElement>::tNode::NumberOfPoints() const
185{
186  return this->number_of_points;
187}
188
189//----------------------------------------------------------------------
190// tKDTree::tNode IsLeaf
191//----------------------------------------------------------------------
192template <size_t Tdimension, typename TElement>
193bool tKDTree<Tdimension, TElement>::tNode::IsLeaf() const
194{
195  return this->left_child == 0;
196}
197
198//----------------------------------------------------------------------
199// tKDTree::tNode BoundingBox
200//----------------------------------------------------------------------
201template <size_t Tdimension, typename TElement>
202const typename tKDTree<Tdimension, TElement>::tNode::tBoundingBox &tKDTree<Tdimension, TElement>::tNode::BoundingBox() const
203{
204  return this->bounding_box;
205}
206
207//----------------------------------------------------------------------
208// tKDTree::tNode CenterOfMass
209//----------------------------------------------------------------------
210template <size_t Tdimension, typename TElement>
211const typename tKDTree<Tdimension, TElement>::tPoint &tKDTree<Tdimension, TElement>::tNode::CenterOfMass() const
212{
213  return this->center_of_mass;
214}
215
216//----------------------------------------------------------------------
217// tKDTree::tNode LeftChild
218//----------------------------------------------------------------------
219template <size_t Tdimension, typename TElement>
220const typename tKDTree<Tdimension, TElement>::tNode &tKDTree<Tdimension, TElement>::tNode::LeftChild() const
221{
222  assert(!this->IsLeaf());
223  return *this->left_child;
224}
225
226//----------------------------------------------------------------------
227// tKDTree::tNode RightChild
228//----------------------------------------------------------------------
229template <size_t Tdimension, typename TElement>
230const typename tKDTree<Tdimension, TElement>::tNode &tKDTree<Tdimension, TElement>::tNode::RightChild() const
231{
232  assert(!this->IsLeaf());
233  return *this->right_child;
234}
235
236//----------------------------------------------------------------------
237// tKDTree::tNode SelectSplitAxis
238//----------------------------------------------------------------------
239template <size_t Tdimension, typename TElement>
240size_t tKDTree<Tdimension, TElement>::tNode::SelectSplitAxis(tMetric metric) const
241{
242  tPoint sample_point;
243  size_t result = 0;
244  TElement max_width = -std::numeric_limits<TElement>::max();
245  for (size_t i = 0; i < Tdimension; ++i)
246  {
247    sample_point[i] = this->bounding_box.Max()[i] - this->bounding_box.Min()[i];
248    TElement width = metric(sample_point, tPoint::Zero());
249    sample_point[i] = 0;
250    if (width > max_width)
251    {
252      max_width = width;
253      result = i;
254    }
255  }
256  return result;
257}
258
259//----------------------------------------------------------------------
260// End of namespace declaration
261//----------------------------------------------------------------------
262}
263}
Note: See TracBrowser for help on using the repository browser.