chunked_kdtree.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2018, the mcl_3dl authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #ifndef MCL_3DL_CHUNKED_KDTREE_H
31 #define MCL_3DL_CHUNKED_KDTREE_H
32 
33 #include <cmath>
34 #include <memory>
35 #include <stdexcept>
36 #include <unordered_map>
37 #include <vector>
38 
39 #include <pcl/point_types.h>
40 #include <pcl/kdtree/kdtree.h>
41 #include <pcl/kdtree/kdtree_flann.h>
42 
43 namespace mcl_3dl
44 {
45 template <typename POINT_TYPE>
47 {
48 public:
49  using Ptr = std::shared_ptr<ChunkedKdtree>;
50 
51  class ChunkId
52  {
53  public:
54  using Ptr = std::shared_ptr<ChunkId>;
55 
56  const int x_;
57  const int y_;
58  const int z_;
59 
60  constexpr bool operator==(const ChunkId& a) const
61  {
62  return x_ == a.x_ && y_ == a.y_ && z_ == a.z_;
63  }
64  constexpr bool operator!=(const ChunkId& a) const
65  {
66  return !operator==(a);
67  }
68  constexpr ChunkId operator+(const ChunkId& a) const
69  {
70  return ChunkId(x_ + a.x_, y_ + a.y_, z_ + a.z_);
71  }
72  size_t operator()(const ChunkId& id) const
73  {
74  return (id.x_) ^ (id.y_ << 11) ^ (id.z_ << 22);
75  }
76  ChunkId(const int x, const int y, const int z)
77  : x_(x)
78  , y_(y)
79  , z_(z)
80  {
81  }
83  : x_(0)
84  , y_(0)
85  , z_(0)
86  {
87  }
88  };
89 
90  explicit ChunkedKdtree(
91  const float chunk_length = 20.0,
92  const float max_search_radius = 1.0,
93  const bool keep_clouds = false)
94  : pos_to_chunk_(1.0 / chunk_length)
95  , chunk_length_(chunk_length)
96  , max_search_radius_(max_search_radius)
97  , set_epsilon_(false)
98  , keep_clouds_(keep_clouds)
99  {
100  chunks_.clear();
101  }
102  void setEpsilon(const float epsilon)
103  {
104  epsilon_ = epsilon;
105  set_epsilon_ = true;
106  for (auto& chunk : chunks_)
107  {
108  chunk.second.kdtree_->setEpsilon(epsilon_);
109  }
110  }
112  boost::shared_ptr<pcl::PointRepresentation<POINT_TYPE>> point_rep)
113  {
114  point_rep_ = point_rep;
115  for (auto& chunk : chunks_)
116  {
117  chunk.second.kdtree_->setPointRepresentation(point_rep_);
118  }
119  }
120  const typename pcl::PointCloud<POINT_TYPE>::ConstPtr& getInputCloud() const
121  {
122  return input_cloud_;
123  }
125  const typename pcl::PointCloud<POINT_TYPE>::ConstPtr& cloud)
126  {
127  input_cloud_ = cloud;
128  if (chunks_.size())
129  chunks_.clear();
130  ChunkOriginalIds ids;
131  ChunkCloud clouds;
132  size_t i = 0;
133  for (auto& p : *cloud)
134  {
135  const auto chunk_id = getChunkId(p);
136  clouds[chunk_id].push_back(p);
137  ids[chunk_id].push_back(i);
138 
139  const float in_chunk_x = p.x - chunk_id.x_ * chunk_length_;
140  int x_bound = 0;
141  if (in_chunk_x < max_search_radius_)
142  x_bound = -1;
143  else if (in_chunk_x > chunk_length_ - max_search_radius_)
144  x_bound = 1;
145 
146  const float in_chunk_y = p.y - chunk_id.y_ * chunk_length_;
147  int y_bound = 0;
148  if (in_chunk_y < max_search_radius_)
149  y_bound = -1;
150  else if (in_chunk_y > chunk_length_ - max_search_radius_)
151  y_bound = 1;
152 
153  const float in_chunk_z = p.z - chunk_id.z_ * chunk_length_;
154  int z_bound = 0;
155  if (in_chunk_z < max_search_radius_)
156  z_bound = -1;
157  else if (in_chunk_z > chunk_length_ - max_search_radius_)
158  z_bound = 1;
159 
160  if (x_bound && y_bound && z_bound)
161  {
162  const ChunkId id(chunk_id + ChunkId(x_bound, y_bound, z_bound));
163  clouds[id].push_back(p);
164  ids[id].push_back(i);
165  }
166  if (x_bound && y_bound)
167  {
168  const ChunkId id(chunk_id + ChunkId(x_bound, y_bound, 0));
169  clouds[id].push_back(p);
170  ids[id].push_back(i);
171  }
172  if (y_bound && z_bound)
173  {
174  const ChunkId id(chunk_id + ChunkId(0, y_bound, z_bound));
175  clouds[id].push_back(p);
176  ids[id].push_back(i);
177  }
178  if (z_bound && x_bound)
179  {
180  const ChunkId id(chunk_id + ChunkId(x_bound, 0, z_bound));
181  clouds[id].push_back(p);
182  ids[id].push_back(i);
183  }
184  if (x_bound)
185  {
186  const ChunkId id(chunk_id + ChunkId(x_bound, 0, 0));
187  clouds[id].push_back(p);
188  ids[id].push_back(i);
189  }
190  if (y_bound)
191  {
192  const ChunkId id(chunk_id + ChunkId(0, y_bound, 0));
193  clouds[id].push_back(p);
194  ids[id].push_back(i);
195  }
196  if (z_bound)
197  {
198  const ChunkId id(chunk_id + ChunkId(0, 0, z_bound));
199  clouds[id].push_back(p);
200  ids[id].push_back(i);
201  }
202  ++i;
203  }
204  for (auto& cloud : clouds)
205  {
206  if (point_rep_)
207  chunks_[cloud.first].kdtree_->setPointRepresentation(point_rep_);
208  if (set_epsilon_)
209  chunks_[cloud.first].kdtree_->setEpsilon(epsilon_);
210  auto cloud_ptr = cloud.second.makeShared();
211  chunks_[cloud.first].kdtree_->setInputCloud(cloud_ptr);
212  if (keep_clouds_)
213  chunks_[cloud.first].cloud_ = cloud_ptr;
214  chunks_[cloud.first].original_ids_ = ids[cloud.first];
215  }
216  }
218  const POINT_TYPE& p,
219  const float& radius,
220  std::vector<int>& id,
221  std::vector<float>& dist_sq,
222  const size_t& num)
223  {
224  if (radius > chunk_length_)
225  throw std::runtime_error("ChunkedKdtree: radius must be <chunk_length");
226 
227  const auto chunk_id = getChunkId(p);
228  if (chunks_.find(chunk_id) == chunks_.end())
229  return false;
230 
231  const auto ret = chunks_[chunk_id].kdtree_->radiusSearch(p, radius, id, dist_sq, num);
232 
233  for (auto& i : id)
234  i = chunks_[chunk_id].original_ids_[i];
235 
236  return ret;
237  }
238  typename pcl::KdTreeFLANN<POINT_TYPE>::Ptr getChunkKdtree(const POINT_TYPE& p)
239  {
240  return getChunkKdtree(getChunkId(p));
241  }
242  typename pcl::KdTreeFLANN<POINT_TYPE>::Ptr getChunkKdtree(const ChunkId& c)
243  {
244  if (chunks_.find(c) == chunks_.end())
245  return typename pcl::KdTreeFLANN<POINT_TYPE>::Ptr();
246  return chunks_[c].kdtree_;
247  }
248  typename pcl::PointCloud<POINT_TYPE>::Ptr getChunkCloud(const POINT_TYPE& p)
249  {
250  return getChunkCloud(getChunkId(p));
251  }
252  typename pcl::PointCloud<POINT_TYPE>::Ptr getChunkCloud(const ChunkId& c)
253  {
254  if (!keep_clouds_ || chunks_.find(c) == chunks_.end())
255  return typename pcl::PointCloud<POINT_TYPE>::Ptr();
256  return chunks_[c].cloud_;
257  }
258  ChunkId getChunkId(const POINT_TYPE& p) const
259  {
260  return ChunkId(static_cast<int>(std::floor(p.x * pos_to_chunk_)),
261  static_cast<int>(std::floor(p.y * pos_to_chunk_)),
262  static_cast<int>(std::floor(p.z * pos_to_chunk_)));
263  }
264 
265 protected:
266  class Chunk
267  {
268  public:
269  typename pcl::KdTreeFLANN<POINT_TYPE>::Ptr kdtree_;
270  std::vector<size_t> original_ids_;
271  typename pcl::PointCloud<POINT_TYPE>::Ptr cloud_;
272 
274  : kdtree_(new pcl::KdTreeFLANN<POINT_TYPE>)
275  , cloud_(new pcl::PointCloud<POINT_TYPE>)
276  {
277  }
278  };
279 
280  const float pos_to_chunk_;
281  const float chunk_length_;
282  const float max_search_radius_;
285  float epsilon_;
287 
288  using ChunkMap = std::unordered_map<ChunkId, Chunk, ChunkId>;
289  using ChunkCloud = std::unordered_map<ChunkId, typename pcl::PointCloud<POINT_TYPE>, ChunkId>;
290  using ChunkOriginalIds = std::unordered_map<ChunkId, std::vector<size_t>, ChunkId>;
292  typename pcl::PointCloud<POINT_TYPE>::ConstPtr input_cloud_;
293 };
294 } // namespace mcl_3dl
295 
296 #endif // MCL_3DL_CHUNKED_KDTREE_H
std::vector< size_t > original_ids_
pcl::PointCloud< POINT_TYPE >::Ptr getChunkCloud(const ChunkId &c)
ChunkId getChunkId(const POINT_TYPE &p) const
std::shared_ptr< ChunkedKdtree > Ptr
void setInputCloud(const typename pcl::PointCloud< POINT_TYPE >::ConstPtr &cloud)
ChunkId(const int x, const int y, const int z)
pcl::KdTreeFLANN< POINT_TYPE >::Ptr getChunkKdtree(const POINT_TYPE &p)
sensor_msgs::PointCloud2 PointCloud
std::unordered_map< ChunkId, typename pcl::PointCloud< mcl_3dl::PointXYZIL >, ChunkId > ChunkCloud
pcl::KdTreeFLANN< POINT_TYPE >::Ptr kdtree_
ChunkedKdtree(const float chunk_length=20.0, const float max_search_radius=1.0, const bool keep_clouds=false)
pcl::PointCloud< POINT_TYPE >::Ptr getChunkCloud(const POINT_TYPE &p)
constexpr bool operator!=(const ChunkId &a) const
int radiusSearch(const POINT_TYPE &p, const float &radius, std::vector< int > &id, std::vector< float > &dist_sq, const size_t &num)
std::unordered_map< ChunkId, Chunk, ChunkId > ChunkMap
std::shared_ptr< ChunkId > Ptr
pcl::PointCloud< POINT_TYPE >::ConstPtr input_cloud_
const float max_search_radius_
void setPointRepresentation(boost::shared_ptr< pcl::PointRepresentation< POINT_TYPE >> point_rep)
constexpr ChunkId operator+(const ChunkId &a) const
boost::shared_ptr< pcl::PointRepresentation< POINT_TYPE > > point_rep_
std::unordered_map< ChunkId, std::vector< size_t >, ChunkId > ChunkOriginalIds
pcl::PointCloud< POINT_TYPE >::Ptr cloud_
pcl::KdTreeFLANN< POINT_TYPE >::Ptr getChunkKdtree(const ChunkId &c)
constexpr bool operator==(const ChunkId &a) const
size_t operator()(const ChunkId &id) const
void setEpsilon(const float epsilon)
const pcl::PointCloud< POINT_TYPE >::ConstPtr & getInputCloud() const


mcl_3dl
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:16:29