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为变量的线性方程组，写为矩阵形式有：
$$\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|$$
构建矩阵方程Ax = b, 其中A为上述方程中左边的参数矩阵方程， x为变量（ABC）, b为等式右边的变量。

通过矩阵运算得到 $x={\text{ A }}^{-1}\ast 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;
	}
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85

结果如下：