OpenCV–拟合平面
项目场景：
在三维空间中，如何通过一组离散的3维坐标来拟合平面空间，使得任意一组空间坐标**（Px, Py, Pz）**在平面方程上。
问题描述
给定n个三维点的坐标，根据这些点坐标由最小二乘法拟合平面。
原因分析：
平面方程一般为Ax+By+Cz+D = 0，为了方便计算这里将平面方程变形为z=Ax+By+C;
像素坐标通过相机内参及外参数矩阵可以得到n个三维点坐标，理想情况下应满足
{
z
0
=
A
x
0
+
B
y
0
+
C
z
1
=
A
x
1
+
B
y
1
+
C
⋯
left{begin{array}{c} z_0=A x_0+B y_0+C \ z_1=A x_1+B y_1+C \ cdots end{array}right.
⎩
⎨
⎧z0=Ax0+By0+Cz1=Ax1+By1+C⋯
但由于测量误差的存在，三维点并不都严格符合在一个平面上，所以上述方程无解，需要用最小二乘法来求解上述方程组。
解决方案：
构建方程目标函数：
J
(
A
,
B
,
C
)
=
∑
i
=
0
n
(
A
x
i
+
B
y
i
+
C
−
z
i
)
2
J(A, B, C)=sum_{i=0}^nleft(A x_i+B y_i+C-z_iright)^2
J(A,B,C)=i=0∑n(Axi+Byi+C−zi)2
求解A,B,C 使得损失函数J最小，采取最小二乘法，分别对A,B,C求偏导，令其偏导数均为0，如下所示：
{
∂
J
/
∂
A
=
2
∗
∑
i
=
0
n
(
A
x
i
+
B
y
i
+
C
−
z
i
)
∗
x
i
=
0
∂
J
/
∂
B
=
2
∗
∑
i
=
0
n
(
A
x
i
+
B
y
i
+
C
−
z
i
)
∗
y
i
=
0
∂
J
/
∂
C
=
2
∗
∑
i
=
0
n
(
A
x
i
+
B
y
i
+
C
−
z
i
)
=
0
left{begin{array}{c} partial J / partial A=2 * sum_{i=0}^nleft(A x_i+B y_i+C-z_iright) * x_i=0 \ partial J / partial B=2 * sum_{i=0}^nleft(A x_i+B y_i+C-z_iright) * y_i=0 \ partial J / partial C=2 * sum_{i=0}^nleft(A x_i+B y_i+C-z_iright)=0 end{array}right.
⎩
⎨
⎧∂J/∂A=2∗∑i=0n(Axi+Byi+C−zi)∗xi=0∂J/∂B=2∗∑i=0n(Axi+Byi+C−zi)∗yi=0∂J/∂C=2∗∑i=0n(Axi+Byi+C−zi)=0
展开，变形得到：
{
∑
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 2left(A x_i+B y_i+C-z_iright) x_i=0 \ sum 2left(A x_i+B y_i+C-z_iright) y_i=0 \ sum 2left(A x_i+B y_i+C-z_iright)=0 end{array}right.
⎩
⎨
⎧∑2(Axi+Byi+C−zi)xi=0∑2(Axi+Byi+C−zi)yi=0∑2(Axi+Byi+C−zi)=0
其中，A, B, C为变量的线性方程组，写为矩阵形式有：
∣
∑
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 mathrm{x}_{mathrm{i}}^2 & sum mathrm{x}_{mathrm{i}} mathrm{y}_{mathrm{i}} & sum mathrm{x}_{mathrm{i}} \ sum mathrm{x}_{mathrm{i}} mathrm{y}_{mathrm{i}} & sum mathrm{y}_{mathrm{i}}^2 & sum mathrm{y}_{mathrm{i}} \ sum mathrm{x}_{mathrm{i}} & sum mathrm{y}_{mathrm{i}} & mathrm{n} end{array}right| cdotleft|begin{array}{c} mathrm{A} \ mathrm{B} \ mathrm{C} end{array}right|=left|begin{array}{c} sum mathrm{z}_{mathrm{i}} mathrm{x}_{mathrm{i}} \ sum mathrm{z}_{mathrm{i}} mathrm{y}_{mathrm{i}} \ sum mathrm{z}_{mathrm{i}} end{array}right|
∑xi2∑xiyi∑xi∑xiyi∑yi2∑yi∑xi∑yin
⋅
ABC
=
∑zixi∑ziyi∑zi
构建矩阵方程Ax = b, 其中A为上述方程中左边的参数矩阵方程， x为变量（ABC）, b为等式右边的变量。
通过矩阵运算得到
x
=
A
−
1
∗
b
x=text { A }^{-1} * b
x= A −1∗b, 最终得到参数向量[A B C]。
结果展示：
随机给一组点进行测试：（该点为三维坐标系中Oxy平面附近的一组点，坐标的z值在0附近小范围波动，拟合的平面法向量应该近似等于(0,0,1)）。
完整代码如下：
#include <iostream>
#include <opencv2/opencv.hpp>
#include <vector>
void CaculatefitPlane(std::vector<cv::Point3f> points, std::vector<double> &res)
{
// 最小二乘法拟合平面 x = A^-1 * B
// step1 create matrix of A, B, X
cv::Mat A = cv::Mat::zeros(3, 3, CV_64FC1); // Matrix
cv::Mat B = cv::Mat::zeros(3, 1, CV_64FC1); // vector
cv::Mat X = cv::Mat::zeros(3, 1, CV_64FC1); // vector
// step2 input points
double xi = 0;
double xi2 = 0;
double xiyi = 0;
double yi = 0;
double yi2 = 0;
double zi = 0;
double zixi = 0;
double ziyi = 0;
for (int i = 0; i<points.size(); i++)
{
xi2 += (double)points[i].x * (double)points[i].x;
yi2 += (double)points[i].y * (double)points[i].y;
xiyi += (double)points[i].x * (double)points[i].y;
xi += (double)points[i].x;
yi += (double)points[i].y;
zixi += (double)points[i].z * (double)points[i].x;
ziyi += (double)points[i].z * (double)points[i].y;
zi += (double)points[i].z;
}
A.at<double>(0, 0) = xi2;
A.at<double>(1, 0) = xiyi;
A.at<double>(2, 0) = xi;
A.at<double>(0, 1) = xiyi;
A.at<double>(1, 1) = yi2;
A.at<double>(2, 1) = yi;
A.at<double>(0, 2) = xi;
A.at<double>(1, 2) = yi;
A.at<double>(2, 2) = points.size();
B.at<double>(0, 0) = zixi;
B.at<double>(1, 0) = ziyi;
B.at<double>(2, 0) = zi;
// step3 calculate plane
// Ax+by+cz=D, c = 1
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;
}
int main()
{
std::vector<cv::Point3f> points3d;
std::vector<double> planeFun;
points3d.push_back(cv::Point3f(10.1, 20.5, 0.12));
points3d.push_back(cv::Point3f(15.1, 34.5, 0.1));
points3d.push_back(cv::Point3f(13.1, 7.5, -0.05));
points3d.push_back(cv::Point3f(10.1, 25.5, 0.03));
points3d.push_back(cv::Point3f(14.1, 10.5, 0.1));
points3d.push_back(cv::Point3f(16.1, 40.5, 0.2));
points3d.push_back(cv::Point3f(32.1, 10.5, -0.2));
CaculatefitPlane(points3d, planeFun);
for (int i = 0; i < planeFun.size();i++)
{
std::cout << planeFun[i] << std::endl;
}
}
for (int i = 0; i < planeFun.size();i++)
{
std::cout << planeFun[i] << std::endl;
}
}
结果如下：
