octree.hpp
Go to the documentation of this file.
1 // kate: replace-tabs off; indent-width 4; indent-mode normal
2 // vim: ts=4:sw=4:noexpandtab
3 /*
4 
5 Copyright (c) 2010--2018,
6 Fran├žois Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
7 You can contact the authors at <f dot pomerleau at gmail dot com> and
8 <stephane at magnenat dot net>
9 
10 All rights reserved.
11 
12 Redistribution and use in source and binary forms, with or without
13 modification, are permitted provided that the following conditions are met:
14  * Redistributions of source code must retain the above copyright
15  notice, this list of conditions and the following disclaimer.
16  * Redistributions in binary form must reproduce the above copyright
17  notice, this list of conditions and the following disclaimer in the
18  documentation and/or other materials provided with the distribution.
19  * Neither the name of the <organization> nor the
20  names of its contributors may be used to endorse or promote products
21  derived from this software without specific prior written permission.
22 
23 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
24 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
25 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
26 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
27 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
28 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
30 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
31 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
32 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33 
34 */
35 #include "octree.h"
36 
37 #include <iterator>
38 #include <future>
39 #include <ciso646>
40 
41 template<typename T, std::size_t dim>
42 Octree_<T,dim>::Octree_(): parent{nullptr}, depth{0}
43 {
44  for(size_t i=0; i< nbCells; ++i) cells[i]=nullptr;
45 }
46 
47 template<typename T, std::size_t dim>
49  bb{o.bb.center, o.bb.radius}, depth{o.depth}
50 {
51  if (!o.parent)
52  parent = nullptr;
53 
54  if(o.isLeaf()) //Leaf case
55  {
56  //nullify childs
57  for(size_t i=0; i<nbCells; ++i)
58  cells[i]= nullptr;
59  //Copy data
60  data.insert(data.end(), o.data.begin(), o.data.end());
61  }
62  else //Node case
63  {
64  //Create each child recursively
65  for(size_t i=0; i<nbCells;++i)
66  {
67  cells[i] = new Octree_<T,dim>(*(o.cells[i]));
68  //Assign parent
69  cells[i]->parent = this;
70  }
71  }
72 }
73 template<typename T, std::size_t dim>
75  parent{nullptr}, bb{o.bb.center, o.bb.radius}, depth{o.depth}
76 {
77  //only allow move of root node
78  assert(o.isRoot());
79 
80  if(o.isLeaf()) //Leaf case
81  {
82  //Copy data
83  data.insert(data.end(),
84  std::make_move_iterator(o.data.begin()),
85  std::make_move_iterator(o.data.end()));
86  }
87  //copy child ptr
88  for(size_t i=0; i<nbCells; ++i)
89  {
90  cells[i] = o.cells[i];
91  //Nullify ptrs
92  o.cells[i]=nullptr;
93  }
94 }
95 
96 template<typename T, std::size_t dim>
98 {
99  //delete recursively childs
100  if(!isLeaf())
101  for(size_t i=0; i<nbCells; ++i)
102  delete cells[i];
103 }
104 
105 template<typename T, std::size_t dim>
107 {
108  if (!o.parent)
109  parent = nullptr;
110 
111  depth=o.depth;
112 
113  if(o.isLeaf()) //Leaf case
114  {
115  //nullify childs
116  for(size_t i=0; i<nbCells; ++i)
117  cells[i]= nullptr;
118  //Copy data
119  data.insert(data.end(), o.data.begin(), o.data.end());
120  }
121  else //Node case
122  {
123  //Create each child recursively
124  for(size_t i=0; i<nbCells; ++i)
125  {
126  cells[i] = new Octree_<T,dim>(*(o.cells[i]));
127  //Assign parent
128  cells[i]->parent = this;
129  }
130  }
131  return *this;
132 }
133 
134 template<typename T, std::size_t dim>
136 {
137  //only allow move of root node
138  assert(o.isRoot());
139 
140  parent = nullptr;
141  bb.center = o.bb.center;
142  bb.radius = o.bb.radius;
143 
144  depth = o.depth;
145 
146  if(o.isLeaf()) //Leaf case
147  {
148  //Copy data
149  data.insert(data.end(),
150  std::make_move_iterator(o.data.begin()),
151  std::make_move_iterator(o.data.end()));
152  }
153 
154  //copy childs ptrs
155  for(size_t i=0; i<nbCells; ++i)
156  {
157  cells[i] = o.cells[i];
158  //Nullify ptrs
159  o.cells[i]=nullptr;
160  }
161 
162  return *this;
163 }
164 
165 template<typename T, std::size_t dim>
167 {
168  return (cells[0]==nullptr);
169 }
170 template<typename T, std::size_t dim>
172 {
173  return (parent==nullptr);
174 }
175 template<typename T, std::size_t dim>
177 {
178  return (data.size() == 0);
179 }
180 template<typename T, std::size_t dim>
181 size_t Octree_<T,dim>::idx(const Point& pt) const
182 {
183  size_t id = 0;
184 
185  for(size_t i=0; i<dim; ++i)
186  id|= ((pt(i) > bb.center(i)) << i);
187 
188  return id;
189 }
190 
191 template<typename T, std::size_t dim>
192 size_t Octree_<T,dim>::idx(const DP& pts, const Data d) const
193 {
194  return idx(pts.features.col(d).head(dim));
195 }
196 template<typename T, std::size_t dim>
198 {
199  return depth;
200 }
201 template<typename T, std::size_t dim>
203 {
204  return bb.radius;
205 }
206 template<typename T, std::size_t dim>
208 {
209  return bb.center;
210 }
211 template<typename T, std::size_t dim>
213 {
214  return &data;
215 }
216 template<typename T, std::size_t dim>
218 {
219  assert(idx<nbCells);
220  return cells[idx];
221 }
222 
223 template<typename T, std::size_t dim>
224 typename Octree_<T,dim>::DataContainer Octree_<T,dim>::toData(const DP& pts, const std::vector<Id>& ids)
225 {
226  return DataContainer{ids.begin(), ids.end()};
227 }
228 
229 // Build tree from DataPoints with a specified number of points by node
230 template<typename T, std::size_t dim>
231 bool Octree_<T,dim>::build(const DP& pts, size_t maxDataByNode, T maxSizeByNode, bool parallelBuild)
232 {
233  typedef typename PM::Vector Vector;
234 
235  //Build bounding box
236  BoundingBox box;
237 
238  Vector minValues = pts.features.rowwise().minCoeff();
239  Vector maxValues = pts.features.rowwise().maxCoeff();
240 
241  Point min = minValues.head(dim);
242  Point max = maxValues.head(dim);
243 
244  Point radii = max - min;
245  box.center = min + radii * 0.5;
246 
247  box.radius = radii(0);
248  for(size_t i=1; i<dim; ++i)
249  if (box.radius < radii(i)) box.radius = radii(i);
250 
251  box.radius*=0.5;
252 
253  //Transform pts in data
254  const size_t nbpts = pts.getNbPoints();
255  std::vector<Id> indexes;
256  indexes.reserve(nbpts);
257 
258  for(size_t i=0; i<nbpts; ++i)
259  indexes.emplace_back(Id(i));
260 
261  DataContainer datas = toData(pts, indexes);
262 
263  //build
264  return this->build(pts, std::move(datas), std::move(box), maxDataByNode, maxSizeByNode, parallelBuild);
265 }
266 
267 //Offset lookup table
268 template<typename T, std::size_t dim>
270 
271 template<typename T>
272 struct OctreeHelper<T,3>
273 {
274  static const typename Octree_<T,3>::Point offsetTable[Octree_<T,3>::nbCells];
275 };
276 template<typename T>
278  {
279  {-0.5, -0.5, -0.5},
280  {+0.5, -0.5, -0.5},
281  {-0.5, +0.5, -0.5},
282  {+0.5, +0.5, -0.5},
283  {-0.5, -0.5, +0.5},
284  {+0.5, -0.5, +0.5},
285  {-0.5, +0.5, +0.5},
286  {+0.5, +0.5, +0.5}
287  };
288 
289 template<typename T>
290 struct OctreeHelper<T,2>
291 {
292  static const typename Octree_<T,2>::Point offsetTable[Octree_<T,2>::nbCells];
293 };
294 template<typename T>
296  {
297  {-0.5, -0.5},
298  {+0.5, -0.5},
299  {-0.5, +0.5},
300  {+0.5, +0.5}
301  };
302 
303 template<typename T, std::size_t dim>
304 bool Octree_<T,dim>::build(const DP& pts, DataContainer&& datas, BoundingBox && bb,
305  size_t maxDataByNode, T maxSizeByNode, bool parallelBuild)
306 {
307  //Assign bounding box
308  this->bb.center = bb.center;
309  this->bb.radius = bb.radius;
310 
311  //Check stop condition
312  if((bb.radius*2.0 <= maxSizeByNode) or (datas.size() <= maxDataByNode))
313  {
314  //insert data
315  data.insert(data.end(),
316  std::make_move_iterator(datas.begin()), make_move_iterator(datas.end()));
317  return (isLeaf());
318  }
319 
320  //Split datas
321  const std::size_t nbData = datas.size();
322 
323  DataContainer sDatas[nbCells];
324  for(size_t i=0; i<nbCells; ++i)
325  sDatas[i].reserve(nbData);
326 
327  for(auto&& d : datas)
328  (sDatas[idx(pts, d)]).emplace_back(d);
329 
330  for(size_t i=0; i<nbCells; ++i)
331  sDatas[i].shrink_to_fit();
332 
333  //Compute new bounding boxes
334  BoundingBox boxes[nbCells];
335  const T half_radius = this->bb.radius * 0.5;
336  for(size_t i=0; i<nbCells; ++i)
337  {
338  const Point offset = OctreeHelper<T,dim>::offsetTable[i] * this->bb.radius;
339  boxes[i].radius = half_radius;
340  boxes[i].center = this->bb.center + offset;
341  }
342 
343  //For each child build recursively
344  bool ret = true;
345  std::vector<std::future<void>> futures;
346 
347  for(size_t i=0; i<nbCells; ++i)
348  {
349  auto compute = [maxDataByNode, maxSizeByNode, i, &pts, &sDatas, &boxes, this](){
350  this->cells[i] = new Octree_<T,dim>();
351  //Assign depth
352  this->cells[i]->depth = this->depth+1;
353  //Assign parent
354  this->cells[i]->parent = this;
355  //next call is not parallelizable
356  this->cells[i]->build(pts, std::move(sDatas[i]), std::move(boxes[i]), maxDataByNode, maxSizeByNode, false);
357  };
358 
359  if(parallelBuild)
360  futures.push_back( std::async( std::launch::async, compute ));
361  else
362  compute();
363  }
364 
365  for(auto& f : futures) f.get();
366 
367  return (!isLeaf() and ret);
368 }
369 
370 //------------------------------------------------------------------------------
371 template<typename T, std::size_t dim>
372 template<typename Callback>
373 bool Octree_<T,dim>::visit(Callback& cb)
374 {
375  // Call the callback for this node (if the callback returns false, then
376  // stop traversing.
377  if (!cb(*this)) return false;
378 
379  // If I'm a node, recursively traverse my children
380  if (!isLeaf())
381  for (size_t i=0; i<nbCells; ++i)
382  if (!cells[i]->visit(cb)) return false;
383 
384  return true;
385 }
Octree_::isLeaf
bool isLeaf() const
Definition: octree.hpp:166
Vector
PM::Vector Vector
Definition: pypoint_matcher_helper.h:55
Octree_::getRadius
T getRadius() const
Definition: octree.hpp:202
Octree_::Octree_
Octree_()
Definition: octree.hpp:42
Octree_::depth
std::size_t depth
Definition: octree.h:120
Octree_::build
bool build(const DP &pts, size_t maxDataByNode=1, T maxSizeByNode=T(0.), bool parallelBuild=false)
Definition: octree.hpp:231
build_map.T
T
Definition: build_map.py:34
Octree_::getCenter
Point getCenter() const
Definition: octree.hpp:207
Octree_::~Octree_
virtual ~Octree_()
Definition: octree.hpp:97
Octree_::operator[]
Octree_< T, dim > * operator[](std::size_t idx)
Definition: octree.hpp:217
Octree_< T, 2 >::Point
Eigen::Matrix< T, dim, 1 > Point
Definition: octree.h:88
point_cloud_generator.box
box
Definition: point_cloud_generator.py:33
Octree_::operator=
Octree_< T, dim > & operator=(const Octree_< T, dim > &o)
Definition: octree.hpp:106
Octree_< T, 2 >::Data
typename DP::Index Data
Definition: octree.h:85
Octree_::parent
Octree_ * parent
Definition: octree.h:100
Octree_::isRoot
bool isRoot() const
Definition: octree.hpp:171
Octree_< T, 2 >::DP
typename PM::DataPoints DP
Definition: octree.h:82
Octree_::DataContainer
std::vector< Data > DataContainer
Definition: octree.h:86
Octree_< T, 2 >::Id
typename DP::Index Id
Definition: octree.h:83
Octree_::bb
BoundingBox bb
Definition: octree.h:116
Octree_::getDepth
std::size_t getDepth() const
Definition: octree.hpp:197
icp.data
data
Definition: icp.py:50
Octree_::visit
bool visit(Callback &cb)
Definition: octree.hpp:373
PointMatcher::Vector
Eigen::Matrix< T, Eigen::Dynamic, 1 > Vector
A vector over ScalarType.
Definition: PointMatcher.h:161
Octree_::isEmpty
bool isEmpty() const
Definition: octree.hpp:176
Octree_::idx
std::size_t idx(const Point &pt) const
Definition: octree.hpp:181
Octree_::data
DataContainer data
Definition: octree.h:118
OctreeHelper
Definition: octree.hpp:269
icp_advance_api.dim
dim
Definition: icp_advance_api.py:152
Octree_
Definition: octree.h:78
Octree_::BoundingBox::center
Point center
Definition: octree.h:96
Octree_::cells
Octree_ * cells[nbCells]
Definition: octree.h:101
Octree_::toData
DataContainer toData(const DP &pts, const std::vector< Id > &ids)
Definition: octree.hpp:224
octree.h
Octree_::BoundingBox::radius
T radius
Definition: octree.h:97
Octree_::getData
DataContainer * getData()
Definition: octree.hpp:212


libpointmatcher
Author(s):
autogenerated on Mon Jul 1 2024 02:22:43