输入一个三维点的数组 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 ∑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
整理得
{ ∑ 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 (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
写为矩阵方程
∣ ∑ 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|∣∣∣∣∣∣∣∣∣∣∑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 ∣∣∣∣∣∣∣∣∣∣
矩阵方程 A ⋅ \cdot ⋅x=b
A是参数矩阵，不是上面多项式的参数，b是向量
通解 x=A-1 ⋅ \cdot ⋅b
得到参数向量
∣ A B C ∣ \left|\begin{array}{l} A \ B \C \end{array}\right|∣∣∣∣∣∣A B C ∣∣∣∣∣∣
代码如下：
void CaculateLaserPlane(std::vector<cv::Point3f> Points3ds,vector<double> &res)
{
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;
res.push_back(X.at<double>(0, 0));
res.push_back(X.at<double>(1, 0));
res.push_back(1.0);
res.push_back(X.at<double>(2, 0));
return;
}
Original: https://blog.csdn.net/u011725598/article/details/123739539
Author: Jieckiee
Title: OpenCV最小二乘法拟合空间平面
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/701929/
转载文章受原作者版权保护。转载请注明原作者出处！