pcl 计算协方差矩阵的特征值和特征向量精度不高的问题

说明

---

• pcl 库依赖 Eigen 第三方库，但是实践表明，Eigen 库的 Eigen::SelfAdjointEigenSolver 求的特征值和特征向量精度并不是很高，所以要换一种方法来求
• 主要用到了 pcl::eigen33

// pcl
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/common.h>
#include <pcl/common/impl/centroid.hpp>

// pcl 计算邻域协方差矩阵的函数
void computeEigenValuesAndEigenVectors(const pcl::PointCloud<pcl::PointXYZ>::Ptr& input_cloud,
										 pcl::PointXYZ search_point,
										 float search_radius){
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
	kdtree.setInputCloud(input_cloud);
	std::vector<int> neighbor_index;
	std::vector<float> neighbor_square_distance;
	if(kdtree.radiusSearch(search_point, search_radius, neighbor_index, neighbor_square_distance) > 0){
		Eigen::Matrix<double, 4, 1> centroid;
		pcl::compute3DCentroid(*input_cloud, neighbor_index, centroid);  // 计算质心
		Eigen::Matrix<double, 3, 3> convariance_matrix;  // 协方差矩阵
		pcl::computeCovarianceMatrix(*input_cloud, neighbor_index, centroid, convariance_matrix);
		Eigen::Matrix3d eigenVectors;
		Eigen::Vector3d eigenValues;
		pcl::eigen33(convariance_matrix, eigenVectors, eigenValues);

		// 自带的求解特征值精度不高
		//Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 3, 3>> eigen_solver(convariance_matrix.transpose()*convariance_matrix);
		//eigenValues = eigen_solver.eigenvalues();

		// 求前两个较大的特征值
		Eigen::Vector3d::Index maxRow, maxCol, minRow, minCol;
		eigenValues.maxCoeff(&maxRow, &maxCol);
		eigenValues.minCoeff(&minRow, &minCol);
		// l1 > l2 > l3
		const double& l1 = eigenValues[maxRow];
		const double& l2 = eigenValues[3 - maxRow - minRow]; // 这个是巧算，基于 0 + 1 + 2 = 3
		const double& l3 = eigenValues[minRow];
	}
}