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) << " %)"); } }