void DataPointsFiltersImpl<T>::OrientNormalsDataPointsFilter::inPlaceFilter( DataPoints& cloud) { if (!cloud.descriptorExists("normals")) throw InvalidField("OrientNormalsDataPointsFilter: Error, cannot find normals in descriptors."); if (!cloud.descriptorExists("observationDirections")) throw InvalidField("OrientNormalsDataPointsFilter: Error, cannot find observation directions in descriptors."); BOOST_AUTO(normals, cloud.getDescriptorViewByName("normals")); const BOOST_AUTO(observationDirections, cloud.getDescriptorViewByName("observationDirections")); assert(normals.rows() == observationDirections.rows()); for (int i = 0; i < cloud.features.cols(); i++) { // Check normal orientation const Vector vecP = observationDirections.col(i); const Vector vecN = normals.col(i); const double scalar = vecP.dot(vecN); // Swap normal if(towardCenter) { if (scalar < 0) normals.col(i) = -vecN; } else { if (scalar > 0) normals.col(i) = -vecN; } } }
void InspectorsImpl<T>::AbstractVTKInspector::buildGenericAttributeStream(std::ostream& stream, const std::string& attribute, const std::string& nameTag, const DataPoints& cloud, const int forcedDim) { if (!cloud.descriptorExists(nameTag)) return; const BOOST_AUTO(desc, cloud.getDescriptorViewByName(nameTag)); assert(desc.rows() <= forcedDim); if(desc.rows() != 0) { if(attribute.compare("COLOR_SCALARS") == 0) { stream << attribute << " " << nameTag << " " << forcedDim << "\n"; stream << padWithOnes(desc, forcedDim, desc.cols()).transpose(); } else { stream << attribute << " " << nameTag << " float\n"; if(attribute.compare("SCALARS") == 0) stream << "LOOKUP_TABLE default\n"; stream << padWithZeros(desc, forcedDim, desc.cols()).transpose(); } stream << "\n"; } }
void DataPointsFiltersImpl<T>::ShadowDataPointsFilter::inPlaceFilter( DataPoints& cloud) { // Check if normals are present if (!cloud.descriptorExists("normals")) { throw InvalidField("ShadowDataPointsFilter, Error: cannot find normals in descriptors"); } const int dim = cloud.features.rows(); const BOOST_AUTO(normals, cloud.getDescriptorViewByName("normals")); int j = 0; for(int i=0; i < cloud.features.cols(); i++) { const Vector normal = normals.col(i).normalized(); const Vector point = cloud.features.block(0, i, dim-1, 1).normalized(); const T value = anyabs(normal.dot(point)); if(value > eps) // test to keep the points { cloud.features.col(j) = cloud.features.col(i); cloud.descriptors.col(j) = cloud.descriptors.col(i); j++; } } cloud.conservativeResize(j); }
void DataPointsFiltersImpl<T>::MaxDensityDataPointsFilter::inPlaceFilter( DataPoints& cloud) { typedef typename DataPoints::View View; typedef typename DataPoints::ConstView ConstView; // Force densities to be computed if (!cloud.descriptorExists("densities")) { throw InvalidField("MaxDensityDataPointsFilter: Error, no densities found in descriptors."); } const int nbPointsIn = cloud.features.cols(); View densities = cloud.getDescriptorViewByName("densities"); const T lastDensity = densities.maxCoeff(); const int nbSaturatedPts = (densities.cwise() == lastDensity).count(); // fill cloud values int j = 0; for (int i = 0; i < nbPointsIn; i++) { const T density(densities(0,i)); if (density > maxDensity) { const float r = (float)std::rand()/(float)RAND_MAX; float acceptRatio = maxDensity/density; // Handle saturation value of density if (density == lastDensity) { acceptRatio = acceptRatio * (1-nbSaturatedPts/nbPointsIn); } if (r < acceptRatio) { cloud.setColFrom(j, cloud, i); j++; } } else { cloud.setColFrom(j, cloud, i); j++; } } cloud.conservativeResize(j); }
void CovarianceSamplingDataPointsFilter<T>::inPlaceFilter(DataPoints& cloud) { const std::size_t featDim(cloud.features.rows()); assert(featDim == 4); //3D pts only //Check number of points const std::size_t nbPoints = cloud.getNbPoints(); if(nbSample >= nbPoints) return; //Check if there is normals info if (!cloud.descriptorExists("normals")) throw InvalidField("OrientNormalsDataPointsFilter: Error, cannot find normals in descriptors."); const auto& normals = cloud.getDescriptorViewByName("normals"); std::vector<std::size_t> keepIndexes; keepIndexes.resize(nbSample); ///---- Part A, as we compare the cloud with himself, the overlap is 100%, so we keep all points //A.1 and A.2 - Compute candidates std::vector<std::size_t> candidates ; candidates.resize(nbPoints); for (std::size_t i = 0; i < nbPoints; ++i) candidates[i] = i; const std::size_t nbCandidates = candidates.size(); //Compute centroid Vector3 center; for(std::size_t i = 0; i < featDim - 1; ++i) center(i) = T(0.); for (std::size_t i = 0; i < nbCandidates; ++i) for (std::size_t f = 0; f <= 3; ++f) center(f) += cloud.features(f,candidates[i]); for(std::size_t i = 0; i <= 3; ++i) center(i) /= T(nbCandidates); //Compute torque normalization T Lnorm = 1.0; if(normalizationMethod == TorqueNormMethod::L1) { Lnorm = 1.0; } else if(normalizationMethod == TorqueNormMethod::Lavg) { Lnorm = 0.0; for (std::size_t i = 0; i < nbCandidates; ++i) Lnorm += (cloud.features.col(candidates[i]).head(3) - center).norm(); Lnorm /= nbCandidates; } else if(normalizationMethod == TorqueNormMethod::Lmax) { const Vector minValues = cloud.features.rowwise().minCoeff(); const Vector maxValues = cloud.features.rowwise().maxCoeff(); const Vector3 radii = maxValues.head(3) - minValues.head(3); Lnorm = radii.maxCoeff() / 2.; //radii.mean() / 2.; } //A.3 - Compute 6x6 covariance matrix + EigenVectors auto computeCovariance = [Lnorm, nbCandidates, &cloud, ¢er, &normals, &candidates](Matrix66 & cov) -> void { //Compute F matrix, see Eq. (4) Eigen::Matrix<T, 6, Eigen::Dynamic> F(6, nbCandidates); for(std::size_t i = 0; i < nbCandidates; ++i) { const Vector3 p = cloud.features.col(candidates[i]).head(3) - center; // pi-c const Vector3 ni = normals.col(candidates[i]).head(3); //compute (1 / L) * (pi - c) x ni F.template block<3, 1>(0, i) = (1. / Lnorm) * p.cross(ni); //set ni part F.template block<3, 1>(3, i) = ni; } // Compute the covariance matrix Cov = FF' cov = F * F.transpose(); }; Matrix66 covariance; computeCovariance(covariance); Eigen::EigenSolver<Matrix66> solver(covariance); const Matrix66 eigenVe = solver.eigenvectors().real(); const Vector6 eigenVa = solver.eigenvalues().real(); ///---- Part B //B.1 - Compute the v-6 for each candidate std::vector<Vector6, Eigen::aligned_allocator<Vector6>> v; // v[i] = [(pi-c) x ni ; ni ]' v.resize(nbCandidates); for(std::size_t i = 0; i < nbCandidates; ++i) { const Vector3 p = cloud.features.col(candidates[i]).head(3) - center; // pi-c const Vector3 ni = normals.col(candidates[i]).head(3); v[i].template block<3, 1>(0, 0) = (1. / Lnorm) * p.cross(ni); v[i].template block<3, 1>(3, 0) = ni; } //B.2 - Compute the 6 sorted list based on dot product (vi . Xk) = magnitude, with Xk the kth-EigenVector std::vector<std::list<std::pair<int, T>>> L; // contain list of pair (index, magnitude) contribution to the eigens vectors L.resize(6); //sort by decreasing magnitude auto comp = [](const std::pair<int, T>& p1, const std::pair<int, T>& p2) -> bool { return p1.second > p2.second; }; for(std::size_t k = 0; k < 6; ++k) { for(std::size_t i = 0; i < nbCandidates; ++i ) { L[k].push_back(std::make_pair(i, std::fabs( v[i].dot(eigenVe.template block<6,1>(0, k)) ))); } L[k].sort(comp); } std::vector<T> t(6, T(0.)); //contains the sums of squared magnitudes std::vector<bool> sampledPoints(nbCandidates, false); //maintain flag to avoid resampling the same point in an other list ///Add point iteratively till we got the desired number of point for(std::size_t i = 0; i < nbSample; ++i) { //B.3 - Equally constrained all eigen vectors // magnitude contribute to t_i where i is the indice of th least contrained eigen vector //Find least constrained vector std::size_t k = 0; for (std::size_t i = 0; i < 6; ++i) { if (t[k] > t[i]) k = i; } // Add the point from the top of the list corresponding to the dimension to the set of samples while(sampledPoints[L[k].front().first]) L[k].pop_front(); //remove already sampled point //Get index to keep const std::size_t idToKeep = static_cast<std::size_t>(L[k].front().first); L[k].pop_front(); sampledPoints[idToKeep] = true; //set flag to avoid resampling //B.4 - Update the running total for (std::size_t k = 0; k < 6; ++k) { const T magnitude = v[idToKeep].dot(eigenVe.template block<6, 1>(0, k)); t[k] += (magnitude * magnitude); } keepIndexes[i] = candidates[idToKeep]; } //TODO: evaluate performances between this solution and sorting the indexes // We build map of (old index to new index), in case we sample pts at the begining of the pointcloud std::unordered_map<std::size_t, std::size_t> mapidx; std::size_t idx = 0; ///(4) Sample the point cloud for(std::size_t id : keepIndexes) { //retrieve index from lookup table if sampling in already switched element if(id<idx) id = mapidx[id]; //Switch columns id and idx cloud.swapCols(idx, id); //Maintain new index position mapidx[idx] = id; //Update index ++idx; } cloud.conservativeResize(nbSample); }