source: rrlib_geometry/space_partitioning/tKDTree.hpp @ 20:8f9a14fd63eb

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

Readded support for different metrics using C++11 constucts

File size: 9.3 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
61//----------------------------------------------------------------------
62// Implementation
63//----------------------------------------------------------------------
64
65//----------------------------------------------------------------------
66// tKDTree constructors
67//----------------------------------------------------------------------
68template <size_t Tdimension, typename TElement>
69template <typename TIterator>
70tKDTree<Tdimension, TElement>::tKDTree(TIterator begin, TIterator end)
71    : root(new tNode(begin, end, [](const tPoint &a, const tPoint &b)
72{
73  return (a - b).Length();
74}))
75{}
76
77template <size_t Tdimension, typename TElement>
78template <typename TIterator>
79tKDTree<Tdimension, TElement>::tKDTree(TIterator begin, TIterator end, tMetric metric)
80    : root(new tNode(begin, end, metric))
81{}
82
83//----------------------------------------------------------------------
84// tKDTree destructor
85//----------------------------------------------------------------------
86template <size_t Tdimension, typename TElement>
87tKDTree<Tdimension, TElement>::~tKDTree()
88{
89  delete this->root;
90}
91
92//----------------------------------------------------------------------
93// tKDTree Root
94//----------------------------------------------------------------------
95template <size_t Tdimension, typename TElement>
96const typename tKDTree<Tdimension, TElement>::tNode &tKDTree<Tdimension, TElement>::Root() const
97{
98  assert(this->root);
99  return *this->root;
100}
101
102//----------------------------------------------------------------------
103// tKDTree::tNode constructors
104//----------------------------------------------------------------------
105template <size_t Tdimension, typename TElement>
106template <typename TIterator>
107tKDTree<Tdimension, TElement>::tNode::tNode(TIterator begin, TIterator end, tMetric metric)
108    : bounding_box(begin, end),
109    split_axis(SelectSplitAxis(metric)),
110    split_value(0.5 *(this->bounding_box.Min()[this->split_axis] + this->bounding_box.Max()[this->split_axis])),
111    left_child(0),
112    right_child(0),
113    number_of_points(std::distance(begin, end))
114{
115  if (this->number_of_points > 1)
116  {
117    std::sort(begin, end,
118              [this](const tPoint &a, const tPoint &b)
119    {
120      return a[this->split_axis] < b[this->split_axis];
121    });
122    TIterator split = begin;
123    while (split != end && (*split)[this->split_axis] <= this->split_value)
124    {
125      ++split;
126    }
127    if (split != begin && split != end)
128    {
129      this->left_child = new tNode(begin, split, metric);
130      this->right_child = new tNode(split, end, metric);
131      assert(this->left_child && this->right_child);
132    }
133  }
134
135  // additional data
136  if (this->IsLeaf())
137  {
138    // there may be more than one point in a leave
139    for (auto it = begin; it != end; it++)
140    {
141      for (size_t i = 0; i < Tdimension; i++)
142      {
143        this->center_of_mass[i] += (*it)[i];
144      }
145    }
146    this->center_of_mass *= 1.0 / this->NumberOfPoints();
147  }
148  else
149  {
150    // exploiting the recursive structure
151    this->center_of_mass = this->left_child->CenterOfMass() * this->left_child->NumberOfPoints() + this->right_child->CenterOfMass() * this->right_child->NumberOfPoints();
152    this->center_of_mass *= 1.0 / this->NumberOfPoints();
153  }
154}
155
156//----------------------------------------------------------------------
157// tKDTree::tNode destructor
158//----------------------------------------------------------------------
159template <size_t Tdimension, typename TElement>
160tKDTree<Tdimension, TElement>::tNode::~tNode()
161{
162  delete this->left_child;
163  delete this->right_child;
164}
165
166//----------------------------------------------------------------------
167// tKDTree::tNode SplitAxis
168//----------------------------------------------------------------------
169template <size_t Tdimension, typename TElement>
170size_t tKDTree<Tdimension, TElement>::tNode::SplitAxis() const
171{
172  return this->split_axis;
173}
174
175//----------------------------------------------------------------------
176// tKDTree::tNode SplitValue
177//----------------------------------------------------------------------
178template <size_t Tdimension, typename TElement>
179TElement tKDTree<Tdimension, TElement>::tNode::SplitValue() const
180{
181  return this->split_value;
182}
183
184//----------------------------------------------------------------------
185// tKDTree::tNode NumberOfPoints
186//----------------------------------------------------------------------
187template <size_t Tdimension, typename TElement>
188size_t tKDTree<Tdimension, TElement>::tNode::NumberOfPoints() const
189{
190  return this->number_of_points;
191}
192
193//----------------------------------------------------------------------
194// tKDTree::tNode IsLeaf
195//----------------------------------------------------------------------
196template <size_t Tdimension, typename TElement>
197bool tKDTree<Tdimension, TElement>::tNode::IsLeaf() const
198{
199  return this->left_child == 0;
200}
201
202//----------------------------------------------------------------------
203// tKDTree::tNode BoundingBox
204//----------------------------------------------------------------------
205template <size_t Tdimension, typename TElement>
206const typename tKDTree<Tdimension, TElement>::tBoundingBox &tKDTree<Tdimension, TElement>::tNode::BoundingBox() const
207{
208  return this->bounding_box;
209}
210
211//----------------------------------------------------------------------
212// tKDTree::tNode CenterOfMass
213//----------------------------------------------------------------------
214template <size_t Tdimension, typename TElement>
215const typename tKDTree<Tdimension, TElement>::tPoint &tKDTree<Tdimension, TElement>::tNode::CenterOfMass() const
216{
217  return this->center_of_mass;
218}
219
220//----------------------------------------------------------------------
221// tKDTree::tNode LeftChild
222//----------------------------------------------------------------------
223template <size_t Tdimension, typename TElement>
224const typename tKDTree<Tdimension, TElement>::tNode &tKDTree<Tdimension, TElement>::tNode::LeftChild() const
225{
226  assert(!this->IsLeaf());
227  return *this->left_child;
228}
229
230//----------------------------------------------------------------------
231// tKDTree::tNode RightChild
232//----------------------------------------------------------------------
233template <size_t Tdimension, typename TElement>
234const typename tKDTree<Tdimension, TElement>::tNode &tKDTree<Tdimension, TElement>::tNode::RightChild() const
235{
236  assert(!this->IsLeaf());
237  return *this->right_child;
238}
239
240//----------------------------------------------------------------------
241// tKDTree::tNode SelectSplitAxis
242//----------------------------------------------------------------------
243template <size_t Tdimension, typename TElement>
244size_t tKDTree<Tdimension, TElement>::tNode::SelectSplitAxis(tMetric metric) const
245{
246  tPoint sample_point;
247  size_t result = 0;
248  TElement max_width = -std::numeric_limits<TElement>::max();
249  for (size_t i = 0; i < Tdimension; ++i)
250  {
251    sample_point[i] = this->bounding_box.Max()[i] - this->bounding_box.Min()[i];
252    TElement width = metric(sample_point, tPoint::Zero());
253    sample_point[i] = 0;
254    if (width > max_width)
255    {
256      max_width = width;
257      result = i;
258    }
259  }
260  return result;
261}
262
263//----------------------------------------------------------------------
264// End of namespace declaration
265//----------------------------------------------------------------------
266}
267}
Note: See TracBrowser for help on using the repository browser.