⎧⎩⎨⎪⎪x−x0=−fa1(X−Xs)+b1(Y−Ys)+c1(Z−Zs)a3(X−Xs)+b3(Y−Ys)+c3(Z−Zs),y−y0=−fa2(X−Xs)+b2(Y−Ys)+c2(Z−Zs)a3(X−Xs)+b3(Y−Ys)+c3(Z−Zs)(3)
(3)
{
x
−
x
0
=
−
f
a
1
(
X
−
X
s
)
+
b
1
(
Y
−
Y
s
)
+
c
1
(
Z
−
Z
s
)
a
3
(
X
−
X
s
)
+
b
3
(
Y
−
Y
s
)
+
c
3
(
Z
−
Z
s
)
,
y
−
y
0
=
−
f
a
2
(
X
−
X
s
)
+
b
2
(
Y
−
Y
s
)
+
c
2
(
Z
−
Z
s
)
a
3
(
X
−
X
s
)
+
b
3
(
Y
−
Y
s
)
+
c
3
(
Z
−
Z
s
)
其中x0,y0,f
x
0
,
y
0
,
f
已知,abc
a
b
c
为旋转矩阵,可以利用ϕ,ω,κ
ϕ
,
ω
,
κ
表示如下:
⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪a1=cosϕcosκ−sinϕsinωsinκ,a2=−cosϕsinκ−sinϕsinωcosκ,a3=−sinϕcosω,b1=cosωsinκ,b2=cosωcosκ,b3=−sinω,c1=sinϕcosκ+cosϕsinωsinκ,c2=−sinϕsinκ+cosϕsinωcosκ,c3=cosϕcosω(4)
(4)
{
a
1
=
c
o
s
ϕ
c
o
s
κ
−
s
i
n
ϕ
s
i
n
ω
s
i
n
κ
,
a
2
=
−
c
o
s
ϕ
s
i
n
κ
−
s
i
n
ϕ
s
i
n
ω
c
o
s
κ
,
a
3
=
−
s
i
n
ϕ
c
o
s
ω
,
b
1
=
c
o
s
ω
s
i
n
κ
,
b
2
=
c
o
s
ω
c
o
s
κ
,
b
3
=
−
s
i
n
ω
,
c
1
=
s
i
n
ϕ
c
o
s
κ
+
c
o
s
ϕ
s
i
n
ω
s
i
n
κ
,
c
2
=
−
s
i
n
ϕ
s
i
n
κ
+
c
o
s
ϕ
s
i
n
ω
c
o
s
κ
,
c
3
=
c
o
s
ϕ
c
o
s
ω
(2)步骤
(2.1)读入数据,格式为(x,y,X,Y,Z)
(
x
,
y
,
X
,
Y
,
Z
)
。前2维像素坐标—单位mm
m
m
,后三维代像坐标—单位m
m
。
(2.2)编写CostFunction结构体。
(2.3)构造一个求解非线性最小二乘法的Problem来进行未知数求解。
(3)运行结果
1.4 Powell’s Quartic Function
(1)数学表达式
f(X)=(x1+10x2)2+5(x3−x4)2+(x2−2x3)4+10(x1−x4)4(5)
其中:
∙−10≤xi≤10,i=1,2,3,4
∙
−
10
≤
x
i
≤
10
,
i
=
1
,
2
,
3
,
4
∙fmin(X∗)=0
∙
f
m
i
n
(
X
∗
)
=
0
∙x∗i=0
∙
x
i
∗
=
0
(2)Powell’s Quartic Function分解
f1(x)f2(x)f3(x)f4(x)F(x)=x1+10x2=5‾√(x3−x4)=(x2−2x3)2=10‾‾‾√(x1−x4)2=[f1(x),f2(x),f3(x),f4(x)](6)
(6)
f
1
(
x
)
=
x
1
+
10
x
2
f
2
(
x
)
=
5
(
x
3
−
x
4
)
f
3
(
x
)
=
(
x
2
−
2
x
3
)
2
f
4
(
x
)
=
10
(
x
1
−
x
4
)
2
F
(
x
)
=
[
f
1
(
x
)
,
f
2
(
x
)
,
f
3
(
x
)
,
f
4
(
x
)
]
(3)最优化问题数学描述
argminx12‖F(x)‖2(7)
(7)
arg
min
x
1
2
‖
F
(
x
)
‖
2