526互联

PCL_PCA点云主方向分析

发布时间 2023-06-15 18:14:12作者: ID是菜鸟

1.使用PCA计算点云主方向，并进行矫正


#include"vtkAutoInit.h"
#ifndef  VTK_MODULE_INIT
VTK_MODULE_INIT(vtkRenderingOpenGL); // VTK was built with vtkRenderingOpenGL
VTK_MODULE_INIT(vtkInteractionStyle);
VTK_MODULE_INIT(vtkRenderingFreeType);
#endif
#include<vtkRenderingFreeTypeModule.h> //屏蔽vtk警告显示窗口
#include <iostream>
#include <string>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <Eigen/Core>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#include <pcl/visualization/pcl_visualizer.h>

using namespace std;
typedef pcl::PointXYZ PointType;

int main(int argc, char **argv)
{
	pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>());

	//std::cout << "请输入需要显示的点云文件名：";
	//std::string fileName("rabbit");
	//getline(cin, fileName);
	//fileName += ".pcd";
	std::string fileName(argv[1]);
	pcl::io::loadPCDFile(fileName, *cloud);

	Eigen::Vector4f pcaCentroid;
	pcl::compute3DCentroid(*cloud, pcaCentroid);//计算点云质心
	Eigen::Matrix3f covariance;
	pcl::computeCovarianceMatrixNormalized(*cloud, pcaCentroid, covariance);//计算目标点云协方差矩阵
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);//构造一个特定的自伴随矩阵类便于后续分解
	Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();//计算特征向量
	Eigen::Vector3f eigenValuesPCA = eigen_solver.eigenvalues();//计算特征值
	eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); //校正主方向间垂直
	eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));
	eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0));

	std::cout << "特征值va(3x1):\n" << eigenValuesPCA << std::endl;
	std::cout << "特征向量ve(3x3):\n" << eigenVectorsPCA << std::endl;
	std::cout << "质心点(4x1):\n" << pcaCentroid << std::endl;
	/*
	// 另一种计算点云协方差矩阵特征值和特征向量的方式:通过pcl中的pca接口，如下，这种情况得到的特征向量相似特征向量
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPCAprojection (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PCA<pcl::PointXYZ> pca;
	pca.setInputCloud(cloudSegmented);
	pca.project(*cloudSegmented, *cloudPCAprojection);
	std::cerr << std::endl << "EigenVectors: " << pca.getEigenVectors() << std::endl;//计算特征向量
	std::cerr << std::endl << "EigenValues: " << pca.getEigenValues() << std::endl;//计算特征值
	*/
	Eigen::Matrix4f tm = Eigen::Matrix4f::Identity();
	Eigen::Matrix4f tm_inv = Eigen::Matrix4f::Identity();
	tm.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();   //R.
	tm.block<3, 1>(0, 3) = -1.0f * (eigenVectorsPCA.transpose()) *(pcaCentroid.head<3>());//  -R*t
	tm_inv = tm.inverse();

	std::cout << "变换矩阵tm(4x4):\n" << tm << std::endl;
	std::cout << "逆变矩阵tm'(4x4):\n" << tm_inv << std::endl;


	pcl::PointCloud<PointType>::Ptr transformedCloud(new pcl::PointCloud<PointType>);
	pcl::transformPointCloud(*cloud, *transformedCloud, tm);

	//x方向与z方向互换
	for (auto it = transformedCloud->begin(); it != transformedCloud->end(); it++)
	{
		float temp_x = it->x;
		float temP_z = it->z;
		it->x = temP_z;
		it->z = temp_x;
	}
	pcl::io::savePCDFileBinary("transformedCloud.pcd", *transformedCloud);
}

2 computeCovarianceMatrixNormalized 计算给点云的协方差矩阵

/** \简介计算给定点集的3x3协方差矩阵归一化。
     *结果作为eigen :: matrix3f返回。
     *归一化意味着每个条目都已除以点云中的点数。
     *对于少量的积分，或者如果要明确的样品变量，请使用ComputeCovarianCematrix
     *并用1 /（n-1）缩放协方差矩阵，其中n是用于计算的点数
     *协方差矩阵，并由ComputeCovarianCematrix函数返回。
     * \ param [in]云输入点云
     * \ param [in] centroid云中点集的质心
     * \ param [out] covariance_matrix结果3x3协方差矩阵
     * \返回数量的有效点用于确定协方差矩阵。
     *如果有密集的点云，则与输入云的大小相同。
     * \ group common
     */

/** \brief Compute normalized the 3x3 covariance matrix of a given set of points.

    * The result is returned as a Eigen::Matrix3f.
    * Normalized means that every entry has been divided by the number of points in the point cloud.
    * For small number of points, or if you want explicitly the sample-variance, use computeCovarianceMatrix
    * and scale the covariance matrix with 1 / (n-1), where n is the number of points used to calculate
    * the covariance matrix and is returned by the computeCovarianceMatrix function.
    * \param[in] cloud the input point cloud
    * \param[in] centroid the centroid of the set of points in the cloud
    * \param[out] covariance_matrix the resultant 3x3 covariance matrix
    * \return number of valid points used to determine the covariance matrix.
    * In case of dense point clouds, this is the same as the size of input cloud.
    * \ingroup common
    */
  template <typename PointT, typename Scalar> inline unsigned int
  computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
                                     const Eigen::Matrix<Scalar, 4, 1> &centroid,
                                     Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);

3 协方差矩阵

https://www.zhihu.com/tardis/zm/art/37609917?source_id=1005
https://zhuanlan.zhihu.com/p/464822791

4 自伴随矩阵

https://zhuanlan.zhihu.com/p/550779050
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);//

构造一个自伴随举证类便于后续分解

5.总体算法思路分析

1首先将点云归一化后计算其3*3协方差矩阵
2.创建一个伴随矩阵的，并基于次对于分解得到特征向量以及值
3.由于特征矩阵只有x\y\z三个方向需要将其矫正垂直
4.后续这一步已经完全看不懂了

	Eigen::Matrix4f tm = Eigen::Matrix4f::Identity();
	Eigen::Matrix4f tm_inv = Eigen::Matrix4f::Identity();
	tm.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();   //R 计算PCA矩阵的转置矩阵
	tm.block<3, 1>(0, 3) = -1.0f * (eigenVectorsPCA.transpose()) *(pcaCentroid.head<3>());//  -R*t
	tm_inv = tm.inverse();