void DataPointsFiltersImpl<T>::SamplingSurfaceNormalDataPointsFilter::inPlaceFilter(
DataPoints& cloud)
{
typedef Matrix Features;
typedef typename DataPoints::View View;
typedef typename DataPoints::Label Label;
typedef typename DataPoints::Labels Labels;
const int pointsCount(cloud.features.cols());
const int featDim(cloud.features.rows());
const int descDim(cloud.descriptors.rows());
int insertDim(0);
if (averageExistingDescriptors)
{
// TODO: this should be in the form of an assert
// Validate descriptors and labels
for(unsigned int i = 0; i < cloud.descriptorLabels.size(); i++)
insertDim += cloud.descriptorLabels[i].span;
if (insertDim != descDim)
throw InvalidField("SamplingSurfaceNormalDataPointsFilter: Error, descriptor labels do not match descriptor data");
}
// Compute space requirement for new descriptors
const int dimNormals(featDim-1);
const int dimDensities(1);
const int dimEigValues(featDim-1);
const int dimEigVectors((featDim-1)*(featDim-1));
// Allocate space for new descriptors
Labels cloudLabels;
if (keepNormals)
cloudLabels.push_back(Label("normals", dimNormals));
if (keepDensities)
cloudLabels.push_back(Label("densities", dimDensities));
if (keepEigenValues)
cloudLabels.push_back(Label("eigValues", dimEigValues));
if (keepEigenVectors)
cloudLabels.push_back(Label("eigVectors", dimEigVectors));
cloud.allocateDescriptors(cloudLabels);
// we keep build data on stack for reentrant behaviour
View cloudExistingDescriptors(cloud.descriptors.block(0,0,cloud.descriptors.rows(),cloud.descriptors.cols()));
BuildData buildData(cloud.features, cloud.descriptors);
// get views
if (keepNormals)
buildData.normals = cloud.getDescriptorViewByName("normals");
if (keepDensities)
buildData.densities = cloud.getDescriptorViewByName("densities");
if (keepEigenValues)
buildData.eigenValues = cloud.getDescriptorViewByName("eigValues");
if (keepEigenVectors)
buildData.eigenVectors = cloud.getDescriptorViewByName("eigVectors");
// build the new point cloud
buildNew(
buildData,
0,
pointsCount,
cloud.features.rowwise().minCoeff(),
cloud.features.rowwise().maxCoeff()
);
// Bring the data we keep to the front of the arrays then
// wipe the leftover unused space.
std::sort(buildData.indicesToKeep.begin(), buildData.indicesToKeep.end());
int ptsOut = buildData.indicesToKeep.size();
for (int i = 0; i < ptsOut; i++){
int k = buildData.indicesToKeep[i];
assert(i <= k);
cloud.features.col(i) = cloud.features.col(k);
if (cloud.descriptors.rows() != 0)
cloud.descriptors.col(i) = cloud.descriptors.col(k);
if(keepNormals)
buildData.normals->col(i) = buildData.normals->col(k);
if(keepDensities)
(*buildData.densities)(0,i) = (*buildData.densities)(0,k);
if(keepEigenValues)
buildData.eigenValues->col(i) = buildData.eigenValues->col(k);
if(keepEigenVectors)
buildData.eigenVectors->col(i) = buildData.eigenVectors->col(k);
}
cloud.features.conservativeResize(Eigen::NoChange, ptsOut);
cloud.descriptors.conservativeResize(Eigen::NoChange, ptsOut);
// warning if some points were dropped
if(buildData.unfitPointsCount != 0)
LOG_INFO_STREAM(" SamplingSurfaceNormalDataPointsFilter - Could not compute normal for " << buildData.unfitPointsCount << " pts.");
}
void DataPointsFiltersImpl<T>::SurfaceNormalDataPointsFilter::inPlaceFilter(
DataPoints& cloud)
{
typedef typename DataPoints::View View;
typedef typename DataPoints::Label Label;
typedef typename DataPoints::Labels Labels;
typedef typename MatchersImpl<T>::KDTreeMatcher KDTreeMatcher;
typedef typename PointMatcher<T>::Matches Matches;
const int pointsCount(cloud.features.cols());
const int featDim(cloud.features.rows());
const int descDim(cloud.descriptors.rows());
// Validate descriptors and labels
int insertDim(0);
for(unsigned int i = 0; i < cloud.descriptorLabels.size(); i++)
insertDim += cloud.descriptorLabels[i].span;
if (insertDim != descDim)
throw InvalidField("SurfaceNormalDataPointsFilter: Error, descriptor labels do not match descriptor data");
// Reserve memory for new descriptors
const int dimNormals(featDim-1);
const int dimDensities(1);
const int dimEigValues(featDim-1);
const int dimEigVectors((featDim-1)*(featDim-1));
//const int dimMatchedIds(knn);
boost::optional<View> normals;
boost::optional<View> densities;
boost::optional<View> eigenValues;
boost::optional<View> eigenVectors;
boost::optional<View> matchedValues;
Labels cloudLabels;
if (keepNormals)
cloudLabels.push_back(Label("normals", dimNormals));
if (keepDensities)
cloudLabels.push_back(Label("densities", dimDensities));
if (keepEigenValues)
cloudLabels.push_back(Label("eigValues", dimEigValues));
if (keepEigenVectors)
cloudLabels.push_back(Label("eigVectors", dimEigVectors));
cloud.allocateDescriptors(cloudLabels);
if (keepNormals)
normals = cloud.getDescriptorViewByName("normals");
if (keepDensities)
densities = cloud.getDescriptorViewByName("densities");
if (keepEigenValues)
eigenValues = cloud.getDescriptorViewByName("eigValues");
if (keepEigenVectors)
eigenVectors = cloud.getDescriptorViewByName("eigVectors");
// TODO: implement keepMatchedIds
// if (keepMatchedIds)
// {
// cloud.allocateDescriptor("normals", dimMatchedIds);
// matchedValues = cloud.getDescriptorViewByName("normals");
// }
// Build kd-tree
Parametrizable::Parameters param;
boost::assign::insert(param) ( "knn", toParam(knn) );
boost::assign::insert(param) ( "epsilon", toParam(epsilon) );
KDTreeMatcher matcher(param);
matcher.init(cloud);
Matches matches(typename Matches::Dists(knn, pointsCount), typename Matches::Ids(knn, pointsCount));
matches = matcher.findClosests(cloud);
// Search for surrounding points and compute descriptors
int degenerateCount(0);
for (int i = 0; i < pointsCount; ++i)
{
// Mean of nearest neighbors (NN)
Matrix d(featDim-1, knn);
for(int j = 0; j < int(knn); j++)
{
const int refIndex(matches.ids(j,i));
d.col(j) = cloud.features.block(0, refIndex, featDim-1, 1);
}
const Vector mean = d.rowwise().sum() / T(knn);
const Matrix NN = d.colwise() - mean;
const Matrix C(NN * NN.transpose());
Vector eigenVa = Vector::Identity(featDim-1, 1);
Matrix eigenVe = Matrix::Identity(featDim-1, featDim-1);
// Ensure that the matrix is suited for eigenvalues calculation
if(keepNormals || keepEigenValues || keepEigenVectors)
{
if(C.fullPivHouseholderQr().rank()+1 >= featDim-1)
{
const Eigen::EigenSolver<Matrix> solver(C);
eigenVa = solver.eigenvalues().real();
eigenVe = solver.eigenvectors().real();
}
else
{
//std::cout << "WARNING: Matrix C needed for eigen decomposition is degenerated. Expected cause: no noise in data" << std::endl;
++degenerateCount;
}
}
if(keepNormals)
normals->col(i) = computeNormal(eigenVa, eigenVe);
if(keepDensities)
(*densities)(0, i) = computeDensity(NN);
if(keepEigenValues)
eigenValues->col(i) = eigenVa;
if(keepEigenVectors)
eigenVectors->col(i) = serializeEigVec(eigenVe);
}
if (degenerateCount)
{
LOG_WARNING_STREAM("WARNING: Matrix C needed for eigen decomposition was degenerated in " << degenerateCount << " points over " << pointsCount << " (" << float(degenerateCount)*100.f/float(pointsCount) << " %)");
}
}
