Loading...
Searching...
No Matches
NearestNeighborsSqrtApprox.h
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2008, Willow Garage, Inc.
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Willow Garage nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Ioan Sucan */
36
37#ifndef OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_SQRT_APPROX_
38#define OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_SQRT_APPROX_
39
40#include "ompl/datastructures/NearestNeighborsLinear.h"
41#include <algorithm>
42#include <cmath>
43
44namespace ompl
45{
56 template <typename _T>
58 {
59 public:
61
62 ~NearestNeighborsSqrtApprox() override = default;
63
64 void clear() override
65 {
67 checks_ = 0;
68 offset_ = 0;
69 }
70
71 void add(const _T &data) override
72 {
75 }
76
77 void add(const std::vector<_T> &data) override
78 {
81 }
82
83 bool remove(const _T &data) override
84 {
85 bool result = NearestNeighborsLinear<_T>::remove(data);
86 if (result)
88 return result;
89 }
90
91 _T nearest(const _T &data) const override
92 {
93 const std::size_t n = NearestNeighborsLinear<_T>::data_.size();
94 std::size_t pos = n;
95
96 if (checks_ > 0 && n > 0)
97 {
98 double dmin = 0.0;
99 for (std::size_t j = 0; j < checks_; ++j)
100 {
101 std::size_t i = (j * checks_ + offset_) % n;
102
104 if (pos == n || dmin > distance)
105 {
106 pos = i;
107 dmin = distance;
108 }
109 }
110 offset_ = (offset_ + 1) % checks_;
111 }
112 if (pos != n)
114
115 throw Exception("No elements found in nearest neighbors data structure");
116 }
117
118 protected:
120 inline void updateCheckCount()
121 {
122 checks_ = 1 + (std::size_t)floor(sqrt((double)NearestNeighborsLinear<_T>::data_.size()));
123 }
124
126 std::size_t checks_{0};
127
129 mutable std::size_t offset_{0};
130 };
131}
132
133#endif
The exception type for ompl.
Definition: Exception.h:47
A nearest neighbors datastructure that uses linear search.
std::size_t size() const override
Get the number of elements in the datastructure.
void add(const _T &data) override
Add an element to the datastructure.
void clear() override
Clear the datastructure.
bool remove(const _T &data) override
Remove an element from the datastructure.
A nearest neighbors datastructure that uses linear search. The linear search is done over sqrt(n) ele...
void add(const std::vector< _T > &data) override
Add a vector of points.
bool remove(const _T &data) override
Remove an element from the datastructure.
void add(const _T &data) override
Add an element to the datastructure.
_T nearest(const _T &data) const override
Get the nearest neighbor of a point.
void clear() override
Clear the datastructure.
void updateCheckCount()
The maximum number of checks to perform when searching for a nearest neighbor.
std::size_t offset_
The offset to start checking at (between 0 and checks_)
std::size_t checks_
The number of checks to be performed when looking for a nearest neighbor.
DistanceFunction distFun_
The used distance function.
Main namespace. Contains everything in this library.