最小二乘法的矩阵形式为:
A
x
=
b
Ax=b
Ax=b 其中
A
A
A 为
n
∗
k
n * k
n∗k 的矩阵,
x
x
x 为
k
∗
1
k*1
k∗1 的列向量,
b
b
b 为
n
∗
1
n*1
n∗1 的列向量。如果
n
>
k
n>k
n>k(方程的个数大于未知量的个数),这个方程系统称为矛盾方程组 Over Determined System,如果
n
<
k
n<k
n<k(方程的个数小于未知量的个数),这个系统就是Under Determined System。
当找到向量
x
x
x 使得
∣
∣
A
x
−
b
∣
∣
||Ax-b||
∣∣Ax−b∣∣ 最小,则
x
x
x 为该方程的最小二乘解。
求解最小二乘的方法有奇异值分解、正规方程、QR分解三种。本文中采用正规方程对平面方程进行拟合,以实现深度相机的外参标定。正规方程组的解为:
x
=
(
A
T
A
)
−
1
A
T
b
x=(A^TA)^{-1}A^Tb
x=(ATA)−1ATb
2.平面方程拟合
平面方程的一般表达式为
A
x
+
B
y
+
C
z
+
D
=
0
(
C
≠
0
)
Ax+By+Cz+D=0 (C\neq0)
Ax+By+Cz+D=0(C=0) 将其变换为如下形式
z
=
−
A
C
x
−
B
C
y
−
D
C
z=-\frac{A}{C}x-\frac{B}{C}y-\frac{D}{C}
z=−CAx−CBy−CD 令
a
0
=
−
A
C
;
a_0=-\frac{A}{C};
a0=−CA;
a
1
=
−
B
C
;
a_1=-\frac{B}{C};
a1=−CB;
a
2
=
−
D
C
;
a_2=-\frac{D}{C};
a2=−CD;
z
=
a
0
x
+
a
1
y
+
a
2
z=a_0x+a_1y+a_2
z=a0x+a1y+a2 此时对应的最小二乘矩阵形式
A
=
(
x
1
y
1
1
x
2
y
2
1
.
.
.
x
n
y
n
1
)
;
x
=
(
a
0
a
1
a
2
)
;
b
=
(
z
1
z
2
.
.
.
z
n
)
;
(
n
≥
3
)
A=\begin{pmatrix} x_1&y_1&1\\x_2&y_2&1\\...\\x_n&y_n&1\end{pmatrix}; x=\begin{pmatrix}a_0\\a_1\\a_2\end{pmatrix};b=\begin{pmatrix} z_1 \\ z_2 \\ ...\\ z_n\end{pmatrix};(n\geq3)
A=⎝⎜⎜⎛x1x2...xny1y2yn111⎠⎟⎟⎞;x=⎝⎛a0a1a2⎠⎞;b=⎝⎜⎜⎛z1z2...zn⎠⎟⎟⎞;(n≥3)
其中
(
x
1
,
y
1
,
z
1
)
,
(
x
2
,
y
2
,
z
2
)
,
.
.
.
,
(
x
n
,
y
n
,
z
n
)
(x_1,y_1,z_1),(x_2,y_2,z_2),...,(x_n,y_n,z_n)
(x1,y1,z1),(x2,y2,z2),...,(xn,yn,zn)为输入的三维点坐标。
套用正规方程组的解,即可求得
(
a
0
,
a
1
,
a
2
)
;
(a_0,a_1,a_2);
(a0,a1,a2);
如上求解出地面的平面方程系数,则平面方程一般式为:
a
0
x
+
a
1
y
−
z
+
a
2
=
0
a_0x+a_1y-z+a_2=0
a0x+a1y−z+a2=0 其法向量为:
(
a
0
a
0
2
+
a
1
2
+
1
,
a
1
a
0
2
+
a
1
2
+
1
,
−
1
a
0
2
+
a
1
2
+
1
)
(\frac{a_0}{\sqrt{a_0^2+a_1^2+1}},\frac{a_1}{\sqrt{a_0^2+a_1^2+1}},\frac{-1}{\sqrt{a_0^2+a_1^2+1}})
(a02+a12+1a0,a02+a12+1a1,a02+a12+1−1) 求得平面法向量的单位向量为
n
⃗
\vec{n}
n。 相机坐标系的Z轴向量
z
⃗
\vec{z}
z 为
(
0
,
0
,
1
)
(0,0,1)
(0,0,1)。 旋转向量为
r
⃗
\vec{r}
r,其中
r
⃗
\vec{r}
r 方向为
n
⃗
×
z
⃗
\vec{n}\times\vec{z}
n×z ,旋转角度为
θ
=
a
r
c
c
o
s
(
n
⃗
⋅
r
⃗
)
\theta=arccos(\vec{n}\cdot\vec{r})
θ=arccos(n⋅r)