项目需求:利用点云数据自动计算灯头、标志牌、路面标线的臂展长度。
解决方案:将点云投影至二维平面,利用PCA主成分分析算法计算点云在主方向上的方向向量,进一步可计算出臂展长度。
(一)具体计算步骤:
(1)假设目前有m个点云,每个点云就是一条数据记录,记录了自身的X和Y坐标,则可以构造出维度是2×m大小的矩阵M:
(2)分别计算m个点云的X坐标与Y坐标的均值,。然后把矩阵M的所有的X坐标减去,Y坐标减去,得到均值化后的矩阵M:
(3)利用矩阵M计算协方差矩阵C:
根据矩阵乘法原理,M是2×m维的,MT是m×2维的,所以协方差矩阵C必然是2×2维的。
(4)计算协方差矩阵C的所有特征值与特征向量。已知C是二维方阵,则可以得到2个特征值和,以及其对应的特征向量c1,c2。
(5)比较特征值的大小,找到最大特征值对应的特征向量,该向量就是这组点云的主方向!
(6)灯头点云臂展计算
假设最大特征值对应的特征向量为c =(a,b),则点云主方向就是(a,b)。将所有点云投影到主方向所在的直线上,则投影后相距最远的两个点之间的距离就是灯头臂展!!!
1)把向量c进行单位化,变成单位向量e:
2)对矩阵M进行基变换,即把矩阵M左乘单位向量e,这样就把点变换到以e为基底的线性空间中去。这一步其实就是完成了点云的投影,即把所有点云投影到主方向所在的直线上。最终得到的矩阵N表示点云在直线上的投影坐标(具有正负性),N的维度必然是1×m大小的。
如下图所示:两个点云P1和P2经过基变换被投影到主方向e所在的直线上面,投影点分别是P11和P22,两者距离原点O的距离是d1和d2。则:
这相当于重新创建了一个一维坐标系,坐标轴就是主方向所在直线,原点不变。P1在该一维坐标系下对应坐标为d1,P2在该一维坐标系下对应坐标为-d2(注意是负值),最终实现了降维。所以,灯头点云臂展长度就非常好算了,只要比较点云在一维坐标系下坐标的大小就行了!最大坐标减去最小坐标,就是灯头臂展长度!!!
(二)PCA主成分分析算法C++代码:
//eigen3矩阵库
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
double PAC(const QList<Point3d>& cloud)
{
int cloudSize = cloud.size();
if (!cloudSize)
{
return -1.0;
}
//step1:构造点云坐标矩阵M
Eigen::MatrixXd M(2, cloudSize);
for (int i = 0; i < cloudSize; ++i)
{
M(0, i) = cloud.at(i).x;
M(1, i) = cloud.at(i).y;
}
//step2:矩阵M的均值零化(使得两行元素的均值为0)
double sumX = M.row(0).sum();
double sumY = M.row(1).sum();
double averageX = sumX / cloudSize;
double averageY = sumY / cloudSize;
for (int i = 0; i < cloudSize; ++i)
{
M(0, i) = cloud.at(i).x - averageX;
M(1, i) = cloud.at(i).y - averageY;
}
//step3:计算协方差矩阵C
Eigen::Matrix2d C = (1.0 / cloudSize)*M*M.transpose();
//step4:计算协方差矩阵C的特征值与特征向量
Eigen::EigenSolver<Eigen::Matrix2d> cc(C);
Eigen::Matrix2d eigenValue = cc.pseudoEigenvalueMatrix();
Eigen::Matrix2d eigenVector = cc.pseudoEigenvectors();
double val_1 = eigenValue(0, 0); //特征值1
Eigen::Vector2d vec_1 = eigenVector.col(0);//特征值1对应的特征向量
double val_2 = eigenValue(1, 1); //特征值2
Eigen::Vector2d vec_2 = eigenVector.col(1);//特征值2对应的特征向量
//step5:找到最大特征值对应的特征向量即为点云的主方向,同时把该特征向量转换成单位向量(注:eigen3库在计算特征值向量的时候,已经帮你完成了单位化!故此处的特征向量本身就是单位向量)
Eigen::Vector2d vec;//主方向向量(单位向量)
if (val_1 >= val_2)
{
vec = vec_1;
}
else
{
vec = vec_2;
}
//step6:臂展长度计算
//计算点云在主方向所在的直线上的一维投影坐标
QList<double> project;
for (int i = 0; i < cloudSize; ++i)
{
project.append(vec(0)*M(0, i) + vec(1)*M(1, i));
}
//投影坐标最值之差即为臂展长度
double minVal = INT_MAX, maxVal = INT_MIN;
for (int i = 0; i < cloudSize; ++i)
{
const double& e = project.at(i);
if (e < minVal)
{
minVal = e;
}
if (e > maxVal)
{
maxVal = e;
}
}
return (maxVal - minVal);
}
(三)算法验证:
数据1
PCA计算臂展长度:6.149
软件实测臂展长度:6.110
数据2
PCA计算臂展长度:1.997
软件实测臂展长度:1.994
不难发现,上述方法在三维空间也同样适用,其实就是增加了点云Z坐标,多了一个维度罢了,计算方法如出一辙。因此,PCA算法还能用于计算点云的方向包围盒(OBB包围盒):最大特征值对应的特征向量为X轴,最小特征值对应的特征向量为Y轴,点云重心为原点O,形成XOY平面,此时Z轴必然垂直于XOY平面,则OBB包围盒的三轴方向就能确定了!!!