OpenCV--拟合平面

项目场景：

在三维空间中，如何通过一组离散的3维坐标来拟合平面空间，使得任意一组空间坐标**（Px, Py, Pz）**在平面方程上。

---

问题描述

给定n个三维点的坐标，根据这些点坐标由最小二乘法拟合平面。

---

原因分析：

平面方程一般为Ax+By+Cz+D = 0，为了方便计算这里将平面方程变形为z=Ax+By+C;

像素坐标通过相机内参及外参数矩阵可以得到n个三维点坐标，理想情况下应满足
$$\{\begin{array}{c}{z}_0=A{x}_0+B{y}_0+C\\ z_1=A{x}_1+B{y}_1+C\\ \cdots \end{array}$$
但由于测量误差的存在，三维点并不都严格符合在一个平面上，所以上述方程无解，需要用最小二乘法来求解上述方程组。

解决方案：

构建方程目标函数：
$$J(A,B,C)=\sum _{i=0}^n{\left(A{x}_i+B{y}_i+C-z_i\right)}^2$$
求解A,B,C 使得损失函数J最小，采取最小二乘法，分别对A,B,C求偏导，令其偏导数均为0，如下所示：
$$\{\begin{array}{c}\partial J/\partial A=2\ast {\sum }_{i=0}^n\left(A{x}_i+B{y}_i+C-z_i\right)\ast x_i=0\\ \partial J/\partial B=2\ast {\sum }_{i=0}^n\left(A{x}_i+B{y}_i+C-z_i\right)\ast y_i=0\\ \partial J/\partial C=2\ast {\sum }_{i=0}^n\left(A{x}_i+B{y}_i+C-z_i\right)=0\end{array}$$
展开，变形得到：
$$\{\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}$$
其中，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| \cdot\left|\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​∑xi​yi​∑xi​​∑xi​yi​∑yi2​∑yi​​∑xi​∑yi​n​ ​⋅ ​ABC​ ​= ​∑zi​xi​∑zi​yi​∑zi​​ ​
构建矩阵方程Ax = b, 其中A为上述方程中左边的参数矩阵方程， x为变量（ABC）, b为等式右边的变量。

通过矩阵运算得到 $x={\text{ A }}^{-1}\ast b$, 最终得到参数向量[A B C]。

结果展示：

随机给一组点进行测试：（该点为三维坐标系中Oxy平面附近的一组点，坐标的z值在0附近小范围波动，拟合的平面法向量应该近似等于(0,0,1)）。

完整代码如下：

#include 
#include 
#include 

void CaculatefitPlane(std::vector points, std::vector &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(0, 0) = xi2;
	A.at(1, 0) = xiyi;
	A.at(2, 0) = xi;
	A.at(0, 1) = xiyi;
	A.at(1, 1) = yi2;
	A.at(2, 1) = yi;
	A.at(0, 2) = xi;
	A.at(1, 2) = yi;
	A.at(2, 2) = points.size();
	B.at(0, 0) = zixi;
	B.at(1, 0) = ziyi;
	B.at(2, 0) = zi;

	// step3 calculate plane
    // Ax+by+cz=D, c = 1
	X = A.inv() * B;

	//A
	res.push_back(X.at(0, 0));
	//B
	res.push_back(X.at(1, 0));
	//Z的系数为1
	res.push_back(1.0);
	//C
	res.push_back(X.at(2, 0));
	return;
}


int main()
{
	std::vector points3d;
	std::vector 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;
	}
}

结果如下：