matlab 计算点云中心_基于平面特征的点云配准C++实现

算法参照王力的论文《激光扫描中平面拟合及坐标转换模型构建》

说明：

这里给出C++的实现，其实我是先做了MATLAB的实现，又转的C++，要是有需要也可以给出MATLAB的代码。代码中的测试数据是王力论文中的数据，最终输出为6参数，即旋转和平移。代码实现了论文中的试验结果的精度，说明本代码的正确。

目的意义：

目前，车载、机载、单站LiDAR扫描仪受到搭载平台姿态、控制点精度、GPS精度、惯导精度等影响，所采集到的多源数据坐标系之间存在微小的偏差。为了充分利用多源数据中的信息，需要匹配多源点云数据的坐标系并根据同名特征（点、线、面）完成点云的拼接。点云拼接的精度影响后续数据处理的精度，因此，点云拼接是数据处理的关键一步。

在体测量模式中，对同一表面的两次测量可能不存在公共点，因此需要研究基于模型或者基于特征的坐标转换方法。平面是一种广泛存在于自然界的规则模型。若能从点云数据中拟合出多个平面，并利用公共平面进行坐标转换，则能避免从海量的点云数据中寻找公共点。基于这一思考，基于平面的坐标转换模型的研究很有必要。

C++代码实现及模拟：

#include <iostream>

#include <Eigen/Dense>

using namespace std;

using namespace Eigen;

int main()

