kdtree_nanoflann_dof.h 8.16 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
/***********************************************************************
 * Software License Agreement (BSD License)
 *
 * Copyright 2011 Jose Luis Blanco (joseluisblancoc@gmail.com).
 *   All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *************************************************************************/
/** \file kdtree_nanoflann_dof.h */

#ifndef KDTREE_NANOFLANN_DOF_H
#define KDTREE_NANOFLANN_DOF_H

#include <nanoflann.hpp>

#include <cstdlib>
#include <iostream>

#include "AMDiS.h"

using namespace nanoflann;
using namespace AMDiS;

namespace experimental {

// =====================================================================================
typedef WorldVector<double> PointType;
typedef DOFVector<PointType> my_vector_of_vectors_t;
// =====================================================================================


  /** A simple vector-of-vectors adaptor for nanoflann, without duplicating the storage.
    *  The i'th vector represents a point in the state space.
    *
    *  \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations.
    *  \tparam value_type The type of the point coordinates (typically, double or float).
    *  \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.
    *  \tparam index_type The type for indices in the KD-tree index (typically, size_t of int)
    */
  template < class VectorOfVectorsType, 
	    typename value_type = double, 
	    int DIM = -1, 
	    class Distance = nanoflann::metric_L2_Simple, 
	    typename index_type = DegreeOfFreedom >
  struct KDTreeVectorOfWorldVectorsAdaptor
  {
    typedef KDTreeVectorOfWorldVectorsAdaptor< VectorOfVectorsType, 
						value_type, 
						DIM, 
						Distance > self_t;
    typedef typename Distance::template traits<value_type, self_t>::distance_t metric_t;
    typedef KDTreeSingleIndexAdaptor< metric_t, 
				      self_t, 
				      DIM, 
				      index_type > index_t;

    index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index.

    /// Constructor: takes a const ref to the vector of vectors object with the data points
    KDTreeVectorOfWorldVectorsAdaptor(const int dimensionality, 
				      VectorOfVectorsType &mat, 
				      const int leaf_max_size = 10) : m_data(mat)
    {
      const size_t dims = (*mat.begin()).getSize();
      assert(mat.getUsedSize() != 0 && dims != 0);
      if (DIM > 0 && static_cast<int>(dims) != DIM)
	throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument");
      index = new index_t( dims, *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size, dims ) );
      index->buildIndex();
    }

    ~KDTreeVectorOfWorldVectorsAdaptor() {
      delete index;
    }
    
    void reinit(const int leaf_max_size = 10)
    {
      delete index;
      const size_t dims = (*m_data.begin()).getSize();
      index = new index_t( dims, *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size, dims ) );
      index->buildIndex();
    }

    VectorOfVectorsType &m_data;

    /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]).
      *  Note that this is a short-cut method for index->findNeighbors().
      *  The user can also call index->... methods as desired.
      * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.
      */
    inline void query(const value_type *query_point, 
		      const size_t num_closest, 
		      index_type *out_indices, 
		      value_type *out_distances_sq, 
		      const int nChecks_IGNORED = 10) const
    {
      nanoflann::KNNResultSet<value_type, index_type> resultSet(num_closest);
      resultSet.init(out_indices, out_distances_sq);
      index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
    }

    /** @name Interface expected by KDTreeSingleIndexAdaptor
      * @{ */

    const self_t & derived() const {
      return *this;
    }
    self_t & derived() {
      return *this;
    }

    // Must return the number of data points
    inline size_t kdtree_get_point_count() const {
      return m_data.getUsedSize();
    }

    // Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
    inline value_type kdtree_distance(const value_type *p1, 
				      const index_type idx_p2, 
				      size_t size) const
    {
      value_type s = 0.0;
      for (size_t i = 0; i < size; i++) {
	const value_type d = p1[i] - m_data[idx_p2][i];
	s += d*d;
      }
      return s;
    }

    // Returns the dim'th component of the idx'th point in the class:
    inline value_type kdtree_get_pt(const index_type idx, size_t dim) const {
      return m_data[idx][dim];
    }

    // Optional bounding-box computation: return false to default to a standard bbox computation loop.
    //   Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
    //   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
    template <class BBOX>
    bool kdtree_get_bbox(BBOX &bb) const {
      return false;
    }

    /** @} */
    
    
    /**
    * strategy and nrOfPoints are ignored
    *  == evalAtPoint_simple
    **/
    template<typename T>
    T evalAtPoint(DOFVector<T>& vec, PointType& x, int strategy = 0, int nrOfPoints = -1)
    {
      const size_t nnp = 1;
      std::vector<DegreeOfFreedom> ret_indexes(nnp);
      std::vector<double> out_dists_sqr(nnp);
      nanoflann::KNNResultSet<double, DegreeOfFreedom> resultSet(nnp);
      
      double eps = 0.0;
      Parameters::get("KD-Tree->eps",eps);
      resultSet.init(&ret_indexes[0], &out_dists_sqr[0]);
      index->findNeighbors(resultSet, x.begin(), nanoflann::SearchParams(10, eps));
      
      T value; nullify(value);
      value = vec[ret_indexes[0]];
      return value;
    }

  }; // end of KDTreeVectorOfVectorsAdaptor

  typedef KDTreeVectorOfWorldVectorsAdaptor< my_vector_of_vectors_t, double >  KD_Tree;
  static std::map<const FiniteElemSpace*, std::pair<int, KD_Tree*> > kdtreeMap;

  inline KD_Tree* provideKDTree(const FiniteElemSpace* feSpace)
  {
    if (kdtreeMap.find(feSpace) != kdtreeMap.end()) {
      if (kdtreeMap[feSpace].first != feSpace->getMesh()->getChangeIndex()) {
	feSpace->getMesh()->getDofIndexCoords(feSpace, kdtreeMap[feSpace].second->m_data);
	kdtreeMap[feSpace].second->reinit();
	kdtreeMap[feSpace].first = feSpace->getMesh()->getChangeIndex();
      }
    } else {
      DOFVector<WorldVector<double> >* coords = new DOFVector<WorldVector<double> >(feSpace, "coords");
      feSpace->getMesh()->getDofIndexCoords(feSpace, *coords);
      
      KD_Tree* kdtree = new KD_Tree(Global::getGeo(WORLD), *coords);
      kdtreeMap[feSpace] = std::make_pair(feSpace->getMesh()->getChangeIndex(), kdtree);
    }

    return kdtreeMap[feSpace].second;
  }
  
} // end namespace

#endif