Loading...
Searching...
No Matches
NearestNeighborsLinear.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_LINEAR_
38#define OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_LINEAR_
39
40#include "ompl/datastructures/NearestNeighbors.h"
41#include "ompl/util/Exception.h"
42#include <algorithm>
43
44namespace ompl
45{
55 template <typename _T>
57 {
58 public:
60 {
61 }
62
63 ~NearestNeighborsLinear() override = default;
64
65 void clear() override
66 {
67 data_.clear();
68 }
69
70 bool reportsSortedResults() const override
71 {
72 return true;
73 }
74
75 void add(const _T &data) override
76 {
77 data_.push_back(data);
78 }
79
80 void add(const std::vector<_T> &data) override
81 {
82 data_.reserve(data_.size() + data.size());
83 data_.insert(data_.end(), data.begin(), data.end());
84 }
85
86 bool remove(const _T &data) override
87 {
88 if (!data_.empty())
89 for (int i = data_.size() - 1; i >= 0; --i)
90 if (data_[i] == data)
91 {
92 data_.erase(data_.begin() + i);
93 return true;
94 }
95 return false;
96 }
97
98 _T nearest(const _T &data) const override
99 {
100 const std::size_t sz = data_.size();
101 std::size_t pos = sz;
102 double dmin = 0.0;
103 for (std::size_t i = 0; i < sz; ++i)
104 {
105 double distance = NearestNeighbors<_T>::distFun_(data_[i], data);
106 if (pos == sz || dmin > distance)
107 {
108 pos = i;
109 dmin = distance;
110 }
111 }
112 if (pos != sz)
113 return data_[pos];
114
115 throw Exception("No elements found in nearest neighbors data structure");
116 }
117
119 void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const override
120 {
121 nbh = data_;
122 if (nbh.size() > k)
123 {
124 std::partial_sort(nbh.begin(), nbh.begin() + k, nbh.end(),
125 ElemSort(data, NearestNeighbors<_T>::distFun_));
126 nbh.resize(k);
127 }
128 else
129 {
130 std::sort(nbh.begin(), nbh.end(), ElemSort(data, NearestNeighbors<_T>::distFun_));
131 }
132 }
133
135 void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const override
136 {
137 nbh.clear();
138 for (const auto &d : data_)
139 if (NearestNeighbors<_T>::distFun_(d, data) <= radius)
140 nbh.push_back(d);
141 std::sort(nbh.begin(), nbh.end(), ElemSort(data, NearestNeighbors<_T>::distFun_));
142 }
143
144 std::size_t size() const override
145 {
146 return data_.size();
147 }
148
149 void list(std::vector<_T> &data) const override
150 {
151 data = data_;
152 }
153
154 protected:
156 std::vector<_T> data_;
157
158 private:
159 struct ElemSort
160 {
161 ElemSort(const _T &e, const typename NearestNeighbors<_T>::DistanceFunction &df) : e_(e), df_(df)
162 {
163 }
164
165 bool operator()(const _T &a, const _T &b) const
166 {
167 return df_(a, e_) < df_(b, e_);
168 }
169
170 const _T &e_;
171 const typename NearestNeighbors<_T>::DistanceFunction &df_;
172 };
173 };
174}
175
176#endif
The exception type for ompl.
Definition: Exception.h:47
A nearest neighbors datastructure that uses linear search.
void list(std::vector< _T > &data) const override
Get all the elements in the datastructure.
std::size_t size() const override
Get the number of elements in the datastructure.
_T nearest(const _T &data) const override
Get the nearest neighbor of a point.
void nearestR(const _T &data, double radius, std::vector< _T > &nbh) const override
Return the nearest neighbors within distance radius in sorted order.
std::vector< _T > data_
The data elements stored in this structure.
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.
void nearestK(const _T &data, std::size_t k, std::vector< _T > &nbh) const override
Return the k nearest neighbors in sorted order.
bool reportsSortedResults() const override
Return true if the solutions reported by this data structure are sorted, when calling nearestK / near...
void add(const std::vector< _T > &data) override
Add a vector of points.
Abstract representation of a container that can perform nearest neighbors queries.
DistanceFunction distFun_
The used distance function.
std::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.
Main namespace. Contains everything in this library.