VoxelGrid.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 "VoxelGrid.h"
36 
37 
38 // VoxelGridDataPointsFilter
39 template <typename T>
41  vSizeX(1),
42  vSizeY(1),
43  vSizeZ(1),
44  useCentroid(true),
45  averageExistingDescriptors(true)
46 {
47 }
48 
49 template <typename T>
51  PointMatcher<T>::DataPointsFilter("VoxelGridDataPointsFilter",
52  VoxelGridDataPointsFilter::availableParameters(), params),
53  vSizeX(Parametrizable::get<T>("vSizeX")),
54  vSizeY(Parametrizable::get<T>("vSizeY")),
55  vSizeZ(Parametrizable::get<T>("vSizeZ")),
56  useCentroid(Parametrizable::get<bool>("useCentroid")),
57  averageExistingDescriptors(Parametrizable::get<bool>("averageExistingDescriptors"))
58 {
59 }
60 
61 template <typename T>
64 {
65  DataPoints output(input);
66  inPlaceFilter(output);
67  return output;
68 }
69 
70 template <typename T>
72 {
73  const unsigned int numPoints(cloud.features.cols());
74  const int featDim(cloud.features.rows());
75  const int descDim(cloud.descriptors.rows());
76  const int timeDim(cloud.times.rows());
77  const unsigned int labelDim(cloud.descriptorLabels.size());
78 
79  assert (featDim == 3 || featDim == 4);
80 
81  int insertDim(0);
82  if (averageExistingDescriptors)
83  {
84  // TODO: this should be in the form of an assert
85  // Validate descriptors and labels
86  for(unsigned int i = 0; i < labelDim ; ++i)
87  insertDim += cloud.descriptorLabels[i].span;
88  if (insertDim != descDim)
89  throw InvalidField("VoxelGridDataPointsFilter: Error, descriptor labels do not match descriptor data");
90  //TODO: timeDim too?
91  }
92 
93  // TODO: Check that the voxel size is not too small, given the size of the data
94  // TODO: Check if sizes are positive
95 
96  // Calculate number of divisions along each axis
97  Vector minValues = cloud.features.rowwise().minCoeff();
98  Vector maxValues = cloud.features.rowwise().maxCoeff();
99 
100  const T minBoundX = minValues.x() / vSizeX;
101  const T maxBoundX = maxValues.x() / vSizeX;
102  const T minBoundY = minValues.y() / vSizeY;
103  const T maxBoundY = maxValues.y() / vSizeY;
104  T minBoundZ = 0;
105  T maxBoundZ = 0;
106 
107  if (featDim == 4)
108  {
109  minBoundZ = minValues.z() / vSizeZ;
110  maxBoundZ = maxValues.z() / vSizeZ;
111  }
112 
113  // number of divisions is total size / voxel size voxels of equal length + 1
114  // with remaining space
115  const unsigned int numDivX = 1 + maxBoundX - minBoundX;
116  const unsigned int numDivY = 1 + maxBoundY - minBoundY;;
117  unsigned int numDivZ = 0;
118 
119  // If a 3D point cloud
120  if (featDim == 4 )
121  numDivZ = 1 + maxBoundZ - minBoundZ;
122 
123  unsigned int numVox = numDivX * numDivY;
124  if (featDim == 4)
125  numVox *= numDivZ;
126 
127  if(numVox == 0)
128  {
129  throw InvalidParameter("VoxelGridDataPointsFilter: The number of voxel couldn't be computed. There might be NaNs in the feature matrix. Use the fileter RemoveNaNDataPointsFilter before this one if it's the case.");
130  }
131 
132  // Assume point cloud is randomly ordered
133  // compute a linear index of the following type
134  // i, j, k are the component indices
135  // nx, ny number of divisions in x and y components
136  // idx = i + j * nx + k * nx * ny
137  std::vector<unsigned int> indices(numPoints);
138 
139  // vector to hold the first point in a voxel
140  // this point will be ovewritten in the input cloud with
141  // the output value
142 
143  std::vector<Voxel> voxels;
144 
145  // try allocating vector. If too big return error
146  try
147  {
148  voxels = std::vector<Voxel>(numVox);
149  }
150  catch (std::bad_alloc&)
151  {
152  throw InvalidParameter((boost::format("VoxelGridDataPointsFilter: Memory allocation error with %1% voxels. Try increasing the voxel dimensions.") % numVox).str());
153  }
154 
155  for (unsigned int p = 0; p < numPoints; ++p)
156  {
157  const unsigned int i = floor(cloud.features(0,p)/vSizeX - minBoundX);
158  const unsigned int j = floor(cloud.features(1,p)/vSizeY- minBoundY);
159  unsigned int k = 0;
160  unsigned int idx = 0;
161  if ( featDim == 4 )
162  {
163  k = floor(cloud.features(2,p)/vSizeZ - minBoundZ);
164  idx = i + j * numDivX + k * numDivX * numDivY;
165  }
166  else
167  {
168  idx = i + j * numDivX;
169  }
170 
171  const unsigned int pointsInVox = voxels[idx].numPoints + 1;
172 
173  if (pointsInVox == 1)
174  {
175  voxels[idx].firstPoint = p;
176  }
177 
178  voxels[idx].numPoints = pointsInVox;
179 
180  indices[p] = idx;
181 
182  }
183 
184 
185  // store which points contain voxel position
186  std::vector<unsigned int> pointsToKeep;
187 
188  // Store voxel centroid in output
189  if (useCentroid)
190  {
191  // Iterate through the indices and sum values to compute centroid
192  for (unsigned int p = 0; p < numPoints ; ++p)
193  {
194  const unsigned int idx = indices[p];
195  const unsigned int firstPoint = voxels[idx].firstPoint;
196 
197  // If this is the first point in the voxel, leave as is
198  // if not sum up this point for centroid calculation
199  if (firstPoint != p)
200  {
201  // Sum up features and descriptors (if we are also averaging descriptors)
202 
203  for (int f = 0; f < (featDim - 1); ++f)
204  {
205  cloud.features(f,firstPoint) += cloud.features(f,p);
206  }
207 
208  if (averageExistingDescriptors)
209  {
210  for (int d = 0; d < descDim; ++d)
211  {
212  cloud.descriptors(d,firstPoint) += cloud.descriptors(d,p);
213  }
214  for (int d = 0; d < timeDim; ++d)
215  {
216  cloud.times(d,firstPoint) += cloud.times(d,p);
217  }
218  }
219  }
220  }
221 
222  // Now iterating through the voxels
223  // Normalize sums to get centroid (average)
224  // Some voxels may be empty and are discarded
225  for(unsigned int idx = 0; idx < numVox; ++idx)
226  {
227  const unsigned int numPoints = voxels[idx].numPoints;
228  const unsigned int firstPoint = voxels[idx].firstPoint;
229  if(numPoints > 0)
230  {
231  for (int f = 0; f < (featDim - 1); ++f)
232  cloud.features(f,firstPoint) /= numPoints;
233 
234  if (averageExistingDescriptors)
235  {
236  for ( int d = 0; d < descDim; ++d )
237  cloud.descriptors(d,firstPoint) /= numPoints;
238  for ( int d = 0; d < timeDim; ++d )
239  cloud.times(d,firstPoint) /= numPoints;
240  }
241  pointsToKeep.push_back(firstPoint);
242  }
243  }
244  }
245  else
246  {
247  // Although we don't sum over the features, we may still need to sum the descriptors
248  if (averageExistingDescriptors)
249  {
250  // Iterate through the indices and sum values to compute centroid
251  for (unsigned int p = 0; p < numPoints ; ++p)
252  {
253  const unsigned int idx = indices[p];
254  const unsigned int firstPoint = voxels[idx].firstPoint;
255 
256  // If this is the first point in the voxel, leave as is
257  // if not sum up this point for centroid calculation
258  if (firstPoint != p)
259  {
260  for (int d = 0; d < descDim; ++d)
261  {
262  cloud.descriptors(d,firstPoint) += cloud.descriptors(d,p);
263  }
264  for (int d = 0; d < timeDim; ++d)
265  {
266  cloud.times(d,firstPoint) += cloud.times(d,p);
267  }
268  }
269  }
270  }
271 
272  for (unsigned int idx = 0; idx < numVox; ++idx)
273  {
274  const unsigned int numPoints = voxels[idx].numPoints;
275  const unsigned int firstPoint = voxels[idx].firstPoint;
276 
277  if (numPoints > 0)
278  {
279  // get back voxel indices in grid format
280  // If we are in the last division, the voxel is smaller in size
281  // We adjust the center as from the end of the last voxel to the bounding area
282  unsigned int i = 0;
283  unsigned int j = 0;
284  unsigned int k = 0;
285  if (featDim == 4)
286  {
287  k = idx / (numDivX * numDivY);
288  if (k == numDivZ)
289  cloud.features(3,firstPoint) = maxValues.z() - (k-1) * vSizeZ/2;
290  else
291  cloud.features(3,firstPoint) = k * vSizeZ + vSizeZ/2;
292  }
293 
294  j = (idx - k * numDivX * numDivY) / numDivX;
295  if (j == numDivY)
296  cloud.features(2,firstPoint) = maxValues.y() - (j-1) * vSizeY/2;
297  else
298  cloud.features(2,firstPoint) = j * vSizeY + vSizeY / 2;
299 
300  i = idx - k * numDivX * numDivY - j * numDivX;
301  if (i == numDivX)
302  cloud.features(1,firstPoint) = maxValues.x() - (i-1) * vSizeX/2;
303  else
304  cloud.features(1,firstPoint) = i * vSizeX + vSizeX / 2;
305 
306  // Descriptors : normalize if we are averaging or keep as is
307  if (averageExistingDescriptors)
308  {
309  for ( int d = 0; d < descDim; ++d)
310  cloud.descriptors(d,firstPoint) /= numPoints;
311  for ( int d = 0; d < timeDim; ++d)
312  cloud.times(d,firstPoint) /= numPoints;
313  }
314  pointsToKeep.push_back(firstPoint);
315  }
316  }
317  }
318 
319  // Move the points to be kept to the start
320  // Bring the data we keep to the front of the arrays then
321  // wipe the leftover unused space.
322  std::sort(pointsToKeep.begin(), pointsToKeep.end());
323  int numPtsOut = pointsToKeep.size();
324  for (int i = 0; i < numPtsOut; ++i)
325  {
326  const int k = pointsToKeep[i];
327  assert(i <= k);
328  cloud.features.col(i) = cloud.features.col(k);
329  if (cloud.descriptors.rows() != 0)
330  cloud.descriptors.col(i) = cloud.descriptors.col(k);
331  if (cloud.times.rows() != 0)
332  cloud.times.col(i) = cloud.times.col(k);
333 
334  }
335 
336  cloud.conservativeResize(numPtsOut);
337 
338  //cloud.features.conservativeResize(Eigen::NoChange, numPtsOut);
339  //
340  //if (cloud.descriptors.rows() != 0)
341  // cloud.descriptors.conservativeResize(Eigen::NoChange, numPtsOut);
342  //if (cloud.times.rows() != 0)
343  // cloud.times.conservativeResize(Eigen::NoChange, numPtsOut)
344 }
345 
346 template struct VoxelGridDataPointsFilter<float>;
347 template struct VoxelGridDataPointsFilter<double>;
348 
349 
VoxelGridDataPointsFilter::VoxelGridDataPointsFilter
VoxelGridDataPointsFilter()
Definition: VoxelGrid.cpp:40
VoxelGridDataPointsFilter::Parameters
Parametrizable::Parameters Parameters
Definition: VoxelGrid.h:49
PointMatcher::DataPoints::descriptorLabels
Labels descriptorLabels
labels of descriptors
Definition: PointMatcher.h:334
build_map.T
T
Definition: build_map.py:34
PointMatcher
Functions and classes that are dependant on scalar type are defined in this templatized class.
Definition: PointMatcher.h:130
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
VoxelGridDataPointsFilter::filter
virtual DataPoints filter(const DataPoints &input)
Apply filters to input point cloud. This is the non-destructive version and returns a copy.
Definition: VoxelGrid.cpp:63
PointMatcher::DataPoints::InvalidField
An exception thrown when one tries to access features or descriptors unexisting or of wrong dimension...
Definition: PointMatcher.h:250
VoxelGridDataPointsFilter::Vector
PointMatcher< T >::Vector Vector
Definition: VoxelGrid.h:55
align_sequence.params
params
Definition: align_sequence.py:13
VoxelGridDataPointsFilter
Definition: VoxelGrid.h:40
PointMatcher::DataPoints::descriptors
Matrix descriptors
descriptors of points in the cloud, might be empty
Definition: PointMatcher.h:333
PointMatcher::DataPointsFilter
A data filter takes a point cloud as input, transforms it, and produces another point cloud as output...
Definition: PointMatcher.h:440
InvalidParameter
Parametrizable::InvalidParameter InvalidParameter
Definition: pypoint_matcher_helper.h:42
VoxelGrid.h
PointMatcher::DataPoints::features
Matrix features
features of points in the cloud
Definition: PointMatcher.h:331
PointMatcher::DataPoints::times
Int64Matrix times
time associated to each points, might be empty
Definition: PointMatcher.h:335
PointMatcherSupport::Parametrizable
The superclass of classes that are constructed using generic parameters. This class provides the para...
Definition: Parametrizable.h:141
PointMatcherSupport::get
const M::mapped_type & get(const M &m, const typename M::key_type &k)
Definition: Bibliography.cpp:57
PointMatcher::DataPoints::conservativeResize
void conservativeResize(Index pointCount)
Resize the cloud to pointCount points, conserving existing ones.
Definition: pointmatcher/DataPoints.cpp:328
VoxelGridDataPointsFilter::inPlaceFilter
virtual void inPlaceFilter(DataPoints &cloud)
Apply these filters to a point cloud without copying.
Definition: VoxelGrid.cpp:71


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