Gestalt.h
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 #pragma once
36 
37 #include "PointMatcher.h"
38 
40 template<typename T>
42 {
49 
50  typedef typename PointMatcher<T>::Vector Vector;
51  typedef typename PointMatcher<T>::Matrix Matrix;
54 
55  inline static const std::string description()
56  {
57  return "Gestalt descriptors filter.";
58  }
59  inline static const ParametersDoc availableParameters()
60  {
61  return {
62  {"ratio", "ratio of points to keep with random subsampling. Matrix (normal, density, etc.) will be associated to all points in the same bin.", "0.1", "0.0000001", "0.9999999", &P::Comp<T>},
63  {"radius", "is the radius of the gestalt descriptor, will be divided into 4 circular and 8 radial bins = 32 bins", "5", "0.1", "2147483647", &P::Comp<T>},
64  {"knn", "determined how many points are used to compute the normals. Direct link with the rapidity of the computation (large = fast). Technically, limit over which a box is splitted in two", "7", "3", "2147483647", &P::Comp<unsigned>},
65  {"vSizeX", "Dimension of each voxel cell in x direction", "1.0", "-inf", "inf", &P::Comp<T>},
66  {"vSizeY", "Dimension of each voxel cell in y direction", "1.0", "-inf", "inf", &P::Comp<T>},
67  {"vSizeZ", "Dimension of each voxel cell in z direction", "1.0", "-inf", "inf", &P::Comp<T>},
68  {"keepMeans", "whether the means should be added as descriptors to the resulting cloud", "0"},
69  {"maxBoxDim", "maximum length of a box above which the box is discarded", "inf"},
70  {"averageExistingDescriptors", "whether the filter keep the existing point descriptors and average them or should it drop them", "1"},
71  {"maxTimeWindow", "maximum spread of times in a surfel", "inf"},
72  {"keepNormals", "whether the normals should be added as descriptors to the resulting cloud", "1"},
73  {"keepEigenValues", "whether the eigen values should be added as descriptors to the resulting cloud", "0"},
74  {"keepEigenVectors", "whether the eigen vectors should be added as descriptors to the resulting cloud", "0"},
75  {"keepCovariances", "whether the covariances should be added as descriptors to the resulting cloud", "0"},
76  {"keepGestaltFeatures", "whether the Gestalt features shall be added to the resulting cloud", "1"}
77  };
78  }
79 
80  const T ratio;
81  const T radius;
82  const unsigned knn;
83  const T vSizeX;
84  const T vSizeY;
85  const T vSizeZ;
86  const T maxBoxDim;
88  const bool keepMeans;
90  const bool keepNormals;
91  const bool keepEigenValues;
92  const bool keepEigenVectors;
93  const bool keepCovariances;
94  const bool keepGestaltFeatures;
95 
96 
97  public:
100  virtual DataPoints filter(const DataPoints& input);
101  virtual void inPlaceFilter(DataPoints& cloud);
102 
103  typename PointMatcher<T>::Vector serializeGestaltMatrix(const Matrix& gestaltFeatures) const;
104  typename PointMatcher<T>::Vector calculateAngles(const Matrix& points, const Eigen::Matrix<T,3,1>&) const;
105  typename PointMatcher<T>::Vector calculateRadii(const Matrix& points, const Eigen::Matrix<T,3,1>&) const;
106 
107 
108  protected:
109  struct BuildData
110  {
111  typedef std::vector<int> Indices;
112  typedef typename DataPoints::View View;
113  typedef typename Eigen::Matrix<std::int64_t, Eigen::Dynamic, Eigen::Dynamic> Int64Matrix;
114  typedef typename Eigen::Matrix<std::int64_t, 1, Eigen::Dynamic> Int64Vector;
115 
121  boost::optional<View> normals;
122  boost::optional<View> means;
123  boost::optional<View> eigenValues;
124  boost::optional<View> eigenVectors;
125  boost::optional<View> covariance;
126  boost::optional<View> gestaltMeans;
127  boost::optional<View> gestaltVariances;
128  boost::optional<View> gestaltShapes;
129  boost::optional<View> warpedXYZ;
132 
136  times(times),
138  {
139  const int pointsCount(features.cols());
140  indices.reserve(pointsCount);
141  for (int i = 0; i < pointsCount; ++i)
142  indices.push_back(i);
143  }
144  };
145 
146  struct CompareDim
147  {
148  const int dim;
151  bool operator() (const int& p0, const int& p1)
152  {
153  return buildData.features(dim, p0) <
154  buildData.features(dim, p1);
155  }
156  };
157 
158  protected:
159  void buildNew(BuildData& data, const int first, const int last, Vector&& minValues, Vector&& maxValues) const;
160  void fuseRange(BuildData& data, DataPoints& input, const int first, const int last) const;
161 
162 };
GestaltDataPointsFilter::CompareDim::operator()
bool operator()(const int &p0, const int &p1)
Definition: Gestalt.h:151
GestaltDataPointsFilter::BuildData::Int64Matrix
Eigen::Matrix< std::int64_t, Eigen::Dynamic, Eigen::Dynamic > Int64Matrix
Definition: Gestalt.h:113
GestaltDataPointsFilter::BuildData::descriptors
Matrix & descriptors
Definition: Gestalt.h:119
GestaltDataPointsFilter::radius
const T radius
Definition: Gestalt.h:81
GestaltDataPointsFilter::keepGestaltFeatures
const bool keepGestaltFeatures
Definition: Gestalt.h:94
GestaltDataPointsFilter::vSizeY
const T vSizeY
Definition: Gestalt.h:84
GestaltDataPointsFilter::ratio
const T ratio
Definition: Gestalt.h:80
Vector
PM::Vector Vector
Definition: pypoint_matcher_helper.h:55
GestaltDataPointsFilter::fuseRange
void fuseRange(BuildData &data, DataPoints &input, const int first, const int last) const
Definition: Gestalt.cpp:343
DataPointsFilter
PM::DataPointsFilter DataPointsFilter
Definition: pypoint_matcher_helper.h:22
GestaltDataPointsFilter::ParameterDoc
Parametrizable::ParameterDoc ParameterDoc
Definition: Gestalt.h:46
GestaltDataPointsFilter::availableParameters
static const ParametersDoc availableParameters()
Definition: Gestalt.h:59
GestaltDataPointsFilter::BuildData::indicesToKeep
Indices indicesToKeep
Definition: Gestalt.h:117
GestaltDataPointsFilter::keepCovariances
const bool keepCovariances
Definition: Gestalt.h:93
GestaltDataPointsFilter::Matrix
PointMatcher< T >::Matrix Matrix
Definition: Gestalt.h:51
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
GestaltDataPointsFilter::keepEigenValues
const bool keepEigenValues
Definition: Gestalt.h:91
GestaltDataPointsFilter::CompareDim::dim
const int dim
Definition: Gestalt.h:148
GestaltDataPointsFilter::BuildData::Indices
std::vector< int > Indices
Definition: Gestalt.h:111
GestaltDataPointsFilter::BuildData::gestaltMeans
boost::optional< View > gestaltMeans
Definition: Gestalt.h:126
GestaltDataPointsFilter::BuildData::features
Matrix & features
Definition: Gestalt.h:118
GestaltDataPointsFilter::BuildData::times
Int64Matrix & times
Definition: Gestalt.h:120
GestaltDataPointsFilter::BuildData::unfitPointsCount
int unfitPointsCount
Definition: Gestalt.h:131
Matrix
PM::Matrix Matrix
Definition: pypoint_matcher_helper.h:59
GestaltDataPointsFilter::BuildData::outputInsertionPoint
int outputInsertionPoint
Definition: Gestalt.h:130
PointMatcher
Functions and classes that are dependant on scalar type are defined in this templatized class.
Definition: PointMatcher.h:130
GestaltDataPointsFilter::BuildData::View
DataPoints::View View
Definition: Gestalt.h:112
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
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
testing::internal::string
::std::string string
Definition: gtest.h:1979
GestaltDataPointsFilter::BuildData::warpedXYZ
boost::optional< View > warpedXYZ
Definition: Gestalt.h:129
GestaltDataPointsFilter::BuildData::Int64Vector
Eigen::Matrix< std::int64_t, 1, Eigen::Dynamic > Int64Vector
Definition: Gestalt.h:114
GestaltDataPointsFilter
Gestalt descriptors filter as described in Bosse & Zlot ICRA 2013.
Definition: Gestalt.h:41
GestaltDataPointsFilter::~GestaltDataPointsFilter
virtual ~GestaltDataPointsFilter()
Definition: Gestalt.h:99
PointMatcher::DataPoints::InvalidField
An exception thrown when one tries to access features or descriptors unexisting or of wrong dimension...
Definition: PointMatcher.h:250
GestaltDataPointsFilter::Parametrizable
PointMatcherSupport::Parametrizable Parametrizable
Definition: Gestalt.h:43
GestaltDataPointsFilter::filter
virtual DataPoints filter(const DataPoints &input)
Definition: Gestalt.cpp:79
GestaltDataPointsFilter::BuildData::normals
boost::optional< View > normals
Definition: Gestalt.h:121
GestaltDataPointsFilter::maxTimeWindow
const T maxTimeWindow
Definition: Gestalt.h:87
GestaltDataPointsFilter::serializeGestaltMatrix
PointMatcher< T >::Vector serializeGestaltMatrix(const Matrix &gestaltFeatures) const
Definition: Gestalt.cpp:583
PointMatcherSupport::Parametrizable::ParametersDoc
std::vector< ParameterDoc > ParametersDoc
The documentation of all parameters.
Definition: Parametrizable.h:187
GestaltDataPointsFilter::BuildData::gestaltVariances
boost::optional< View > gestaltVariances
Definition: Gestalt.h:127
align_sequence.params
params
Definition: align_sequence.py:13
GestaltDataPointsFilter::InvalidField
PointMatcher< T >::DataPoints::InvalidField InvalidField
Definition: Gestalt.h:53
GestaltDataPointsFilter::keepNormals
const bool keepNormals
Definition: Gestalt.h:90
GestaltDataPointsFilter::averageExistingDescriptors
const bool averageExistingDescriptors
Definition: Gestalt.h:89
icp.data
data
Definition: icp.py:50
GestaltDataPointsFilter::P
PointMatcherSupport::Parametrizable P
Definition: Gestalt.h:44
GestaltDataPointsFilter::DataPoints
PointMatcher< T >::DataPoints DataPoints
Definition: Gestalt.h:52
PointMatcher::Matrix
Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dense matrix over ScalarType.
Definition: PointMatcher.h:169
GestaltDataPointsFilter::ParametersDoc
Parametrizable::ParametersDoc ParametersDoc
Definition: Gestalt.h:47
GestaltDataPointsFilter::maxBoxDim
const T maxBoxDim
Definition: Gestalt.h:86
GestaltDataPointsFilter::inPlaceFilter
virtual void inPlaceFilter(DataPoints &cloud)
Definition: Gestalt.cpp:88
GestaltDataPointsFilter::calculateAngles
PointMatcher< T >::Vector calculateAngles(const Matrix &points, const Eigen::Matrix< T, 3, 1 > &) const
Definition: Gestalt.cpp:598
PointMatcherSupport::Parametrizable::InvalidParameter
An exception thrown when one tries to fetch the value of an unexisting parameter.
Definition: Parametrizable.h:144
GestaltDataPointsFilter::BuildData::indices
Indices indices
Definition: Gestalt.h:116
PointMatcher::Vector
Eigen::Matrix< T, Eigen::Dynamic, 1 > Vector
A vector over ScalarType.
Definition: PointMatcher.h:161
GestaltDataPointsFilter::BuildData
Definition: Gestalt.h:109
GestaltDataPointsFilter::keepMeans
const bool keepMeans
Definition: Gestalt.h:88
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
GestaltDataPointsFilter::vSizeX
const T vSizeX
Definition: Gestalt.h:83
PointMatcherSupport::Parametrizable
The superclass of classes that are constructed using generic parameters. This class provides the para...
Definition: Parametrizable.h:141
GestaltDataPointsFilter::CompareDim
Definition: Gestalt.h:146
GestaltDataPointsFilter::knn
const unsigned knn
Definition: Gestalt.h:82
PointMatcherSupport::Parametrizable::ParameterDoc
The documentation of a parameter.
Definition: Parametrizable.h:160
GestaltDataPointsFilter::keepEigenVectors
const bool keepEigenVectors
Definition: Gestalt.h:92
PointMatcher::DataPoints::View
Eigen::Block< Matrix > View
A view on a feature or descriptor.
Definition: PointMatcher.h:210
GestaltDataPointsFilter::BuildData::BuildData
BuildData(Matrix &features, Matrix &descriptors, Int64Matrix &times)
Definition: Gestalt.h:133
GestaltDataPointsFilter::vSizeZ
const T vSizeZ
Definition: Gestalt.h:85
GestaltDataPointsFilter::BuildData::means
boost::optional< View > means
Definition: Gestalt.h:122
GestaltDataPointsFilter::CompareDim::buildData
const BuildData & buildData
Definition: Gestalt.h:149
GestaltDataPointsFilter::Vector
PointMatcher< T >::Vector Vector
Definition: Gestalt.h:50
GestaltDataPointsFilter::description
static const std::string description()
Definition: Gestalt.h:55
PointMatcher.h
public interface
GestaltDataPointsFilter::CompareDim::CompareDim
CompareDim(const int dim, const BuildData &buildData)
Definition: Gestalt.h:150
PointMatcherSupport::Parametrizable::Parameters
std::map< std::string, Parameter > Parameters
Parameters stored as a map of string->string.
Definition: Parametrizable.h:199
GestaltDataPointsFilter::InvalidParameter
Parametrizable::InvalidParameter InvalidParameter
Definition: Gestalt.h:48


libpointmatcher
Author(s):
autogenerated on Mon Jan 1 2024 03:24:42