39 #include "Eigen/Eigenvalues" 45 #include <boost/format.hpp> 93 const int pointsCount(cloud.
features.cols());
94 const int featDim(cloud.
features.rows());
100 for(
unsigned int i = 0; i < labelDim ; ++i)
102 if (insertDim != descDim)
103 throw InvalidField(
"SurfaceNormalDataPointsFilter: Error, descriptor labels do not match descriptor data");
106 const int dimNormals(featDim-1);
107 const int dimDensities(1);
108 const int dimEigValues(featDim-1);
109 const int dimEigVectors((featDim-1)*(featDim-1));
111 const int dimMeanDist(1);
113 boost::optional<View> normals;
114 boost::optional<View> densities;
115 boost::optional<View> eigenValues;
116 boost::optional<View> eigenVectors;
117 boost::optional<View> matchedValues;
118 boost::optional<View> matchIds;
119 boost::optional<View> meanDists;
123 cloudLabels.push_back(
Label(
"normals", dimNormals));
125 cloudLabels.push_back(
Label(
"densities", dimDensities));
127 cloudLabels.push_back(
Label(
"eigValues", dimEigValues));
129 cloudLabels.push_back(
Label(
"eigVectors", dimEigVectors));
131 cloudLabels.push_back(
Label(
"matchedIds",
knn));
133 cloudLabels.push_back(
Label(
"meanDists", dimMeanDist));
154 boost::assign::insert(param) (
"knn",
toParam(
knn) );
161 Matches
matches(
typename Matches::Dists(
knn, pointsCount),
typename Matches::Ids(
knn, pointsCount));
162 matches = matcher.findClosests(cloud);
165 int degenerateCount(0);
166 for (
int i = 0; i < pointsCount; ++i)
168 bool isDegenerate =
false;
173 for(
int j = 0; j < int(
knn); ++j)
175 if (matches.dists(j,i) != Matches::InvalidDist)
177 const int refIndex(matches.ids(j,i));
178 d.col(realKnn) = cloud.
features.block(0, refIndex, featDim-1, 1);
182 d.conservativeResize(Eigen::NoChange, realKnn);
184 const Vector mean = d.rowwise().sum() /
T(realKnn);
185 const Matrix NN = d.colwise() - mean;
187 const Matrix C((NN * NN.transpose()) /
T(realKnn));
188 Vector eigenVa = Vector::Zero(featDim-1, 1);
189 Matrix eigenVe = Matrix::Zero(featDim-1, featDim-1);
193 if(C.fullPivHouseholderQr().rank()+1 >= featDim-1)
195 const Eigen::EigenSolver<Matrix> solver(C);
196 eigenVa = solver.eigenvalues().real();
197 eigenVe = solver.eigenvectors().real();
201 const std::vector<size_t> idx = sortIndexes<T>(eigenVa);
202 const size_t idxSize = idx.size();
203 Vector tmp_eigenVa = eigenVa;
204 Matrix tmp_eigenVe = eigenVe;
205 for(
size_t i=0; i<idxSize; ++i)
207 eigenVa(i,0) = tmp_eigenVa(idx[i], 0);
208 eigenVe.col(i) = tmp_eigenVe.col(idx[i]);
223 normals->col(i) = eigenVe.col(0);
225 normals->col(i) = computeNormal<T>(eigenVa, eigenVe);
228 normals->col(i) = normals->col(i).cwiseMax(-1.0).cwiseMin(1.0);
233 (*densities)(0, i) = 0.;
235 (*densities)(0, i) = computeDensity<T>(NN);
238 eigenValues->col(i) = eigenVa;
240 eigenVectors->col(i) = serializeEigVec<T>(eigenVe);
244 (*meanDists)(0, i) = std::numeric_limits<std::size_t>::max();
248 (*meanDists)(0, i) = (point - mean).norm();
256 matchIds.get() = matches.ids.template cast<T>();
261 for (
int i = 0; i < pointsCount; ++i)
263 const Vector currentNormal = normals->col(i);
264 Vector mean = Vector::Zero(featDim-1);
266 for(
int j = 0; j < int(
knn); ++j)
268 if (matches.dists(j,i) != Matches::InvalidDist)
270 const int refIndex(matches.ids(j,i));
271 const Vector normal = normals->col(refIndex);
272 if(currentNormal.dot(normal) > 0.)
281 normals->col(i) = mean /
T(n);
287 LOG_WARNING_STREAM(
"WARNING: Matrix C needed for eigen decomposition was degenerated in " << degenerateCount <<
" points over " << pointsCount <<
" (" <<
float(degenerateCount)*100.f/
float(pointsCount) <<
" %)");
Matrix descriptors
descriptors of points in the cloud, might be empty
std::string toParam(const S &value)
Return the a string value using lexical_cast.
ConstView getDescriptorViewByName(const std::string &name) const
Get a const view on a descriptor by name, throw an exception if it does not exist.
PointMatcher< T >::Matrix Matrix
The name for a certain number of dim.
virtual void inPlaceFilter(DataPoints &cloud)
Apply these filters to a point cloud without copying.
SurfaceNormalDataPointsFilter(const Parameters ¶ms=Parameters())
std::map< std::string, Parameter > Parameters
Parameters stored as a map of string->string.
Functions and classes that are not dependant on scalar type are defined in this namespace.
DataPoints::Labels Labels
Functions and classes that are dependant on scalar type are defined in this templatized class...
virtual DataPoints filter(const DataPoints &input)
Apply filters to input point cloud. This is the non-destructive version and returns a copy...
Result of the data-association step (Matcher::findClosests), before outlier rejection.
const M::mapped_type & get(const M &m, const typename M::key_type &k)
#define LOG_WARNING_STREAM(args)
The superclass of classes that are constructed using generic parameters. This class provides the para...
Eigen::Block< Matrix > View
A view on a feature or descriptor.
const bool keepMatchedIds
Surface normals estimation. Find the normal for every point using eigen-decomposition of neighbour po...
const bool keepEigenVectors
PointMatcher< T >::Vector Vector
Parametrizable::Parameters Parameters
Matrix features
features of points in the cloud
PM::DataPointsFilter DataPointsFilter
void allocateDescriptors(const Labels &newLabels)
Make sure a vector of descriptors of given names exist.
const bool keepEigenValues
PointMatcher< T >::DataPoints::InvalidField InvalidField
Labels descriptorLabels
labels of descriptors