Eigen库实例—三维点云拟合最佳平面C++实现

一、Eigen库相关基础

1、矩阵&向量的定义与赋值

 MatrixXd：定义任意维double矩阵。其中X表示维度，d表示double型。同理d可改为f或者i，分别表示float型、int型。
 Matrix2d：定义2维double矩阵（2×2）
 VectorXd 定义任意维double向量
 Vector2d 定义2维double向量（2×1）

2、矩阵的基本运算（“+”，“-”，“*”， “/” 与一般的C语言操作相同）

 MatrixXd R(3, 3);
 double row = R.rows();       //row为R矩阵的行数
 double col = R.cols();       //col为R矩阵的列数
 R.transpose();               //转置
 R.inverse();                 //逆
 double d = R.determinant();  //行列式
 //************SVD分解********//
 Matrix3d  H;
 JacobiSVD svd(H, ComputeThinU | ComputeThinV);
 Matrix3d V = svd.matrixV(), U = svd.matrixU();
 Matrix3d D = U.inverse() * H * V.transpose().inverse(); // S = U^-1 * A * VT^ -1
 //***************************//
 int r = lu.rank();           //矩阵的秩

二、实例用Eigen库（空间点云拟合空间平面）

struct PLANEPT
{
	Eigen::RowVector3d n; // 平面法向量
	double D; // D：原点到平面的距离（沿着法向量方向，可能是负值）
};

// 功能描述：计算平面模型参数：= D
// 输入：待拟合点云 MatrixXd pc_input
// 返回：n：平面法向量方向，D：原点到平面的距离（沿着法向量方向，可能是负值）
void plane_det_pc(Eigen::MatrixXd pc_input, PLANEPT &plane_output)
{
	Eigen::RowVector3d meanVec = pc_input.colwise().mean();
	Eigen::MatrixXd zeroMeanMat = pc_input;
	zeroMeanMat.rowwise() -= meanVec;
	//Eigen::MatrixXd covMat = zeroMeanMat.transpose()*zeroMeanMat;//是否采用协方差矩阵，效果一样，但需要思考
	Eigen::MatrixXd covMat = zeroMeanMat;
	Eigen::JacobiSVD svd(covMat, Eigen::ComputeThinU | Eigen::ComputeThinV);
	Eigen::Matrix3d V = svd.matrixV();
	Eigen::MatrixXd U = svd.matrixU();
	Eigen::Matrix3d S = U.inverse() * covMat * V.transpose().inverse();
	plane_output.n << V(6), V(7), V(8);
	Eigen::MatrixXd D_Mat = plane_output.n * meanVec.transpose();
	plane_output.D = D_Mat(0);
}

上述代码中是否采用协方差矩阵需要思考明，可参考一资料，详细说明见下期。

 SVD:  https://blog.csdn.net/oChenWen/article/details/84373582
       https://www.cnblogs.com/endlesscoding/p/10033527.html
 PCA_SVD: https://zhuanlan.zhihu.com/p/58064462