00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #pragma once
00036
00037 #include "PointMatcher.h"
00038
00040 template<typename T>
00041 struct GestaltDataPointsFilter: public PointMatcher<T>::DataPointsFilter
00042 {
00043 typedef PointMatcherSupport::Parametrizable Parametrizable;
00044 typedef PointMatcherSupport::Parametrizable P;
00045 typedef Parametrizable::Parameters Parameters;
00046 typedef Parametrizable::ParameterDoc ParameterDoc;
00047 typedef Parametrizable::ParametersDoc ParametersDoc;
00048 typedef Parametrizable::InvalidParameter InvalidParameter;
00049
00050 typedef typename PointMatcher<T>::Vector Vector;
00051 typedef typename PointMatcher<T>::Matrix Matrix;
00052 typedef typename PointMatcher<T>::DataPoints DataPoints;
00053 typedef typename PointMatcher<T>::DataPoints::InvalidField InvalidField;
00054
00055 inline static const std::string description()
00056 {
00057 return "Gestalt descriptors filter.";
00058 }
00059 inline static const ParametersDoc availableParameters()
00060 {
00061 return {
00062 {"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>},
00063 {"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>},
00064 {"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>},
00065 {"vSizeX", "Dimension of each voxel cell in x direction", "1.0", "-inf", "inf", &P::Comp<T>},
00066 {"vSizeY", "Dimension of each voxel cell in y direction", "1.0", "-inf", "inf", &P::Comp<T>},
00067 {"vSizeZ", "Dimension of each voxel cell in z direction", "1.0", "-inf", "inf", &P::Comp<T>},
00068 {"keepMeans", "whether the means should be added as descriptors to the resulting cloud", "0"},
00069 {"maxBoxDim", "maximum length of a box above which the box is discarded", "inf"},
00070 {"averageExistingDescriptors", "whether the filter keep the existing point descriptors and average them or should it drop them", "1"},
00071 {"maxTimeWindow", "maximum spread of times in a surfel", "inf"},
00072 {"keepNormals", "whether the normals should be added as descriptors to the resulting cloud", "1"},
00073 {"keepEigenValues", "whether the eigen values should be added as descriptors to the resulting cloud", "0"},
00074 {"keepEigenVectors", "whether the eigen vectors should be added as descriptors to the resulting cloud", "0"},
00075 {"keepCovariances", "whether the covariances should be added as descriptors to the resulting cloud", "0"},
00076 {"keepGestaltFeatures", "whether the Gestalt features shall be added to the resulting cloud", "1"}
00077 };
00078 }
00079
00080 const T ratio;
00081 const T radius;
00082 const unsigned knn;
00083 const T vSizeX;
00084 const T vSizeY;
00085 const T vSizeZ;
00086 const T maxBoxDim;
00087 const T maxTimeWindow;
00088 const bool keepMeans;
00089 const bool averageExistingDescriptors;
00090 const bool keepNormals;
00091 const bool keepEigenValues;
00092 const bool keepEigenVectors;
00093 const bool keepCovariances;
00094 const bool keepGestaltFeatures;
00095
00096
00097 public:
00098 GestaltDataPointsFilter(const Parameters& params = Parameters());
00099 virtual ~GestaltDataPointsFilter() {}
00100 virtual DataPoints filter(const DataPoints& input);
00101 virtual void inPlaceFilter(DataPoints& cloud);
00102
00103 typename PointMatcher<T>::Vector serializeGestaltMatrix(const Matrix& gestaltFeatures) const;
00104 typename PointMatcher<T>::Vector calculateAngles(const Matrix& points, const Eigen::Matrix<T,3,1>&) const;
00105 typename PointMatcher<T>::Vector calculateRadii(const Matrix& points, const Eigen::Matrix<T,3,1>&) const;
00106
00107
00108 protected:
00109 struct BuildData
00110 {
00111 typedef std::vector<int> Indices;
00112 typedef typename DataPoints::View View;
00113 typedef typename Eigen::Matrix<std::int64_t, Eigen::Dynamic, Eigen::Dynamic> Int64Matrix;
00114 typedef typename Eigen::Matrix<std::int64_t, 1, Eigen::Dynamic> Int64Vector;
00115
00116 Indices indices;
00117 Indices indicesToKeep;
00118 Matrix& features;
00119 Matrix& descriptors;
00120 Int64Matrix& times;
00121 boost::optional<View> normals;
00122 boost::optional<View> means;
00123 boost::optional<View> eigenValues;
00124 boost::optional<View> eigenVectors;
00125 boost::optional<View> covariance;
00126 boost::optional<View> gestaltMeans;
00127 boost::optional<View> gestaltVariances;
00128 boost::optional<View> gestaltShapes;
00129 boost::optional<View> warpedXYZ;
00130 int outputInsertionPoint;
00131 int unfitPointsCount;
00132
00133 BuildData(Matrix& features, Matrix& descriptors, Int64Matrix& times):
00134 features(features),
00135 descriptors(descriptors),
00136 times(times),
00137 unfitPointsCount(0)
00138 {
00139 const int pointsCount(features.cols());
00140 indices.reserve(pointsCount);
00141 for (int i = 0; i < pointsCount; ++i)
00142 indices.push_back(i);
00143 }
00144 };
00145
00146 struct CompareDim
00147 {
00148 const int dim;
00149 const BuildData& buildData;
00150 CompareDim(const int dim, const BuildData& buildData):dim(dim),buildData(buildData){}
00151 bool operator() (const int& p0, const int& p1)
00152 {
00153 return buildData.features(dim, p0) <
00154 buildData.features(dim, p1);
00155 }
00156 };
00157
00158 protected:
00159 void buildNew(BuildData& data, const int first, const int last, Vector&& minValues, Vector&& maxValues) const;
00160 void fuseRange(BuildData& data, DataPoints& input, const int first, const int last) const;
00161
00162 };