# 原因分析：

{

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=0n(Axi+Byi+Czi)2

{

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=2i=0n(Axi+Byi+Czi)xi=0J/B=2i=0n(Axi+Byi+Czi)yi=0J/C=2i=0n(Axi+Byi+Czi)=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+Czi)xi=02(Axi+Byi+Czi)yi=02(Axi+Byi+Czi)=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 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|

xi2xiyixixiyiyi2yixiyin

ABC

=

zixiziyizi

x

=

A

1

b

x=text { A }^{-1} * b

x= A 1b, 最终得到参数向量[A B C]。

# 结果展示：

#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;
}
}



THE END