學(xué)習(xí)目標(biāo)：
本文主要介如何用SVD分解法和最小二乘法擬合平面點(diǎn)云，包含原理推導(dǎo)和代碼
1.SVD分解法求解平面點(diǎn)云
1.1問題描述
- 將空間中的離散點(diǎn)擬合為一個(gè)平面，就是使離散點(diǎn)到某個(gè)平面距離和最小的問題，可以將求解過程看作最優(yōu)化的過程。
- 一個(gè)先驗(yàn)知識(shí)為擬合平面一定經(jīng)過離散點(diǎn)的質(zhì)心（離散點(diǎn)坐標(biāo)的平均值）。平面方程可以通過求解求解平面的法向量來獲得。根據(jù)協(xié)方差矩陣的SVD變換，最小奇異值對(duì)應(yīng)的奇異向量就是平面的方向。
注意：這個(gè)方法是直接的計(jì)算方法，沒辦法解決數(shù)值計(jì)算遇到的病態(tài)矩陣問題.在公式轉(zhuǎn)化代碼之前必須對(duì)空間點(diǎn)坐標(biāo)進(jìn)行近似歸一化！
1.2問題建模：
已知若干三維點(diǎn)坐標(biāo)
(
x
i
,
y
i
,
z
i
)
(x_{i},y_{i},z_{i})
(xi?,yi?,zi?),擬合出平面方程
a
x
+
b
y
+
c
z
=
d
ax+by+cz=d
ax+by+cz=d (1)
約束條件為
a
2
+
b
2
+
c
2
=
1
a^{2}+b^{2}+c^{2}=1
a2+b2+c2=1 (2)
目標(biāo)為使該平面到所有點(diǎn)的距離之和最小
1.3推導(dǎo)：
所有點(diǎn)的平均坐標(biāo)為 ( x ˉ , y ˉ , z ˉ ) (\bar{x},\bar{y},\bar{z}) (xˉ,yˉ?,zˉ),則由先驗(yàn)知識(shí)（擬合平面一定經(jīng)過離散點(diǎn)的質(zhì)心），可以到以下方程 a x ˉ + b y ˉ + c z ˉ = d . a\bar{x}+b\bar{y}+c\bar{z}=d. axˉ+byˉ?+czˉ=d.(3)
式（1）與式（3）相減，得 a ( x i ? x ˉ ) + b ( y i ? y ˉ ) + c ( z i ? z ˉ ) = 0 a(x_{i}-\bar{x})+b(y_{i}-\bar{y})+c(z_{i}-\bar{z})=0 a(xi??xˉ)+b(yi??yˉ?)+c(zi??zˉ)=0(4)
將所有點(diǎn)數(shù)據(jù)帶入式4，整理可以得到矩陣 A = [ x 1 ? x ˉ , y 1 ? y ˉ , z 1 ? z ˉ x 2 ? x ˉ , y 2 ? y ˉ , z 2 ? z ˉ x 3 ? x ˉ , y 3 ? y ˉ , z 3 ? z ˉ . . . x n ? x ˉ , y n ? y ˉ , z n ? z ˉ ] A=\begin{bmatrix} x_{1}-\bar{x},y_{1}-\bar{y},z_{1}-\bar{z}\\ x_{2}-\bar{x},y_{2}-\bar{y},z_{2}-\bar{z}\\ x_{3}-\bar{x},y_{3}-\bar{y},z_{3}-\bar{z}\\ ...\\ x_{n}-\bar{x},y_{n}-\bar{y},z_{n}-\bar{z} \end{bmatrix} A= ?x1??xˉ,y1??yˉ?,z1??zˉx2??xˉ,y2??yˉ?,z2??zˉx3??xˉ,y3??yˉ?,z3??zˉ...xn??xˉ,yn??yˉ?,zn??zˉ? ?,列矩陣 X = [ a b c ] X = \begin{bmatrix} a\\ b\\ c \end{bmatrix} X= ?abc? ? ,則式（4）等價(jià)與AX=0 (5)
理想情況下所有點(diǎn)都在平面上，式（5）成立；實(shí)際情況下有部分點(diǎn)在平面外，擬合的目的為平面距離所有點(diǎn)的距離之和盡量小，所以目標(biāo)函數(shù)為 m i n ∥ A X ∥ min\left \| AX \right \| min∥AX∥ (6)
約束條件為 ∥ X ∥ = 1 \left \| X \right \|=1 ∥X∥=1 (7)
若A可做奇異值分解： A = U D V T A = UDV^{T} A=UDVT (8)
其中，D是對(duì)角矩陣，U和V均為酉矩陣。
則 ∥ A X ∥ = ∥ U D V T X ∥ = ∥ D V T X ∥ \left \| AX \right \|=\left \| UDV^{T}X \right \|=\left \| DV^{T}X \right \| ∥AX∥= ?UDVTX ?= ?DVTX ? (9)
其中為列矩陣，并且 ∥ V T X ∥ = ∥ X ∥ = 1 \left \| V^{T}X \right \|=\left \|X \right \|=1 ?VTX ?=∥X∥=1 (10)
因?yàn)镈的對(duì)角元素為奇異值，假設(shè)最后一個(gè)對(duì)角元素為最小奇異值，則當(dāng)且僅當(dāng)
V
T
X
=
[
0
0
0
.
.
.
1
]
V^{T}X=\begin{bmatrix} 0\\ 0\\ 0\\ ...\\ 1 \end{bmatrix}
VTX=
?000...1?
?（11）時(shí)，式（9）可以取得最小值，即式(6)成立。
此時(shí) X = V [ 0 0 0 . . . 1 ] = [ v 1 v 2 v 3 . . . v n ] [ 0 0 0 . . . 1 ] = v n X=V\begin{bmatrix} 0\\ 0\\ 0\\ ...\\ 1 \end{bmatrix}=\begin{bmatrix} v_{1} &v_{2} &v_{3} &... &v_{n} \end{bmatrix}\begin{bmatrix} 0\\ 0\\ 0\\ ...\\ 1 \end{bmatrix}=v_{n} X=V ?000...1? ?=[v1??v2??v3??...?vn??] ?000...1? ?=vn? (12)
所以，目標(biāo)函數(shù)（6）在約束條件（7）下的最優(yōu)解為 X = ( a , b , c ) = ( v n , 1 , v n , 2 , v n , 3 ) X=(a,b,c)=(v_{n,1},v_{n,2},v_{n,3}) X=(a,b,c)=(vn,1?,vn,2?,vn,3?) (13)
綜上：對(duì)矩陣A做奇異值分解，最小奇異值對(duì)應(yīng)的特征向量就是擬合平面的系數(shù)向量。
1.4 偽代碼
1 讀取點(diǎn)云數(shù)據(jù)
2 求解點(diǎn)云質(zhì)心(x0,y0,z0)
3 將原始點(diǎn)云坐標(biāo)減去點(diǎn)云質(zhì)心，構(gòu)建矩陣A
4 對(duì)矩陣A進(jìn)行SVD分解，A = U * S * VT
5 V最后一列對(duì)應(yīng)為(A,B,C); D=-(Ax0+By0+C*z0)
1.5 使用opencv進(jìn)行求解
void fitPlane(cv::Mat &points,cv::Mat &plane)
//points輸入點(diǎn)云
//plane輸出平面方程系數(shù)
{
cv::Mat centor = cv::Mat::zeros(1,points.cols,CV_32FC1);
for(int i =0;i < points.cols;++i)
{
for(int j =0;j < points.rows;++j)
{
centor.at<float>(0,i) = centor.at<float>(0,i) + points.at<float>(j,i);
}
centor.at<float>(0.i) = centor.at<float>(0.i) / points.rows;
}
cv::Mat pointC = cv::Mat::ones(points.rows,points.cols,CV_32FC1);
for(int i =0;i < points.cols;++i)
{
for(int j =0;j < points.rows;++j)
{
pointC.at<float>(j,i) = points.at<float>(j,i) - centor.at<float>(0.i);
}
}
//SVD分解
cv::Mat A,W,U,V;
//構(gòu)建奇異值矩陣(gemm矩陣相乘)
SVD::compute(pointC,W,U,V);
// 提取最小奇異值和相應(yīng)的向量
double min_sing_val = W.at<double>(svd.w.rows-1); // 最后一個(gè)元素為最小奇異值
Mat min_sing_vec = V.row(V.rows-1); // 最后一行為最小奇異值對(duì)應(yīng)的右奇異向量
plane[0]=min_sing_vec[0]
plane[1]=min_sing_vec[1]
plane[2]=min_sing_vec[2]
plane[4]= -(plane[0]*centor.at<float>(0,0)+plane[1]*centor.at<float>(0,1)+plane[2]*centor.at<float>(0,2)
}
1.6 使用Eigen進(jìn)行求解
void FitPlaneSVD::compute()
{
// 1、計(jì)算質(zhì)心
Eigen::RowVector3d centroid = m_cloud.colwise().mean();
// 2、去質(zhì)心
Eigen::MatrixXd demean = m_cloud;
demean.rowwise() -= centroid;
// 3、SVD分解求解協(xié)方差矩陣的特征值特征向量
Eigen::JacobiSVD<Eigen::MatrixXd> svd(demean, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::Matrix3d V = svd.matrixV();
Eigen::MatrixXd U = svd.matrixU();
Eigen::Matrix3d S = U.inverse() * demean * V.transpose().inverse();
// 5、平面的法向量a,b,c
Eigen::RowVector3d normal;
normal << V(0,2), V(1,2), V(2,2);
// 6、原點(diǎn)到平面的距離d
double d = -normal * centroid.transpose();
// 7、獲取擬合平面的參數(shù)a,b,c,d和質(zhì)心x,y,z。
m_planeparameters << normal, d, centroid
}
2.最小二乘法求解平面點(diǎn)云
2.1問題描述
找到一個(gè)平面
Z=Ax+By+C
根據(jù)最小二乘法，使各個(gè)點(diǎn)到這個(gè)平面的距離最近：
S=∑(Axi + Byi + C - Zi) 2
2.2問題描述
文章來源：http://www.zghlxwxcb.cn/news/detail-437147.html
2.3問題使用opencv進(jìn)行求解
void CaculateLaserPlane(std::vector<cv::Point3f> Points3ds,vector<double> &res)
{
//最小二乘法擬合平面
//獲取cv::Mat的坐標(biāo)系以縱向?yàn)閤軸，橫向?yàn)閥軸，而cvPoint等則相反
//系數(shù)矩陣
cv::Mat A = cv::Mat::zeros(3, 3, CV_64FC1);
//
cv::Mat B = cv::Mat::zeros(3, 1, CV_64FC1);
//結(jié)果矩陣
cv::Mat X = cv::Mat::zeros(3, 1, CV_64FC1);
double x2 = 0, xiyi = 0, xi = 0, yi = 0, zixi = 0, ziyi = 0, zi = 0, y2 = 0;
for (int i = 0; i < Points3ds.size(); i++)
{
x2 += (double)Points3ds[i].x * (double)Points3ds[i].x;
y2 += (double)Points3ds[i].y * (double)Points3ds[i].y;
xiyi += (double)Points3ds[i].x * (double)Points3ds[i].y;
xi += (double)Points3ds[i].x;
yi += (double)Points3ds[i].y;
zixi += (double)Points3ds[i].z * (double)Points3ds[i].x;
ziyi += (double)Points3ds[i].z * (double)Points3ds[i].y;
zi += (double)Points3ds[i].z;
}
A.at<double>(0, 0) = x2;
A.at<double>(1, 0) = xiyi;
A.at<double>(2, 0) = xi;
A.at<double>(0, 1) = xiyi;
A.at<double>(1, 1) = y2;
A.at<double>(2, 1) = yi;
A.at<double>(0, 2) = xi;
A.at<double>(1, 2) = yi;
A.at<double>(2, 2) = Points3ds.size();
B.at<double>(0, 0) = zixi;
B.at<double>(1, 0) = ziyi;
B.at<double>(2, 0) = zi;
//計(jì)算平面系數(shù)
X = A.inv() * B;
//A
res.push_back(X.at<double>(0, 0));
//B
res.push_back(X.at<double>(1, 0));
//Z的系數(shù)為1
res.push_back(1.0);
//C
res.push_back(X.at<double>(2, 0));
return;
}
參考鏈接
OpenCV最小二乘法擬合空間平面
最小二乘法擬合平面
點(diǎn)云擬合—平面擬合
SVD解決平面擬合問題文章來源地址http://www.zghlxwxcb.cn/news/detail-437147.html
到了這里，關(guān)于3D點(diǎn)云處理：用SVD分解法和最小二乘法擬合平面點(diǎn)云，求解平面方程的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
