ground_truth.h
Go to the documentation of this file.
1 /***********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
5  * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
6  *
7  * THE BSD LICENSE
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * 1. Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * 2. Redistributions in binary form must reproduce the above copyright
16  * notice, this list of conditions and the following disclaimer in the
17  * documentation and/or other materials provided with the distribution.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
20  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
21  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
22  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
23  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
24  * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
26  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
28  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29  *************************************************************************/
30 
31 #ifndef RTABMAP_FLANN_GROUND_TRUTH_H_
32 #define RTABMAP_FLANN_GROUND_TRUTH_H_
33 
35 #include "rtflann/util/matrix.h"
36 
37 
38 namespace rtflann
39 {
40 
41 template <typename Distance>
42 void find_nearest(const Matrix<typename Distance::ElementType>& dataset, typename Distance::ElementType* query, size_t* matches, size_t nn,
43  size_t skip = 0, Distance distance = Distance())
44 {
45  //typedef typename Distance::ElementType ElementType;
46  typedef typename Distance::ResultType DistanceType;
47  int n = nn + skip;
48 
49  int* match = new int[n];
50  DistanceType* dists = new DistanceType[n];
51 
52  dists[0] = distance(dataset[0], query, dataset.cols);
53  match[0] = 0;
54  int dcnt = 1;
55 
56  for (size_t i=1; i<dataset.rows; ++i) {
57  DistanceType tmp = distance(dataset[i], query, dataset.cols);
58 
59  if (dcnt<n) {
60  match[dcnt] = i;
61  dists[dcnt++] = tmp;
62  }
63  else if (tmp < dists[dcnt-1]) {
64  dists[dcnt-1] = tmp;
65  match[dcnt-1] = i;
66  }
67 
68  int j = dcnt-1;
69  // bubble up
70  while (j>=1 && dists[j]<dists[j-1]) {
71  std::swap(dists[j],dists[j-1]);
72  std::swap(match[j],match[j-1]);
73  j--;
74  }
75  }
76 
77  for (size_t i=0; i<nn; ++i) {
78  matches[i] = match[i+skip];
79  }
80 
81  delete[] match;
82  delete[] dists;
83 }
84 
85 
86 template <typename Distance>
88  int skip=0, Distance d = Distance())
89 {
90  for (size_t i=0; i<testset.rows; ++i) {
91  find_nearest<Distance>(dataset, testset[i], matches[i], matches.cols, skip, d);
92  }
93 }
94 
95 
96 }
97 
98 #endif //FLANN_GROUND_TRUTH_H_
rtflann::find_nearest
void find_nearest(const Matrix< typename Distance::ElementType > &dataset, typename Distance::ElementType *query, size_t *matches, size_t nn, size_t skip=0, Distance distance=Distance())
Definition: ground_truth.h:70
dist.h
n
int n
matches
matches
matrix.h
match
bool match(const T &xpr, std::string ref, std::string str_xpr="")
j
std::ptrdiff_t j
Eigen::PlainObjectBase::rows
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT
std::swap
void swap(GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &a, GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &b)
rtflann::compute_ground_truth
void compute_ground_truth(const Matrix< typename Distance::ElementType > &dataset, const Matrix< typename Distance::ElementType > &testset, Matrix< size_t > &matches, int skip=0, Distance d=Distance())
Definition: ground_truth.h:115
Eigen::PlainObjectBase::cols
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT
Eigen.Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor >
distance
Double_ distance(const OrientedPlane3_ &p)
nn
idx_t * nn
rtflann
Definition: all_indices.h:49
i
int i


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:11