Gestalt.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 "Gestalt.h"
36 
38 
39 #include <cmath>
40 
41 // Eigenvalues
42 #include "Eigen/QR"
43 #include "Eigen/Eigenvalues"
44 
45 #include "PointMatcherPrivate.h"
46 
47 #include <vector>
48 
50 
51 // GestaltDataPointsFilter
52 
53 // Constructor
54 template<typename T>
56  PointMatcher<T>::DataPointsFilter("GestaltDataPointsFilter",
57  GestaltDataPointsFilter::availableParameters(), params),
58  ratio(Parametrizable::get<T>("ratio")),
59  radius(Parametrizable::get<T>("radius")),
60  knn(Parametrizable::get<int>("knn")),
61  vSizeX(Parametrizable::get<T>("vSizeX")),
62  vSizeY(Parametrizable::get<T>("vSizeY")),
63  vSizeZ(Parametrizable::get<T>("vSizeZ")),
64  maxBoxDim(Parametrizable::get<T>("maxBoxDim")),
65  maxTimeWindow(Parametrizable::get<T>("maxTimeWindow")),
66  keepMeans(Parametrizable::get<bool>("keepMeans")),
67  averageExistingDescriptors(Parametrizable::get<bool>("averageExistingDescriptors")),
68  keepNormals(Parametrizable::get<bool>("keepNormals")),
69  keepEigenValues(Parametrizable::get<bool>("keepEigenValues")),
70  keepEigenVectors(Parametrizable::get<bool>("keepEigenVectors")),
71  keepCovariances(Parametrizable::get<bool>("keepCovariances")),
72  keepGestaltFeatures(Parametrizable::get<bool>("keepGestaltFeatures"))
73 {
74 }
75 
76 // Compute
77 template<typename T>
80 {
81  DataPoints output(input);
82  inPlaceFilter(output);
83  return output;
84 }
85 
86 // In-place filter
87 template<typename T>
89 {
90  typedef typename DataPoints::View View;
91  typedef typename DataPoints::Label Label;
92  typedef typename DataPoints::Labels Labels;
93  typedef typename DataPoints::TimeView TimeView;
94 
95  const int pointsCount(cloud.features.cols());
96  const int featDim(cloud.features.rows());
97  const int descDim(cloud.descriptors.rows());
98  const unsigned int labelDim(cloud.descriptorLabels.size());
99 
100  int insertDim(0);
101  if (averageExistingDescriptors)
102  {
103  // TODO: this should be in the form of an assert
104  // Validate descriptors and labels
105  for(unsigned int i = 0; i < labelDim; ++i)
106  insertDim += cloud.descriptorLabels[i].span;
107  if (insertDim != descDim)
108  throw InvalidField("GestaltDataPointsFilter: Error, descriptor labels do not match descriptor data");
109  }
110 
111  // Compute space requirement for new descriptors
112  const int dimNormals(featDim-1);
113  const int dimMeans(featDim-1);
114  const int dimEigValues(featDim-1);
115  const int dimEigVectors((featDim-1)*(featDim-1));
116  const int dimCovariances((featDim-1)*(featDim-1));
117  const int dimGestalt = 32;
118 
119  // Allocate space for new descriptors
120  Labels cloudLabels, timeLabels;
121 
122  if (keepNormals)
123  cloudLabels.push_back(Label("normals", dimNormals));
124  if (keepMeans)
125  cloudLabels.push_back(Label("means", dimMeans));
126  if (keepEigenValues)
127  cloudLabels.push_back(Label("eigValues", dimEigValues));
128  if (keepEigenVectors)
129  cloudLabels.push_back(Label("eigVectors", dimEigVectors));
130  if (keepCovariances)
131  cloudLabels.push_back(Label("covariance", dimCovariances));
132  if (keepGestaltFeatures)
133  {
134  cloudLabels.push_back(Label("gestaltMeans", dimGestalt));
135  cloudLabels.push_back(Label("gestaltVariances", dimGestalt));
136  cloudLabels.push_back(Label("warpedXYZ", 3));
137  cloudLabels.push_back(Label("gestaltShapes", 2));
138  }
139  timeLabels.push_back(Label("time", 3));
140 
141  cloud.allocateDescriptors(cloudLabels);
142  cloud.allocateTimes(timeLabels);
143 
144  // we keep build data on stack for reentrant behaviour
145  View cloudExistingDescriptors(cloud.descriptors.block(0,0,cloud.descriptors.rows(),cloud.descriptors.cols()));
146  TimeView cloudExistingTimes(cloud.times.block(0,0,cloud.times.rows(),cloud.times.cols()));
147  BuildData buildData(cloud.features, cloud.descriptors, cloud.times);
148 
149  // get views
150  if (keepNormals)
151  buildData.normals = cloud.getDescriptorViewByName("normals");
152  if(keepMeans)
153  buildData.means = cloud.getDescriptorViewByName("means");
154  if (keepEigenValues)
155  buildData.eigenValues = cloud.getDescriptorViewByName("eigValues");
156  if (keepEigenVectors)
157  buildData.eigenVectors = cloud.getDescriptorViewByName("eigVectors");
158  if (keepCovariances)
159  buildData.covariance = cloud.getDescriptorViewByName("covariance");
160  if (keepGestaltFeatures)
161  {
162  buildData.gestaltMeans = cloud.getDescriptorViewByName("gestaltMeans");
163  buildData.gestaltVariances = cloud.getDescriptorViewByName("gestaltVariances");
164  buildData.warpedXYZ = cloud.getDescriptorViewByName("warpedXYZ");
165  buildData.gestaltShapes = cloud.getDescriptorViewByName("gestaltShapes");
166  }
167  // build the new point cloud
168  buildNew(
169  buildData,
170  0,
171  pointsCount,
172  cloud.features.rowwise().minCoeff(),
173  cloud.features.rowwise().maxCoeff()
174  );
175 
176  // buildData.indicesToKeep contains all the indices where we want Gestalt features at
177  fuseRange(buildData, cloud, 0, pointsCount);
178 
179  // Bring the data we keep to the front of the arrays then
180  // wipe the leftover unused space.
181  std::sort(buildData.indicesToKeep.begin(), buildData.indicesToKeep.end());
182  const int ptsOut = buildData.indicesToKeep.size();
183  for (int i = 0; i < ptsOut; ++i)
184  {
185  const int k = buildData.indicesToKeep[i];
186  assert(i <= k);
187  cloud.features.col(i) = cloud.features.col(k);
188  cloud.times.col(i) = cloud.times.col(k);
189  if (cloud.descriptors.rows() != 0)
190  cloud.descriptors.col(i) = cloud.descriptors.col(k);
191  if(keepNormals)
192  buildData.normals->col(i) = buildData.normals->col(k);
193  if(keepMeans)
194  buildData.means->col(i) = buildData.means->col(k);
195  if(keepEigenValues)
196  buildData.eigenValues->col(i) = buildData.eigenValues->col(k);
197  if(keepEigenVectors)
198  buildData.eigenVectors->col(i) = buildData.eigenVectors->col(k);
199  if(keepCovariances)
200  buildData.covariance->col(i) = buildData.covariance->col(k);
201  if(keepGestaltFeatures)
202  {
203  buildData.gestaltMeans->col(i) = buildData.gestaltMeans->col(k);
204  buildData.gestaltVariances->col(i) = buildData.gestaltVariances->col(k);
205  buildData.warpedXYZ->col(i) = buildData.warpedXYZ->col(k);
206  buildData.gestaltShapes->col(i) = buildData.gestaltShapes->col(k);
207  }
208  }
209  cloud.features.conservativeResize(Eigen::NoChange, ptsOut);
210  cloud.descriptors.conservativeResize(Eigen::NoChange, ptsOut);
211  cloud.times.conservativeResize(Eigen::NoChange, ptsOut);
212  // warning if some points were dropped
213  if(buildData.unfitPointsCount != 0)
214  LOG_INFO_STREAM(" GestaltDataPointsFilter - Could not compute normal for " << buildData.unfitPointsCount << " pts.");
215 }
216 
217 #include "VoxelGrid.h"
218 template<typename T>
220  BuildData& data, const int first, const int last,
221  Vector&& minValues, Vector&& maxValues) const
222 {
223  using Voxel = typename VoxelGridDataPointsFilter<T>::Voxel;
224 
225  const T minBoundX = minValues.x() / vSizeX;
226  const T maxBoundX = maxValues.x() / vSizeX;
227  const T minBoundY = minValues.y() / vSizeY;
228  const T maxBoundY = maxValues.y() / vSizeY;
229  const T minBoundZ = minValues.z() / vSizeZ;
230  const T maxBoundZ = maxValues.z() / vSizeZ;
231 
232  // number of divisions is total size / voxel size voxels of equal length + 1
233  // with remaining space
234  const unsigned int numDivX = 1 + maxBoundX - minBoundX;
235  const unsigned int numDivY = 1 + maxBoundY - minBoundY;;
236  const unsigned int numDivZ = 1 + maxBoundZ - minBoundZ;
237  const unsigned int numVox = numDivX * numDivY * numDivZ;
238 
239  // Assume point cloud is randomly ordered
240  // compute a linear index of the following type
241  // i, j, k are the component indices
242  // nx, ny number of divisions in x and y components
243  // idx = i + j * nx + k * nx * ny
244  const int numPoints = last - first;
245  std::vector<unsigned int> indices(numPoints);
246 
247  // vector to hold the first point in a voxel
248  // this point will be ovewritten in the input cloud with
249  // the output value
250  std::vector<Voxel> voxels;
251 
252  // try allocating vector. If too big return error
253  try
254  {
255  voxels = std::vector<Voxel>(numVox);
256  }
257  catch (std::bad_alloc&)
258  {
259  throw InvalidParameter((boost::format("GestaltDataPointsFilter: Memory allocation error with %1% voxels. Try increasing the voxel dimensions.") % numVox).str());
260  }
261 
262  const int featDim(data.features.rows());
263 
264  for (int p = 0; p < numPoints; ++p)
265  {
266  const unsigned int i = floor(data.features(0,p)/vSizeX - minBoundX);
267  const unsigned int j = floor(data.features(1,p)/vSizeY- minBoundY);
268  unsigned int k = 0;
269  unsigned int idx;
270  if ( featDim == 4 )
271  {
272  k = floor(data.features(2,p)/vSizeZ - minBoundZ);
273  idx = i + j * numDivX + k * numDivX * numDivY;
274  }
275  else
276  {
277  idx = i + j * numDivX;
278  }
279 
280  const unsigned int pointsInVox = voxels[idx].numPoints + 1;
281 
282  if (pointsInVox == 1)
283  {
284  voxels[idx].firstPoint = p;
285  }
286 
287  voxels[idx].numPoints = pointsInVox;
288 
289  indices[p] = idx;
290 
291  }
292 
293  // store which points contain voxel position
294  std::vector<unsigned int> pointsToKeep;
295 
296  // take centers of voxels for now
297  // Todo revert to random point selection within cell
298  for (int p = 0; p < numPoints ; ++p)
299  {
300  const unsigned int idx = indices[p];
301  const unsigned int firstPoint = voxels[idx].firstPoint;
302 
303  // Choose random point in voxel
304  const int randomIndex = std::rand() % numPoints;
305  for (int f = 0; f < (featDim - 1); ++f)
306  {
307  data.features(f,firstPoint) = data.features(f,randomIndex);
308  }
309  }
310 
311  for (unsigned int idx = 0; idx < numVox; ++idx)
312  {
313  const unsigned int numPoints = voxels[idx].numPoints;
314  const unsigned int firstPoint = voxels[idx].firstPoint;
315 
316  if (numPoints > 0)
317  {
318  // get back voxel indices in grid format
319  // If we are in the last division, the voxel is smaller in size
320  // We adjust the center as from the end of the last voxel to the bounding area
321 
322  pointsToKeep.push_back(firstPoint);
323  }
324  }
325 
326  const unsigned int nbPointsToKeep(pointsToKeep.size());
327  // now the keypoints are in pointsToKeep
328  // downsample with ratio
329  for(unsigned int i=0; i<nbPointsToKeep; ++i)
330  {
331  const float r = (float)std::rand()/(float)RAND_MAX;
332  if(r < ratio)
333  {
334  // Keep points with their descriptors
335  const int k = pointsToKeep[i];
336  // Mark the indices which will be part of the final data
337  data.indicesToKeep.push_back(k);
338  }
339  }
340 }
341 
342 template<typename T>
344  BuildData& data, DataPoints& input, const int first, const int last) const
345 {
346  using namespace PointMatcherSupport;
347 
348  typedef typename Eigen::Matrix<std::int64_t, Eigen::Dynamic, Eigen::Dynamic> Int64Matrix;
349 
350  const unsigned int nbIdxToKeep(data.indicesToKeep.size());
351  const int inputFeatDim(input.features.cols());
352 
353  std::vector<int> indicesToKeepStrict;
354  for (unsigned int i = 0; i < nbIdxToKeep ; ++i)
355  {
356  Eigen::Matrix<T,3,1> keyPoint;
357  keyPoint = input.features.col(data.indicesToKeep[i]);
358 
359  // Define a search box around each keypoint to search for nearest neighbours.
360  const T minBoundX = keyPoint(0,0) - radius;
361  const T maxBoundX = keyPoint(0,0) + radius;
362  const T minBoundY = keyPoint(1,0) - radius;
363  const T maxBoundY = keyPoint(1,0) + radius;
364  const T minBoundZ = keyPoint(2,0) - radius;
365  const T maxBoundZ = keyPoint(2,0) + radius;
366  // iterate over data and find in- / outliers
367  Eigen::Matrix<T,3,1> feature;
368  std::vector<int> goodIndices;
369  for (int j = 0; j < inputFeatDim; ++j)
370  {
371  feature = input.features.col(j);
372  if(feature(0,0) <= maxBoundX && feature(0,0) >= minBoundX &&
373  feature(1,0) <= maxBoundY && feature(1,0) >= minBoundY &&
374  feature(2,0) <= maxBoundZ && feature(2,0) >= minBoundZ &&
375  keyPoint != feature)
376  {
377  goodIndices.push_back(j);
378  }
379  }
380  const int colCount = goodIndices.size();
381  // if empty neighbourhood unfit the point
382  if (colCount == 0)
383  {
384  ++(data.unfitPointsCount);
385  continue;
386  }
387 
388  const int featDim(data.features.rows());
389 
390  Matrix d(featDim-1, colCount);
391  Int64Matrix t(1, colCount);
392 
393  for (int j = 0; j < colCount; ++j)
394  {
395  d.col(j) = data.features.block(0,data.indices[goodIndices[j]],featDim-1, 1);
396  t.col(j) = data.times.col(data.indices[goodIndices[j]]);
397  }
398 
399  const Vector mean = d.rowwise().sum() / T(colCount);
400  const Matrix NN = d.colwise() - mean;
401  const std::int64_t minTime = t.minCoeff();
402  const std::int64_t maxTime = t.maxCoeff();
403  const std::int64_t meanTime = t.sum() / T(colCount);
404  // compute covariance
405  const Matrix C(NN * NN.transpose());
406  Vector eigenVa = Vector::Identity(featDim-1, 1);
407  Matrix eigenVe = Matrix::Identity(featDim-1, featDim-1);
408  // Ensure that the matrix is suited for eigenvalues calculation
409  if(keepNormals || keepEigenValues || keepEigenVectors || keepCovariances || keepGestaltFeatures)
410  {
411  if(C.fullPivHouseholderQr().rank()+1 >= featDim-1)
412  {
413  const Eigen::EigenSolver<Matrix> solver(C);
414  eigenVa = solver.eigenvalues().real();
415  eigenVe = solver.eigenvectors().real();
416  }
417  else
418  {
419  data.unfitPointsCount += colCount;
420  continue;
421  }
422  }
423  Eigen::Matrix<T,3,1> normal, newX, newY;
424  Eigen::Matrix<T,3,3> newBasis;
425  double planarity = 0.;
426  double cylindricality = 0.;
427 
428  if(keepNormals || keepGestaltFeatures)
429  {
430  // calculate orientation of NN
431  normal = computeNormal<T>(eigenVa, eigenVe);
432 
433  if(keepGestaltFeatures)
434  {
435  Vector eigenVaSort = sortEigenValues<T>(eigenVa);
436  planarity = 2 * (eigenVaSort(1) - eigenVaSort(0))/eigenVaSort.sum();
437  cylindricality = (eigenVaSort(2) - eigenVaSort(1))/eigenVaSort.sum();
438  // project normal on horizontal plane
439  Eigen::Matrix<T,3,1> up, base;
440  up << 0,0,1;
441  base << 1,0,0;
442  newX << normal(0), normal(1), 0;
443  newX.normalize();
444  newY = up.cross(newX);
445  newY = newY / newY.norm();
446  // form a new basis with world z-axis and projected x & y-axis
447  newBasis << newX(0), newY(0), up(0),
448  newX(1), newY(1), up(1),
449  newX(2), newY(2), up(2);
450 
451  // discard keypoints with high planarity
452  if(planarity > 0.9)
453  {
454  data.unfitPointsCount += colCount;
455  continue;
456  }
457  // discard keypoints with normal too close to vertical
458  if(acos(normal.dot(up)) < abs(10 * M_PI/180))
459  {
460  data.unfitPointsCount += colCount;
461  continue;
462  }
463 
464  // define features in new basis that is oriented with the covariance
465  for (int j = 0; j < colCount; ++j)
466  {
467  data.warpedXYZ->col(j) = ((data.features.block(0,j,3,1) - keyPoint).transpose() * newBasis).transpose();
468  }
469  }
470  }
471  Vector angles(colCount), radii(colCount), heights(colCount);
472  Matrix gestaltMeans(4, 8), gestaltVariances(4, 8), numOfValues(4, 8);
473  if(keepGestaltFeatures)
474  {
475  // calculate the polar coordinates of points
476  angles = GestaltDataPointsFilter::calculateAngles(*data.warpedXYZ, keyPoint);
477  radii = GestaltDataPointsFilter::calculateRadii(*data.warpedXYZ, keyPoint);
478  heights = data.warpedXYZ->row(2);
479 
480  // sort points into Gestalt bins
481  const T angularBinWidth = M_PI/4;
482  const T radialBinWidth = radius/4;
483  Eigen::MatrixXi indices(2, colCount);
484  gestaltMeans = Matrix::Zero(4, 8);
485  gestaltVariances = Matrix::Zero(4, 8);
486  numOfValues = Matrix::Zero(4, 8);
487 
488  for (int it=0; it < colCount; ++it)
489  {
490  indices(0,it) = floor(radii(it)/radialBinWidth);
491  // if value exceeds borders of bin -> put in outmost bin
492  if(indices(0,it) > 3)
493  // this case should never happen - just in case
494  indices(0,it) = 3;
495  indices(1,it) = floor(angles(it)/angularBinWidth);
496  if(indices(1,it) > 7)
497  indices(1,it) = 7;
498  gestaltMeans(indices(0,it), indices(1,it)) += heights(it);
499  ++(numOfValues(indices(0,it), indices(1,it)));
500  }
501 
502  for (int radial=0; radial < 4; ++radial)
503  {
504  for (int angular = 0; angular < 8; ++angular)
505  {
506  if (numOfValues(radial, angular) > 0)
507  {
508  gestaltMeans(radial, angular) = gestaltMeans(radial, angular)/numOfValues(radial, angular);
509  }
510  }
511  }
512  for (int it=0; it < colCount; ++it)
513  {
514  gestaltVariances(indices(0,it), indices(1,it)) += (heights(it)-gestaltMeans(indices(0,it), indices(1,it))) * (heights(it)-gestaltMeans(indices(0,it), indices(1,it)));
515  }
516  for (int radial=0; radial < 4; ++radial)
517  {
518  for (int angular = 0; angular < 8; ++angular)
519  {
520  // if bins are == 0 -> propagate with value in bin closer to keypoint
521  if (gestaltMeans(radial,angular) == 0 && radial > 0)
522  {
523  gestaltMeans(radial, angular) = gestaltMeans(radial-1, angular);
524  gestaltVariances(radial, angular) = gestaltVariances(radial-1, angular);
525  }
526  else if (numOfValues(radial, angular) > 0)
527  {
528  gestaltVariances(radial, angular) = gestaltVariances(radial, angular)/numOfValues(radial, angular);
529  }
530  }
531  }
532  }
533 
534  Vector serialEigVector;
535  if(keepEigenVectors)
536  serialEigVector = serializeEigVec<T>(eigenVe);
537  Vector serialCovVector;
538  if(keepCovariances)
539  serialCovVector = serializeEigVec<T>(C);
540  Vector serialGestaltMeans;
541  Vector serialGestaltVariances;
542  if(keepGestaltFeatures)
543  {
544  serialGestaltMeans = GestaltDataPointsFilter::serializeGestaltMatrix(gestaltMeans);
545  serialGestaltVariances = GestaltDataPointsFilter::serializeGestaltMatrix(gestaltVariances);
546  }
547  // some safety check
548  if(data.descriptors.rows() != 0)
549  assert(data.descriptors.cols() != 0);
550 
551  // write the updated times: min, max, mean
552  data.times(0, data.indicesToKeep[i]) = minTime;
553  data.times(1, data.indicesToKeep[i]) = maxTime;
554  data.times(2, data.indicesToKeep[i]) = meanTime;
555 
556  // Build new descriptors
557  if(keepNormals)
558  data.normals->col(data.indicesToKeep[i]) = normal;
559  if(keepMeans)
560  data.means->col(data.indicesToKeep[i]) = mean;
561  if(keepEigenValues)
562  data.eigenValues->col(data.indicesToKeep[i]) = eigenVa;
563  if(keepEigenVectors)
564  data.eigenVectors->col(data.indicesToKeep[i]) = serialEigVector;
565  if(keepCovariances)
566  data.covariance->col(data.indicesToKeep[i]) = serialCovVector;
567  if(keepGestaltFeatures)
568  {
569  // preserve gestalt features
570  data.gestaltMeans->col(data.indicesToKeep[i]) = serialGestaltMeans;
571  data.gestaltVariances->col(data.indicesToKeep[i]) = serialGestaltVariances;
572  (*data.gestaltShapes)(0,data.indicesToKeep[i]) = planarity;
573  (*data.gestaltShapes)(1,data.indicesToKeep[i]) = cylindricality;
574  }
575  // all went well so far - so keep this keypoint
576  indicesToKeepStrict.push_back(data.indicesToKeep[i]);
577  }
578  data.indicesToKeep = indicesToKeepStrict;
579 }
580 
581 template<typename T>
584 {
585  // serialize the gestalt descriptors
586  const int dim = gestaltFeatures.rows() * gestaltFeatures.cols();
587  Vector output(dim);
588  for(int k=0; k < gestaltFeatures.rows(); ++k)
589  {
590  output.segment(k*gestaltFeatures.cols(), gestaltFeatures.cols()) =
591  gestaltFeatures.row(k).transpose();
592  }
593  return output;
594 }
595 
596 template<typename T>
599  const Matrix& points, const Eigen::Matrix<T,3,1>& keyPoint) const
600 {
601  const unsigned int dim(points.cols());
602  Vector angles(dim);
603 
604  for (unsigned int i = 0; i < dim; ++i)
605  {
606  angles(i) = atan2(points(0,i), points(1,i));
607  if (angles(i) < 0)
608  angles(i) += (2 * M_PI);
609  }
610 
611  return angles;
612 }
613 
614 template<typename T>
617  const Matrix& points, const Eigen::Matrix<T,3,1>& keyPoint) const
618 {
619  const unsigned int dim(points.cols());
620  Vector radii(dim);
621 
622  for (unsigned int i = 0; i < dim; ++i)
623  {
624  radii(i) = sqrt((points(0,i)) * (points(0,i)) + (points(1,i)) * (points(1,i)));
625  }
626  return radii;
627 }
628 
629 template struct GestaltDataPointsFilter<float>;
630 template struct GestaltDataPointsFilter<double>;
Label
DataPoints::Label Label
Definition: pypoint_matcher_helper.h:17
GestaltDataPointsFilter::fuseRange
void fuseRange(BuildData &data, DataPoints &input, const int first, const int last) const
Definition: Gestalt.cpp:343
PointMatcher::DataPoints::descriptorLabels
Labels descriptorLabels
labels of descriptors
Definition: PointMatcher.h:334
DataPointsFilter
PM::DataPointsFilter DataPointsFilter
Definition: pypoint_matcher_helper.h:22
GestaltDataPointsFilter::BuildData::indicesToKeep
Indices indicesToKeep
Definition: Gestalt.h:117
GestaltDataPointsFilter::Matrix
PointMatcher< T >::Matrix Matrix
Definition: Gestalt.h:51
compute_overlap.knn
int knn
Definition: compute_overlap.py:128
GestaltDataPointsFilter::calculateRadii
PointMatcher< T >::Vector calculateRadii(const Matrix &points, const Eigen::Matrix< T, 3, 1 > &) const
Definition: Gestalt.cpp:616
build_map.T
T
Definition: build_map.py:34
PointMatcher::DataPoints::Labels
A vector of Label.
Definition: PointMatcher.h:229
LOG_INFO_STREAM
#define LOG_INFO_STREAM(args)
Definition: PointMatcherPrivate.h:58
GestaltDataPointsFilter::BuildData::gestaltMeans
boost::optional< View > gestaltMeans
Definition: Gestalt.h:126
GestaltDataPointsFilter::BuildData::unfitPointsCount
int unfitPointsCount
Definition: Gestalt.h:131
PointMatcher
Functions and classes that are dependant on scalar type are defined in this templatized class.
Definition: PointMatcher.h:130
PointMatcherPrivate.h
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
VoxelGridDataPointsFilter::Voxel
Definition: VoxelGrid.h:83
GestaltDataPointsFilter::BuildData::gestaltShapes
boost::optional< View > gestaltShapes
Definition: Gestalt.h:128
GestaltDataPointsFilter::BuildData::eigenVectors
boost::optional< View > eigenVectors
Definition: Gestalt.h:124
GestaltDataPointsFilter::GestaltDataPointsFilter
GestaltDataPointsFilter(const Parameters &params=Parameters())
Definition: Gestalt.cpp:55
GestaltDataPointsFilter::BuildData::warpedXYZ
boost::optional< View > warpedXYZ
Definition: Gestalt.h:129
utils.h
GestaltDataPointsFilter
Gestalt descriptors filter as described in Bosse & Zlot ICRA 2013.
Definition: Gestalt.h:41
Gestalt.h
PointMatcher::DataPoints::InvalidField
An exception thrown when one tries to access features or descriptors unexisting or of wrong dimension...
Definition: PointMatcher.h:250
GestaltDataPointsFilter::filter
virtual DataPoints filter(const DataPoints &input)
Definition: Gestalt.cpp:79
GestaltDataPointsFilter::BuildData::normals
boost::optional< View > normals
Definition: Gestalt.h:121
GestaltDataPointsFilter::serializeGestaltMatrix
PointMatcher< T >::Vector serializeGestaltMatrix(const Matrix &gestaltFeatures) const
Definition: Gestalt.cpp:583
GestaltDataPointsFilter::BuildData::gestaltVariances
boost::optional< View > gestaltVariances
Definition: Gestalt.h:127
align_sequence.params
params
Definition: align_sequence.py:13
Int64Matrix
PM::Int64Matrix Int64Matrix
Definition: pypoint_matcher_helper.h:61
PointMatcher::DataPoints::descriptors
Matrix descriptors
descriptors of points in the cloud, might be empty
Definition: PointMatcher.h:333
PointMatcher::DataPoints::TimeView
Eigen::Block< Int64Matrix > TimeView
A view on a time.
Definition: PointMatcher.h:212
icp.data
data
Definition: icp.py:50
GestaltDataPointsFilter::inPlaceFilter
virtual void inPlaceFilter(DataPoints &cloud)
Definition: Gestalt.cpp:88
Labels
DataPoints::Labels Labels
Definition: pypoint_matcher_helper.h:18
GestaltDataPointsFilter::calculateAngles
PointMatcher< T >::Vector calculateAngles(const Matrix &points, const Eigen::Matrix< T, 3, 1 > &) const
Definition: Gestalt.cpp:598
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::Vector
Eigen::Matrix< T, Eigen::Dynamic, 1 > Vector
A vector over ScalarType.
Definition: PointMatcher.h:161
GestaltDataPointsFilter::BuildData
Definition: Gestalt.h:109
GestaltDataPointsFilter::Parameters
Parametrizable::Parameters Parameters
Definition: Gestalt.h:45
GestaltDataPointsFilter::BuildData::eigenValues
boost::optional< View > eigenValues
Definition: Gestalt.h:123
GestaltDataPointsFilter::buildNew
void buildNew(BuildData &data, const int first, const int last, Vector &&minValues, Vector &&maxValues) const
Definition: Gestalt.cpp:219
GestaltDataPointsFilter::BuildData::covariance
boost::optional< View > covariance
Definition: Gestalt.h:125
PointMatcher::DataPoints::times
Int64Matrix times
time associated to each points, might be empty
Definition: PointMatcher.h:335
PointMatcher::DataPoints::allocateTimes
void allocateTimes(const Labels &newLabels)
Make sure a vector of time of given names exist.
Definition: pointmatcher/DataPoints.cpp:629
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
icp_advance_api.dim
dim
Definition: icp_advance_api.py:152
PointMatcher::DataPoints::View
Eigen::Block< Matrix > View
A view on a feature or descriptor.
Definition: PointMatcher.h:210
GestaltDataPointsFilter::BuildData::means
boost::optional< View > means
Definition: Gestalt.h:122
PointMatcherSupport
Functions and classes that are not dependant on scalar type are defined in this namespace.
Definition: Bibliography.cpp:45
GestaltDataPointsFilter::Vector
PointMatcher< T >::Vector Vector
Definition: Gestalt.h:50
PointMatcher::DataPoints::getDescriptorViewByName
ConstView getDescriptorViewByName(const std::string &name) const
Get a const view on a descriptor by name, throw an exception if it does not exist.
Definition: pointmatcher/DataPoints.cpp:555
PointMatcher::DataPoints::allocateDescriptors
void allocateDescriptors(const Labels &newLabels)
Make sure a vector of descriptors of given names exist.
Definition: pointmatcher/DataPoints.cpp:526
PointMatcher::DataPoints::Label
The name for a certain number of dim.
Definition: PointMatcher.h:221


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