{

//2019年12月2日14:40:12

//---------------------------测试数据-----------------------------

//两个坐标系下的，各4个平面的法向量abc

MatrixXd A1(4, 4);

A1 << -0.371391, -0.557086, 0.742781, 0.928477,

0.331042, -0.579324, 0.744845, 0.248282,

0.348743, 0.464991, 0.813733, -0.581238,

0.566139, 0.226455, 0.792594, -1.019049;

MatrixXd A2(4, 4);

A2 << 0.538125, 0.398469, 0.742727, -2.720210,

-0.115270, 0.657273, 0.744785, -1.843040,

-0.485580, -0.319340, 0.813778, -3.184530,

-0.609350, -0.021280, 0.792613, -2.992090;

//-----------------------前期准备-----------------------------------

//获取矩阵的行列数

const int row = A1.rows(); //行数代表了平面法向量的组数，即1行代表1有1个平面

const int cols = A1.cols()-1; //列为法向量abc，column为常亮不变

MatrixXd Normal_vector_matrix1(row, cols);

MatrixXd Normal_vector_matrix2(row, cols);

MatrixXd Distance1(row, 1);

MatrixXd Distance2(row, 1);

for (int j = 0; j < cols; j++)

{

for (int i = 0; i < row; i++)

{

Normal_vector_matrix1(i,j) = A1(i, j); //获取坐标系一下，各个面的法向量abc

Normal_vector_matrix2(i, j) = A2(i, j); //获取坐标系二下，各个面的法向量abc

}

}

for (int i = 0; i < row; i++)

{

Distance1(i, 0) = A1(i, cols); //获取坐标系一下距离d

Distance2(i, 0) = A2(i, cols); //获取坐标系二下距离d

}

//-----------------------------计算旋转矩阵-------------------------

//计算平均值

//定义参数

int i, j;

double sum1;

MatrixXd sum_M1(1, cols);

MatrixXd avg1(1, cols);

double sum2;

MatrixXd sum_M2(1, cols);

MatrixXd avg2(1, cols);

for (j = 0; j < cols; j++)

{

sum1 = 0;

for (i = 0; i < row; i++)

{

sum1 += Normal_vector_matrix1(i, j);

}

sum_M1(j) = sum1;

avg1 = sum_M1 / row; //坐标系一下，各个法向量的均值

}

for (j = 0; j < cols; j++)

{

sum2 = 0;

for (i = 0; i < row; i++)

{

sum2 += Normal_vector_matrix2(i, j);

}

sum_M2(j) = sum2;

avg2 = sum_M2 / row; //坐标系而下，各个法向量的均值

}

//矩阵每个值减去均值

MatrixXd P1(row, cols);

MatrixXd X1(row, cols);

for (i = 0; i < row; i++)

{

for (j = 0; j < cols; j++)

{

P1(i, j) = Normal_vector_matrix1(i, j) - avg1(j);

X1(i, j) = Normal_vector_matrix2(i, j) - avg2(j);

}

}

//计算点集协方差

double sigma_trace = 0;

MatrixXd sigma(cols, cols);

MatrixXd sigma_mi(cols, cols);

MatrixXd sigma_ni(cols, cols);

MatrixXd P1_tra(cols, row);

MatrixXd M(cols, cols);

MatrixXd Ones(3, 3);

MatrixXd Q(4, 4);

//生成一个单位矩阵

for (i = 0; i < cols; i++)

{

for (j = 0; j < cols; j++)

Ones(i, j) = 0;

}

Ones(0, 0) = 1; Ones(1, 1) = 1; Ones(2, 2) = 1; //*******有问题

P1_tra = P1.transpose(); //P1的转置

sigma = P1_tra * X1 / row;

sigma_mi = sigma - sigma.transpose();

sigma_ni = sigma + sigma.transpose();

sigma_trace = sigma.trace();

M = sigma_ni - sigma_trace * Ones;

//由协方差构造4 * 4对称矩阵

Q << sigma_trace, sigma_mi(1, 2), sigma_mi(2, 0), sigma_mi(0, 1),

sigma_mi(1, 2), M(0, 0), M(0, 1), M(0, 2),

sigma_mi(2, 0), M(1, 0), M(1, 1), M(1, 2),

sigma_mi(0, 1), M(2, 0), M(2, 1), M(2, 2);

//计算协方差Q对应的特征值和特征向量

EigenSolver<Matrix4d> es(Q);

Matrix4d D = es.pseudoEigenvalueMatrix(); //特征值

Matrix4d V = es.pseudoEigenvectors(); //特征向量

//计算最大特征值对应的特征向量

MatrixXd e(4, 1); //取特征值

e << D(0, 0), D(1, 1), D(2, 2), D(3, 03);

double e_max = 0;

int ii = 0;

for (int k = 0; k < 4; k++)

{

if (e_max < e(k))

{

e_max = e(k); //获得最大特征值

ii = k; //最大特征值对应的位置

}

}

//最大特征值对应的特征向量

MatrixXd Feature_vector_max(4, 1);

Feature_vector_max << V(0, ii), V(1, ii), V(2, ii), V(3, ii);

//四元数

double q0, q1, q2, q3;

q0 = Feature_vector_max(0);

q1 = Feature_vector_max(1);

q2 = Feature_vector_max(2);

q3 = Feature_vector_max(3);

//由四元数构造旋转矩阵

MatrixXd RR(3, 3);

//旋转参数

RR << pow(q0, 2) + pow(q1, 2) - pow(q2, 2) - pow(q3, 2), 2 * (q1 * q2 - q0 * q3), 2 * (q1 * q3 + q0 * q2),

2 * (q1 * q2 + q0 * q3), pow(q0, 2) - pow(q1, 2) + pow(q2, 2) - pow(q3, 2), 2 * (q2 * q3 - q0 * q1),

2 * (q1 * q3 - q0 * q2), 2 * (q2 * q3 + q0 * q1), pow(q0, 2) - pow(q1, 2) - pow(q2, 2) + pow(q3, 2);

//-----------------------------计算平移矩阵-------------------------------

//最小二乘LS

MatrixXd L(row, 1);

MatrixXd B(cols, cols);

MatrixXd B_inv(cols, cols);

MatrixXd TT(cols, 1);

L = Distance2 - Distance1;

B = Normal_vector_matrix1.transpose() * Normal_vector_matrix1;

B_inv = B.inverse();

TT = B_inv * Normal_vector_matrix1.transpose() * L; //平移参数

//-----------------------------输出6参数----------------------------------

cout << "旋转参数：" << 'n' << RR << endl;

cout << "平移参数：" << 'n' << TT << endl;

return 0;

}