输入一个三维点的数组 std::vectorcv::Point3f Points3ds;
找到一个平面
Z=Ax+By+C
根据最小二乘法,使各个点到这个平面的距离最近:
S=∑(Axi + Byi + C - Zi) 2
求使得S最小的ABC的数值
首先取得最小值时,对各参数偏导数为零。
{
∂
S
∂
A
=
0
∂
S
∂
B
=
0
∂
S
∂
C
=
0
\left\{\begin{array}{l}\frac{\partial S}{\partial A}=0 \\ \\ \frac{\partial S}{\partial B}=0 \\ \\\frac{\partial S}{\partial C}=0\end{array}\right.
⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧∂A∂S=0∂B∂S=0∂C∂S=0
求偏导:
{
A
∑
x
i
2
+
B
∑
y
i
x
i
+
C
∑
x
i
=
∑
z
i
x
i
A
∑
x
i
y
i
+
B
∑
y
i
2
+
C
∑
y
i
=
∑
z
i
y
i
A
∑
x
i
+
B
∑
y
i
+
C
n
=
∑
z
i
\left\{\begin{array}{l}A \sum x_{i}^{2}+B \sum y_{i} x_{i}+C \sum x_{i}=\sum z_{i} x_{i} \\ \\ A \sum x_{i} y_{i}+B \sum y_{i}{ }^{2}+C \sum y_{i}=\sum z_{i} y_{i} \\ \\ A \sum x_{i}+B \sum y_{i}+C n=\sum z_{i}\end{array}\right.
⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧A∑xi2+B∑yixi+C∑xi=∑zixiA∑xiyi+B∑yi2+C∑yi=∑ziyiA∑xi+B∑yi+Cn=∑zi
整理得
{
∑
2
(
A
x
i
+
B
y
i
+
C
−
z
i
)
x
i
=
0
∑
2
(
A
x
i
+
B
y
i
+
C
−
z
i
)
y
i
=
0
∑
2
(
A
x
i
+
B
y
i
+
C
−
z
i
)
=
0
\left\{\begin{array}{l}\sum 2\left(A x_{i}+B y_{i}+C-z_{i}\right) x_{i}=0 \\ \\ \sum 2\left(A x_{i}+B y_{i}+C-z_{i}\right) y_{i}=0 \\\\ \sum 2\left(A x_{i}+B y_{i}+C-z_{i}\right)=0\end{array}\right.
⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧∑2(Axi+Byi+C−zi)xi=0∑2(Axi+Byi+C−zi)yi=0∑2(Axi+Byi+C−zi)=0
写为矩阵方程
∣
∑
x
i
2
∑
x
i
y
i
∑
x
i
∑
x
i
y
i
∑
y
i
2
∑
y
i
∑
x
i
∑
y
i
n
∣
⋅
∣
A
B
C
∣
=
∣
∑
z
i
x
i
∑
z
i
y
i
∑
z
i
∣
\left|\begin{array}{ccc}\sum x_{i}^{2} & \sum x_{i} y_{i} & \sum x_{i} \\\\ \sum x_{i} y_{i} & \sum y_{i}^{2} & \sum y_{i} \\\\ \sum x_{i} & \sum y_{i} & n\end{array}\right| \cdot\left|\begin{array}{l} A \\\\ B \\\\C \end{array}\right|=\left|\begin{array}{c}\sum z_{i} x_{i} \\ \\\sum z_{i} y_{i} \\\\ \sum z_{i}\end{array}\right|
∣∣∣∣∣∣∣∣∣∣∑xi2∑xiyi∑xi∑xiyi∑yi2∑yi∑xi∑yin∣∣∣∣∣∣∣∣∣∣⋅∣∣∣∣∣∣∣∣∣∣ABC∣∣∣∣∣∣∣∣∣∣=∣∣∣∣∣∣∣∣∣∣∑zixi∑ziyi∑zi∣∣∣∣∣∣∣∣∣∣
矩阵方程A
⋅
\cdot
⋅x=b
A是参数矩阵,不是上面多项式的参数,b是向量
通解x=A-1
⋅
\cdot
⋅b
得到参数向量
∣
A
B
C
∣
\left|\begin{array}{l} A \\ B \\C \end{array}\right|
∣∣∣∣∣∣ABC∣∣∣∣∣∣
代码如下:
void CaculateLaserPlane(std::vector<cv::Point3f> Points3ds,vector<double> &res)
{
//最小二乘法拟合平面
//获取cv::Mat的坐标系以纵向为x轴,横向为y轴,而cvPoint等则相反
//系数矩阵
cv::Mat A = cv::Mat::zeros(3, 3, CV_64FC1);
//
cv::Mat B = cv::Mat::zeros(3, 1, CV_64FC1);
//结果矩阵
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;
//计算平面系数
X = A.inv() * B;
//A
res.push_back(X.at<double>(0, 0));
//B
res.push_back(X.at<double>(1, 0));
//Z的系数为1
res.push_back(1.0);
//C
res.push_back(X.at<double>(2, 0));
return;
}