KernelCollectionKDTree.cpp
Go to the documentation of this file.
1 // (C) Copyright Renaud Detry 2007-2015.
2 // Distributed under the GNU General Public License and under the
3 // BSD 3-Clause License (See accompanying file LICENSE.txt).
4 
5 /** @file */
6 
7 #include "KernelCollectionTypes.h"
8 
9 #include <nuklei/KernelCollection.h>
10 
11 #include "nanoflann.hpp"
12 
13 namespace nuklei {
14 
15  namespace nanoflann_types
16  {
17 
18  struct PointCloud
19  {
20  typedef double T;
21  struct Point
22  {
23  Point(T x, T y, T z) : x(x), y(y), z(z) {}
24  T x,y,z;
25  };
26 
27  std::vector<Point> pts;
28 
29  // Must return the number of data points
30  inline size_t kdtree_get_point_count() const { return pts.size(); }
31 
32  // Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
33  inline T kdtree_distance(const T *p1, const size_t idx_p2,size_t size) const
34  {
35  const T d0=p1[0]-pts[idx_p2].x;
36  const T d1=p1[1]-pts[idx_p2].y;
37  const T d2=p1[2]-pts[idx_p2].z;
38  return d0*d0+d1*d1+d2*d2;
39  }
40 
41  // Returns the dim'th component of the idx'th point in the class:
42  // Since this is inlined and the "dim" argument is typically an immediate value, the
43  // "if/else's" are actually solved at compile time.
44  inline T kdtree_get_pt(const size_t idx, int dim) const
45  {
46  if (dim==0) return pts[idx].x;
47  else if (dim==1) return pts[idx].y;
48  else return pts[idx].z;
49  }
50 
51  // Optional bounding-box computation: return false to default to a standard bbox computation loop.
52  // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
53  // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
54  template <class BBOX>
55  bool kdtree_get_bbox(BBOX &bb) const { return false; }
56 
57  };
58 
59  using namespace nuklei_nanoflann;
60  typedef KDTreeSingleIndexAdaptor<
61  L2_Simple_Adaptor<double, PointCloud > ,
62  PointCloud,
63  3 /* dim */
64  > KDTreeIndex;
65  typedef std::pair<boost::shared_ptr<KDTreeIndex>, PointCloud> Tree;
66  }
67 
68  void KernelCollection::buildNeighborSearchTree()
69  {
70  NUKLEI_TRACE_BEGIN();
71 #ifdef NUKLEI_USE_CGAL
72  using namespace cgal_neighbor_search_types;
73 
74  boost::shared_ptr<Tree> tree(new Tree);
75  for (const_iterator i = as_const(*this).begin(); i != as_const(*this).end(); ++i)
76  {
77  Vector3 loc = i->getLoc();
78  tree->insert(Point_d(loc.X(), loc.Y(), loc.Z()));
79  }
80  if (deco_.has_key(NSTREE_KEY)) deco_.erase(NSTREE_KEY);
81  deco_.insert(NSTREE_KEY, tree);
82 
83 #else
84  NUKLEI_THROW("This function requires CGAL. See http://renaud-detry.net/nuklei/group__install.html");
85 #endif
86  NUKLEI_TRACE_END();
87  }
88 
89  void KernelCollection::buildKdTree()
90  {
91  if (KDTREE_NANOFLANN)
92  {
93  using namespace nanoflann_types;
94 
95  PointCloud pc;
96  for (const_iterator i = as_const(*this).begin(); i != as_const(*this).end(); ++i)
97  {
98  Vector3 loc = i->getLoc();
99  pc.pts.push_back(PointCloud::Point(loc.X(), loc.Y(), loc.Z()));
100  }
101 
102  boost::shared_ptr<Tree> tree(new Tree(boost::shared_ptr<KDTreeIndex>(), pc));
103  tree->first = boost::shared_ptr<KDTreeIndex>(new KDTreeIndex(3 /*dim*/, tree->second, KDTreeSingleIndexAdaptorParams(10 /* max leaf */) ));
104  tree->first->buildIndex();
105 
106  if (deco_.has_key(KDTREE_KEY)) deco_.erase(KDTREE_KEY);
107  deco_.insert(KDTREE_KEY, tree);
108  }
109  else
110  {
111  using namespace libkdtree_types;
112 
113  boost::shared_ptr<Tree> tree(new Tree);
114  for (const_iterator i = as_const(*this).begin(); i != as_const(*this).end(); ++i)
115  {
116  Vector3 loc = i->getLoc();
117  tree->insert(FlexiblePoint(loc.X(), loc.Y(), loc.Z(),
118  std::distance(as_const(*this).begin(), i)));
119  }
120  tree->optimise();
121 
122  if (deco_.has_key(KDTREE_KEY)) deco_.erase(KDTREE_KEY);
123  deco_.insert(KDTREE_KEY, tree);
124  }
125  }
126 
127  template<class KernelType>
128  weight_t KernelCollection::staticEvaluationAt(const kernel::base &k,
129  const EvaluationStrategy strategy) const
130  {
131  NUKLEI_TRACE_BEGIN();
132 
133  NUKLEI_ASSERT(size() > 0);
134 
135  NUKLEI_ASSERT(KernelType().type() == *kernelType_);
136  NUKLEI_ASSERT(*kernelType_ == k.polyType());
137 
138  coord_t value = 0;
139  if (KDTREE_DENSITY_EVAL && size() > 1000)
140  {
141 #if NUKLEI_CHECK_KDTREE_COUNT
142  int n_inside = 0;
143  coord_t rng = maxLocCutPoint()*maxLocCutPoint();
144  {
145  const KernelType &evalPoint = static_cast<const KernelType&>(k);
146  for (const_iterator i = begin(); i != end(); i++)
147  {
148  const KernelType &densityPoint = static_cast<const KernelType&>(*i);
149 
150  if ((densityPoint.loc_-evalPoint.loc_).SquaredLength() < rng)
151  n_inside++;
152  }
153  }
154 #endif
155 
156  if (KDTREE_NANOFLANN)
157  {
158  using namespace nanoflann_types;
159  if (!deco_.has_key(KDTREE_KEY))
160  NUKLEI_THROW("Undefined kd-tree. Call buildKdTree() first.");
161 
162  const KernelType &evalPoint = static_cast<const KernelType&>(k);
163 
164  boost::shared_ptr<Tree> tree(deco_.get< boost::shared_ptr<Tree> >(KDTREE_KEY));
165  //FlexiblePoint s(evalPoint.loc_.X(), evalPoint.loc_.Y(), evalPoint.loc_.Z(), -1);
166 
167  coord_t range = maxLocCutPoint();
168  // nanoflann takes squared distances.
169  range = range*range;
170 
171  std::vector<std::pair<size_t,coord_t> > indices_dists;
172  RadiusResultSet<coord_t,size_t> resultSet(range,indices_dists);
173 
174  as_const(*tree).first->findNeighbors(resultSet, evalPoint.loc_, nuklei_nanoflann::SearchParams());
175 
176  for (std::vector<std::pair<size_t,coord_t> >::const_iterator i = indices_dists.begin(); i != indices_dists.end(); i++)
177  {
178  coord_t cvalue = 0;
179  const KernelType &densityPoint = static_cast<const KernelType&>(at(i->first));
180  cvalue = densityPoint.eval(evalPoint);
181  if (strategy == MAX_EVAL) value = std::max(value, cvalue);
182  else if (strategy == SUM_EVAL) value += cvalue;
183  else if (strategy == WEIGHTED_SUM_EVAL) value += cvalue * densityPoint.getWeight();
184  else NUKLEI_ASSERT(false);
185  }
186  }
187  else
188  {
189  using namespace libkdtree_types;
190  if (!deco_.has_key(KDTREE_KEY))
191  NUKLEI_THROW("Undefined kd-tree. Call buildKdTree() first.");
192 
193  const KernelType &evalPoint = static_cast<const KernelType&>(k);
194  std::vector<FlexiblePoint> in_range;
195 
196 
197  boost::shared_ptr<Tree> tree(deco_.get< boost::shared_ptr<Tree> >(KDTREE_KEY));
198  FlexiblePoint s(evalPoint.loc_.X(), evalPoint.loc_.Y(), evalPoint.loc_.Z(), -1);
199 
200  coord_t range = maxLocCutPoint();
201 
202  as_const(*tree).find_within_range(s, range, std::back_inserter(in_range));
203 
204  for (std::vector<FlexiblePoint>::const_iterator i = in_range.begin(); i != in_range.end(); i++)
205  {
206  coord_t cvalue = 0;
207  const KernelType &densityPoint = static_cast<const KernelType&>(at(i->idx()));
208  cvalue = densityPoint.eval(evalPoint);
209  if (strategy == MAX_EVAL) value = std::max(value, cvalue);
210  else if (strategy == SUM_EVAL) value += cvalue;
211  else if (strategy == WEIGHTED_SUM_EVAL) value += cvalue * densityPoint.getWeight();
212  else NUKLEI_ASSERT(false);
213  }
214  }
215  }
216  else
217  {
218  const KernelType &evalPoint = static_cast<const KernelType&>(k);
219  for (const_iterator i = begin(); i != end(); i++)
220  {
221  coord_t cvalue = 0;
222  const KernelType &densityPoint = static_cast<const KernelType&>(*i);
223  cvalue = densityPoint.eval(evalPoint);
224  if (strategy == MAX_EVAL) value = std::max(value, cvalue);
225  else if (strategy == SUM_EVAL) value += cvalue;
226  else if (strategy == WEIGHTED_SUM_EVAL) value += cvalue * densityPoint.getWeight();
227  else NUKLEI_ASSERT(false);
228  }
229  }
230 
231  return value;
232  NUKLEI_TRACE_END();
233  }
234 
235  weight_t KernelCollection::evaluationAt(const kernel::base &k,
236  const EvaluationStrategy strategy) const
237  {
238  NUKLEI_TRACE_BEGIN();
239  if (empty()) return 0;
240  NUKLEI_ASSERT(kernelType_ == k.polyType());
241 
242  coord_t value = 0;
243  switch (*kernelType_)
244  {
245  case kernel::base::R3:
246  {
247  value = staticEvaluationAt<kernel::r3>(k, strategy);
248  break;
249  }
250  case kernel::base::R3XS2:
251  {
252  value = staticEvaluationAt<kernel::r3xs2>(k, strategy);
253  break;
254  }
255  case kernel::base::R3XS2P:
256  {
257  value = staticEvaluationAt<kernel::r3xs2p>(k, strategy);
258  break;
259  }
260  case kernel::base::SE3:
261  {
262  value = staticEvaluationAt<kernel::se3>(k, strategy);
263  break;
264  }
265  default:
266  {
267  // One could implement a fallback polymorphic eval here (polyEval), but
268  // it's so slow that it's not really useful.
269  NUKLEI_THROW("Unknow kernel type.");
270  break;
271  }
272  }
273 
274  return value;
275  NUKLEI_TRACE_END();
276  }
277 
278 }
279 
nuklei::FlexiblePoint
Definition: KernelCollectionFlexiblePoint.h:16
nuklei
Public namespace.
Definition: Color.cpp:9
nuklei::KernelCollection::const_iterator
Container::const_iterator const_iterator
KernelCollection iterator.
Definition: KernelCollection.h:165
nuklei::weight_t
double weight_t
Type for particle weights.
Definition: Definitions.h:31
NUKLEI_ASSERT
#define NUKLEI_ASSERT(expression)
Throws an Error if expression is not true.
Definition: Common.h:113
nuklei::kernel::base
Polymorphic kernel class.
Definition: Kernel.h:45
KernelCollectionTypes.h
nuklei::coord_t
double coord_t
Type for point coordinates.
Definition: Definitions.h:25
nuklei::KernelCollection::buildKdTree
void buildKdTree()
Builds a kd-tree of the kernel positions and stores the tree internally. See Intermediary Results.
Definition: KernelCollectionKDTree.cpp:89
nuklei::KernelCollection::evaluationAt
weight_t evaluationAt(const kernel::base &f, const EvaluationStrategy strategy=WEIGHTED_SUM_EVAL) const
Evaluates the density represented by *this at f.
Definition: KernelCollectionKDTree.cpp:235
nuklei::kernel::base::polyType
virtual Type polyType() const =0
Get the "kernel type", i.e., its domain of definition.
KernelCollection.h
nuklei::KernelCollection::buildNeighborSearchTree
void buildNeighborSearchTree()
Builds a neighbor search tree of the kernel positions and stores the tree internally....
Definition: KernelCollectionKDTree.cpp:68
nuklei::as_const
T const & as_const(T const &t)
Provides a const reference to an object.
Definition: Common.h:197
NUKLEI_THROW
#define NUKLEI_THROW(x)
Throws an Error.
Definition: Common.h:94
© Copyright 2007-2013 Renaud Detry.
Distributed under the terms of the GNU General Public License (GPL).
(See accompanying file LICENSE.txt or copy at http://www.gnu.org/copyleft/gpl.html.)
Revised Sun Sep 13 2020 19:10:06.