C++ DataPoints::descriptorExists方法代码示例

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 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, &center, &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;
//.........这里部分代码省略.........