OctreeGrid.cpp
Go to the documentation of this file.
1 // kate: replace-tabs off; indent-width 4; indent-mode normal
2 // vim: ts=4:sw=4:noexpandtab
3 /*
4 
5 Copyright (c) 2010--2018,
6 Fran├žois Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
7 You can contact the authors at <f dot pomerleau at gmail dot com> and
8 <stephane at magnenat dot net>
9 
10 All rights reserved.
11 
12 Redistribution and use in source and binary forms, with or without
13 modification, are permitted provided that the following conditions are met:
14  * Redistributions of source code must retain the above copyright
15  notice, this list of conditions and the following disclaimer.
16  * Redistributions in binary form must reproduce the above copyright
17  notice, this list of conditions and the following disclaimer in the
18  documentation and/or other materials provided with the distribution.
19  * Neither the name of the <organization> nor the
20  names of its contributors may be used to endorse or promote products
21  derived from this software without specific prior written permission.
22 
23 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
24 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
25 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
26 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
27 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
28 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
30 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
31 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
32 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33 
34 */
35 #include "OctreeGrid.h"
36 
37 #include <limits>
38 
39 //Define Visitor classes to apply processing
40 template<typename T>
42  : idx{0}, pts(dp)
43 {
44 }
45 
46 template <typename T>
47 template<std::size_t dim>
49 {
50  if(oc.isLeaf() and not oc.isEmpty())
51  {
52  auto* data = oc.getData();
53  const auto& d = (*data)[0];
54 
55  std::size_t j = d;
56 
57  //retrieve index from lookup table if sampling in already switched element
58  while(j < idx)
59  j = mapidx[j];
60 
61  assert(j >= idx);
62  //Switch columns j and idx
63  pts.swapCols(idx, j);
64 
65  //Maintain new index position
66  mapidx[idx] = j;
67  //Update index
68  ++idx;
69  }
70 
71  return true;
72 }
73 
74 template <typename T>
76 {
77  //Resize point cloud
78  pts.conservativeResize(idx);
79  //Reset param
80  idx=0;
81  return true;
82 }
83 
84 
85 template<typename T>
88 {
89  std::srand(seed);
90 }
91 template<typename T>
93  DataPoints& dp, const std::size_t seed_
94 ): OctreeGridDataPointsFilter<T>::FirstPtsSampler{dp}, seed{seed_}
95 {
96  std::srand(seed);
97 }
98 template<typename T>
99 template<std::size_t dim>
101 {
102  if(oc.isLeaf() and not oc.isEmpty())
103  {
104  auto* data = oc.getData();
105  const std::size_t nbData = (*data).size() - 1;
106  const std::size_t randId =
107  static_cast<std::size_t>( nbData *
108  (static_cast<float>(std::rand()/static_cast<float>(RAND_MAX))));
109 
110  const auto& d = (*data)[randId];
111 
112  std::size_t j = d;
113 
114  //retrieve index from lookup table if sampling in already switched element
115  if(std::size_t(d)<idx)
116  j = mapidx[d];
117 
118  //Switch columns j and idx
119  pts.swapCols(idx, j);
120 
121  //Maintain new index position
122  mapidx[idx] = j;
123  //Update index
124  ++idx;
125  }
126 
127  return true;
128 }
129 
130 template<typename T>
132 {
133  bool ret = FirstPtsSampler::finalize();
134  //Reset seed
135  std::srand(seed);
136 
137  return ret;
138 }
139 
140 template<typename T>
143 {
144 }
145 
146 template<typename T>
147 template<std::size_t dim>
149 {
150  if(oc.isLeaf() and not oc.isEmpty())
151  {
152  const int featDim(pts.features.rows());
153  const int descDim(pts.descriptors.rows());
154  const int timeDim(pts.times.rows());
155 
156  auto* data = oc.getData();
157  const std::size_t nbData = (*data).size();
158 
159  const auto& d = (*data)[0]; //get first data
160  std::size_t j = d; //j contains real index of first point
161 
162  //retrieve index from lookup table if sampling in already switched element
163  while(j < idx)
164  j = mapidx[j];
165 
166  assert(j >= idx);
167  //We sum all the data in the first data
168  for(std::size_t id=1;id<nbData;++id)
169  {
170  //get current idx
171  const auto& curId = (*data)[id];
172  std::size_t i = curId; //i contains real index
173 
174  //retrieve index from lookup table if sampling in already switched element
175  while (i < idx)
176  i = mapidx[i];
177  assert(i >= idx);
178 
179  for (int f = 0; f < (featDim - 1); ++f)
180  pts.features(f,j) += pts.features(f,i);
181 
182  if (pts.descriptors.cols() > 0)
183  for (int d = 0; d < descDim; ++d)
184  pts.descriptors(d,j) += pts.descriptors(d,i);
185 
186  if (pts.times.cols() > 0)
187  for (int t = 0; t < timeDim; ++t)
188  pts.times(t,j) += pts.times(t,i);
189  }
190 
191  // Normalize sums to get centroid (average)
192  for (int f = 0; f < (featDim - 1); ++f)
193  pts.features(f,j) /= T(nbData);
194 
195  if (pts.descriptors.cols() > 0)
196  for (int d = 0; d < descDim; ++d)
197  pts.descriptors(d,j) /= T(nbData);
198 
199  if (pts.times.cols() > 0)
200  for (int t = 0; t < timeDim; ++t)
201  pts.times(t,j) /= T(nbData);
202 
203  //Switch columns j and idx
204  pts.swapCols(idx, j);
205 
206  //Maintain new index position
207  mapidx[idx] = j;
208  //Update index
209  ++idx;
210  }
211 
212  return true;
213 }
214 template<typename T>
217 {
218 }
219 
220 template<typename T>
221 template<std::size_t dim>
223 {
224  if(oc.isLeaf() and not oc.isEmpty())
225  {
226  auto* data = oc.getData();
227  const std::size_t nbData = (*data).size();
228 
229  auto dist = [](const typename Octree_<T,dim>::Point& p1, const typename Octree_<T,dim>::Point& p2) -> T {
230  return (p1 - p2).norm();
231  };
232 
233  //Build centroid
234  typename Octree_<T,dim>::Point center;
235  for(std::size_t i=0;i<dim;++i) center(i)=T(0.);
236 
237  for(std::size_t id=0;id<nbData;++id)
238  {
239  //get current idx
240  const auto& curId = (*data)[id];
241  std::size_t i = curId; //i contains real index
242 
243  //retrieve index from lookup table if sampling in already switched element
244  while (i < idx)
245  i = mapidx[i];
246  assert(i >= idx);
247 
248  for (std::size_t f = 0; f < dim; ++f)
249  center(f) += pts.features(f,i);
250  }
251  for(std::size_t i=0;i<dim;++i) center(i)/=T(nbData);
252 
253  //Get the closest point from the center
254  T minDist = std::numeric_limits<T>::max();
255  std::size_t medId = 0;
256 
257  for(std::size_t id=0;id<nbData;++id)
258  {
259  //get current idx
260  const auto curId = (*data)[id];
261  std::size_t i = curId; //i contains real index
262 
263  //retrieve index from lookup table if sampling in already switched element
264  while (i < idx)
265  i = mapidx[i];
266  assert(i >= idx);
267  const T curDist = dist(pts.features.col(i).head(dim), center);
268  if(curDist<minDist)
269  {
270  minDist = curDist;
271  medId=i;
272  }
273  }
274 
275  //Switch columns j and idx
276  pts.swapCols(idx, medId);
277 
278  //Maintain new index position
279  mapidx[idx] = medId;
280 
281  //Update index
282  ++idx;
283  }
284 
285  return true;
286 }
287 
288 // OctreeGridDataPointsFilter
289 template <typename T>
291  PointMatcher<T>::DataPointsFilter("OctreeGridDataPointsFilter",
293  buildParallel{Parametrizable::get<bool>("buildParallel")},
294  maxPointByNode{Parametrizable::get<std::size_t>("maxPointByNode")},
295  maxSizeByNode{Parametrizable::get<T>("maxSizeByNode")}
296 {
297  try
298  {
299  const int sm = this->template get<int>("samplingMethod");
300  samplingMethod = SamplingMethod(sm);
301  }
302  catch (const InvalidParameter&)
303  {
304  samplingMethod = SamplingMethod::FIRST_PTS;
305  }
306 }
307 
308 template <typename T>
311 {
312  DataPoints output(input);
313  inPlaceFilter(output);
314  return output;
315 }
316 
317 template <typename T>
319 {
320  const std::size_t featDim = cloud.features.rows();
321 
322  assert(featDim == 4 or featDim == 3);
323 
324  if(featDim==3) //2D case
325  this->sample<2>(cloud);
326 
327  else if(featDim==4) //3D case
328  this->sample<3>(cloud);
329 }
330 
331 template<typename T>
332 template<std::size_t dim>
334 {
335  Octree_<T,dim> oc;
336 
337  oc.build(cloud, maxPointByNode, maxSizeByNode, buildParallel);
338 
339  switch(samplingMethod)
340  {
341  case SamplingMethod::FIRST_PTS:
342  {
343  FirstPtsSampler sampler(cloud);
344  oc.visit(sampler);
345  sampler.finalize();
346  break;
347  }
348  case SamplingMethod::RAND_PTS:
349  {
350  RandomPtsSampler sampler(cloud); //FIXME: add seed parameter
351  oc.visit(sampler);
352  sampler.finalize();
353  break;
354  }
355  case SamplingMethod::CENTROID:
356  {
357  CentroidSampler sampler(cloud);
358  oc.visit(sampler);
359  sampler.finalize();
360  break;
361  }
362  case SamplingMethod::MEDOID:
363  {
364  MedoidSampler sampler(cloud);
365  oc.visit(sampler);
366  sampler.finalize();
367  break;
368  }
369  }
370 }
371 
372 
373 template struct OctreeGridDataPointsFilter<float>;
374 template struct OctreeGridDataPointsFilter<double>;
Octree_::isLeaf
bool isLeaf() const
Definition: octree.hpp:166
OctreeGridDataPointsFilter::buildParallel
bool buildParallel
Definition: OctreeGrid.h:156
OctreeGridDataPointsFilter::MedoidSampler
Definition: OctreeGrid.h:138
OctreeGridDataPointsFilter::CentroidSampler
Definition: OctreeGrid.h:124
OctreeGridDataPointsFilter::FirstPtsSampler::FirstPtsSampler
FirstPtsSampler(DataPoints &dp)
Definition: OctreeGrid.cpp:41
Octree_::build
bool build(const DP &pts, size_t maxDataByNode=1, T maxSizeByNode=T(0.), bool parallelBuild=false)
Definition: octree.hpp:231
OctreeGridDataPointsFilter::FirstPtsSampler::finalize
virtual bool finalize()
Definition: OctreeGrid.cpp:75
build_map.T
T
Definition: build_map.py:34
OctreeGridDataPointsFilter::RandomPtsSampler::RandomPtsSampler
RandomPtsSampler(DataPoints &dp)
Definition: OctreeGrid.cpp:86
OctreeGridDataPointsFilter::RandomPtsSampler::operator()
bool operator()(Octree_< T, dim > &oc)
Definition: OctreeGrid.cpp:100
PointMatcher
Functions and classes that are dependant on scalar type are defined in this templatized class.
Definition: PointMatcher.h:130
Octree_::Point
Eigen::Matrix< T, dim, 1 > Point
Definition: octree.h:88
OctreeGridDataPointsFilter::CentroidSampler::CentroidSampler
CentroidSampler(DataPoints &dp)
Definition: OctreeGrid.cpp:141
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
OctreeGridDataPointsFilter::OctreeGridDataPointsFilter
OctreeGridDataPointsFilter()
OctreeGridDataPointsFilter::MedoidSampler::operator()
bool operator()(Octree_< T, dim > &oc)
Definition: OctreeGrid.cpp:222
OctreeGridDataPointsFilter::CentroidSampler::operator()
bool operator()(Octree_< T, dim > &oc)
Definition: OctreeGrid.cpp:148
OctreeGridDataPointsFilter
Data Filter based on Octree representation.
Definition: OctreeGrid.h:54
OctreeGridDataPointsFilter::RandomPtsSampler::finalize
virtual bool finalize()
Definition: OctreeGrid.cpp:131
align_sequence.params
params
Definition: align_sequence.py:13
OctreeGridDataPointsFilter::FirstPtsSampler::operator()
bool operator()(Octree_< T, dim > &oc)
Definition: OctreeGrid.cpp:48
icp.data
data
Definition: icp.py:50
PointMatcher::DataPointsFilter
A data filter takes a point cloud as input, transforms it, and produces another point cloud as output...
Definition: PointMatcher.h:440
OctreeGridDataPointsFilter::availableParameters
static const ParametersDoc availableParameters()
Definition: OctreeGrid.h:77
PointMatcherSupport::Parametrizable::InvalidParameter
An exception thrown when one tries to fetch the value of an unexisting parameter.
Definition: Parametrizable.h:144
OctreeGrid.h
Octree_::visit
bool visit(Callback &cb)
Definition: octree.hpp:373
OctreeGridDataPointsFilter::sample
void sample(DataPoints &cloud)
Definition: OctreeGrid.cpp:333
PointMatcher::DataPoints::features
Matrix features
features of points in the cloud
Definition: PointMatcher.h:331
Octree_::isEmpty
bool isEmpty() const
Definition: octree.hpp:176
OctreeGridDataPointsFilter::FirstPtsSampler
Definition: OctreeGrid.h:90
icp_advance_api.dim
dim
Definition: icp_advance_api.py:152
OctreeGridDataPointsFilter::Parameters
Parametrizable::Parameters Parameters
Definition: OctreeGrid.h:63
OctreeGridDataPointsFilter::RandomPtsSampler
Definition: OctreeGrid.h:107
Octree_
Definition: octree.h:78
icp_advance_api.dist
dist
Definition: icp_advance_api.py:158
OctreeGridDataPointsFilter::inPlaceFilter
virtual void inPlaceFilter(DataPoints &cloud)
Apply these filters to a point cloud without copying.
Definition: OctreeGrid.cpp:318
Octree_::getData
DataContainer * getData()
Definition: octree.hpp:212
OctreeGridDataPointsFilter::filter
virtual DataPoints filter(const DataPoints &input)
Apply filters to input point cloud. This is the non-destructive version and returns a copy.
Definition: OctreeGrid.cpp:310
OctreeGridDataPointsFilter::MedoidSampler::MedoidSampler
MedoidSampler(DataPoints &dp)
Definition: OctreeGrid.cpp:215


libpointmatcher
Author(s):
autogenerated on Mon Jul 1 2024 02:22:43