Camera Centric Parameterization
1. Camera and image plane coordinate
pi = (ρi, ϕi,θi)
(ox,oy)
u
v
θi
ϕi
X
i
ρi=1/d
Camera
Center
Y
Z
2. System Coordinate
Camera Current
Location
(0,0,0)
Camera Predicted
Location
(Cx,Cy,Cz)
ck, rk
hc
(xi,yi,zi)k
(xi,yi,zi)k-1
pi =
(ox,oy)
u
v
θi
X
m
Feature
Acquired
Y
Z
ϕi
ρi=1/d
i
(Xi,Yi,Zi)
or
(ρi, ϕi,θi)
2.1.
State Vector
𝑻
State vector is composed of the world reference point 𝑶𝑪 camera motion parameters[𝒄𝑪 , 𝒓𝑪 ] , and
feature location parameters 𝒑𝑪𝒊 in camera centered frame.
𝑶𝑿𝐶𝑾
𝒄𝑪
𝑪
𝒙 = 𝒓𝑪
𝒑𝟏
𝒑𝑪𝟐
[ ⋮ ]
𝑪
where O contains the world origin coordinate and orientation in camera frame. 𝑂𝑋𝑊
=
[𝑶𝑪𝒙
𝑶𝑪𝒚
𝑶𝑪𝒛 𝑾𝑪𝒙
𝑾𝑪𝒚
𝑻
𝑾𝑪𝒛 ] . The parameter 𝑝𝑖𝐶 contains the feature coordinate in camera frame
𝑻
represented in inverse parameterization: 𝑝𝑖𝐶 = [𝒙𝑪𝒊 𝒚𝑪𝒊 𝒛𝑪𝒊 𝝆𝒊 𝝓𝑪𝒊 𝜽𝑪𝒊 ] . The first three parameters
[𝒙𝑪𝒊 , 𝒚𝑪𝒊 , 𝒛𝑪𝒊 ] memorize the camera’s coordinate when the feature is first observed. The elevationazimuth pair [𝝓𝑪𝒊 , 𝜽𝑪𝒊 ] encodes a unit vector pointing from initialization point [𝒙𝑪𝒊 , 𝒚𝑪𝒊 , 𝒛𝑪𝒊 ] to the feature,
and 𝝆𝒊 is the inverse distance along this vector.
2.2.
Prediction
2.2.1. Method 1
No change is made to the state vector except the camera translation and rotation. If INS unit is available,
the INS measurement is assigned to 𝒄𝑪 and 𝒓𝑪 . Otherwise, 𝒄𝑪 and 𝒓𝑪 will be assigned new value based
on the estimation model used. For now, only the constant velocity is implemented to compare with the
INS measurement.
𝑶𝑿𝐶𝑾
𝑶𝑿𝐶𝑾
𝒄𝑪𝑚𝑒𝑎𝑠𝑢𝑟𝑒𝑑
𝒄𝑪
𝑪
𝒓𝑪
= 𝒓𝑚𝑒𝑎𝑠𝑢𝑟𝑒𝑑
𝑪
𝒑𝟏
𝒑𝑪𝟏
𝑪
𝒑𝟐
𝒑𝑪𝟐
[ ⋮ ]𝑘+1 [
]
⋮
or
𝑶𝑿𝐶𝑾
𝑶𝑿𝐶𝑾
𝒄𝑪
𝒄𝑪
𝑪
𝑪
𝒓
= 𝒓𝑪
𝑪
𝒑𝟏
𝒑𝟏
𝑪
𝒑𝟐
𝒑𝑪𝟐
[ ⋮ ]𝑘+1 [ ⋮ ]𝑘
Writing the above in the form of 𝒙𝒌 = 𝐅𝒌−𝟏 𝒙𝒌−𝟏 + 𝑩𝒌 𝒖𝒌 + 𝒘𝒌−𝟏 , without INS measurement, 𝐅𝒌 is
simply 𝑰. With INS,
𝑰𝟔×𝟔
𝟎𝟑×𝟑
𝟎
𝟎𝟑×𝟑
𝒙𝒌 =
𝑰𝟔×𝟔
𝟎
[
⋱
𝟎𝟔×𝟗
𝑶𝑿𝐶𝑾
1 0 0 1 0 0
𝒄𝑪
0 1 0 0 1 0
𝒓𝑪
+ 0 0 1 0 0 1
𝑪
𝒑𝟏
𝟎𝟑×𝟑
𝟎𝟑×𝟑
⋮
𝑪
𝑰𝟔×𝟔 ] [ 𝒑𝒏 ]𝑘−1
𝟎𝟔𝒏×𝟗
[
𝑣𝑥𝐶 ∆𝑡
𝑣𝑦𝐶 ∆𝑡
𝑣𝑧𝐶 ∆𝑡
1 𝐶 2
𝑎 ∆𝑡
𝟎𝟑×𝟑
2 𝑥
1 𝐶 2
1 0 0 2 𝑎𝑦 ∆𝑡
0 1 0 1
𝐶
2
0 0 1 2 𝑎𝑧 ∆𝑡
]
𝐶
𝑤𝑥
𝑤𝐶𝑦
[
𝑤𝐶𝑧
]
+ 𝒘𝒌−𝟏
2.2.2.
Method 2
𝑻
For every prediction step, camera motion can be described by its translation and rotation [𝒄𝑪 , 𝒓𝑪 ] .
These parameters can either be estimated by including them into the state vector, or acquired from an
INS measurement unit.
World reference point coordinate and orientation from k-1 to k is related by
𝑂𝑥𝐶
𝑂𝑥𝐶
𝑐𝑥𝐶
− [𝑐𝑦𝐶 ] )
[𝑂𝑦𝐶 ] = 𝑅 −1 (𝒓𝑪 ) ([𝑂𝑦𝐶 ]
𝑂𝑧𝐶 𝑘
𝑂𝑧𝐶 𝑘−1
𝑐𝑧𝐶 𝑘
𝑊𝑥𝐶
𝑊𝑥𝐶
= [ 𝑊𝑦𝐶 ] − 𝒓𝑪
[ 𝑊𝑦𝐶 ]
𝑊𝑧𝐶 𝑘−1
𝑊𝑧𝐶 𝑘
Feature parameters are related from one step to the next by
𝑥𝑖𝐶
𝑥𝑖𝐶
𝑐𝑥𝐶
− [𝑐𝑦𝐶 ] )
[𝑦𝑖𝐶 ] = 𝑅 −1 (𝒓𝑪 ) ([𝑦𝑖𝐶 ]
𝑐𝑧𝐶 𝑘
𝑧𝑖𝐶 𝑘
𝑧𝑖𝐶 𝑘−1
𝜙𝑖𝐶
𝐶
𝐶
𝒎−𝟏 (𝑅−1 (𝒓𝑪 ) 𝒎(𝜙𝑖,𝑘−1
, 𝜃𝑖,𝑘−1
)
)
[ 𝜃𝑖𝐶 ] = (
𝜌𝑖,𝑘−1
𝜌𝑖 𝑘
𝐶
𝐶
where 𝒎(𝜙𝑖,𝑘−1
, 𝜃𝑖,𝑘−1
) is the unit vector pointing from the initialization point to the feature seen by
the camera at step k-1
𝐶
𝐶
𝑐𝑜𝑠𝜙𝑖,𝑘−1
𝑐𝑜𝑠𝜃𝑖,𝑘−1
𝐶
𝐶
𝐶
𝐶
𝑠𝑖𝑛𝜃𝑖,𝑘−1
𝒎(𝜙𝑖,𝑘−1
, 𝜃𝑖,𝑘−1
) = [ 𝑐𝑜𝑠𝜙𝑖,𝑘−1
]
𝐶
sin𝜙𝑖,𝑘−1
Let 𝒉𝑹
𝒌 be the predicted vector pointing from camera location to feature at step k, then
𝐶
𝐶
𝑐𝑜𝑠𝜙𝑖,𝑘−1
𝑐𝑜𝑠𝜃𝑖,𝑘−1
𝐶
𝐶
−1 𝑪
𝒉𝑹
𝒌 = 𝑅 (𝒓 ) [ 𝑐𝑜𝑠𝜙𝑖,𝑘−1 𝑠𝑖𝑛𝜃𝑖,𝑘−1 ]
𝐶
sin𝜙𝑖,𝑘−1
and
𝜙𝐶
[ 𝐶𝑖 ] = 𝒎−𝟏 (𝒉𝑹
𝒌) =
𝜃𝑖 𝑘
2
(
𝑅
𝑅
√ℎ𝑥,𝑘−1
+ ℎ𝑦,𝑘−1
2
)
𝑅
ℎ𝑦,𝑘−1
arctan ( 𝑅
)
ℎ𝑥,𝑘−1
[
2.3.
𝑅
ℎ𝑧,𝑘−1
arctan
]
Prediction Matrix Linearization
2.3.1. Method 1
The matrices are linear. No linearization is needed.
2.3.2. Method 2
𝜕𝒙𝒌
𝜕𝒙𝒌
𝜕𝒙𝒌
=[
𝐶
𝜕𝒙𝒌−𝟏
𝜕𝑶𝑿𝑾,𝒌−𝟏 𝜕𝒄𝒌−𝟏
𝜕𝒙𝒌
𝜕𝒙𝒌
𝜕𝒙𝒌
⋯]
𝑪
𝜕𝒓𝒌−𝟏 𝜕𝒑𝟏,𝒌−𝟏 𝜕𝒑𝑪𝟐,𝒌−𝟏
With respect to 𝑶𝑿𝐶𝑾 :
𝜕𝑶𝑪𝑾,𝒌
𝜕𝑶𝑪𝑾,𝒌
𝜕𝑶𝑪𝑾,𝒌−𝟏
𝜕𝑾𝑪𝑾,𝒌
𝜕𝑶𝑪𝑾,𝒌−𝟏
𝜕𝑾𝑪𝑾,𝒌−𝟏
𝜕𝑾𝑪𝑾,𝒌
𝜕𝑾𝑪𝑾,𝒌−𝟏
𝜕𝒙𝒌
𝜕𝒄𝒌
=
𝐶
𝜕𝑶𝑿𝑾,𝒌−𝟏
𝜕𝑶𝑪𝑾,𝒌−𝟏
𝜕𝒓𝒌
𝜕𝑾𝑪𝑾,𝒌−𝟏
𝜕𝒓𝒌
𝜕𝑶𝑪𝑾,𝒌−𝟏
𝜕𝑾𝑪𝑾,𝒌−𝟏
𝜕𝒑𝑪𝒊
𝑪
[𝜕𝑶𝑾,𝒌−𝟏
𝜕𝒑𝑪𝒊
𝜕𝑾𝑪𝑾,𝒌−𝟏 ]
𝜕𝒄𝒌
𝜕𝑶𝑪𝑾,𝒌
𝜕𝑶𝑪𝑾,𝒌−𝟏
𝜕𝑶𝑪𝑾,𝒌
𝜕𝑶𝑪𝑾,𝒌−𝟏
0
=
[
= 𝑅 −1 (𝒓𝑪 )
0
𝜕𝑾𝑪𝑾,𝒌
𝜕𝑾𝑪𝑾,𝒌−𝟏
0
0
0
0
0
0
]
𝜕𝑾𝑪𝑾,𝒌
𝜕𝑾𝑪𝑾,𝒌−𝟏
=𝟏
With respect to 𝒄 and 𝒑
Since 𝒄 and 𝒓 doesn’t change, their derivative is an identity matrix.
𝜕𝑶𝑪𝑾,𝒌
𝜕𝒄𝒌−𝟏
𝜕𝑾𝑪𝑾,𝒌
𝜕𝒄𝒌−𝟏
𝜕𝒄𝒌
𝜕𝒄𝒌−𝟏
𝜕𝒓𝒌
𝜕𝒄𝒌−𝟏
𝜕𝒑(0: 2)𝑪𝒊
𝜕𝒄𝒌−𝟏
𝜕𝒑(3: 5)𝑪𝒊
[ 𝜕𝒄𝒌−𝟏
𝑪
𝜕𝑶𝑪𝑾,𝒌
𝜕𝑅 −1 (𝒓𝒌𝒌−𝟏 )
−1 𝑪𝒌−𝟏
−𝑅 (𝒓𝒌 )
𝜕𝒓𝒌−𝟏
𝜕𝒓𝒌−𝟏
𝜕𝑾𝑪𝑾,𝒌
0
−𝑰
𝜕𝒓𝒌−𝟏
𝜕𝒄𝒌
𝐼
0
𝜕𝒓𝒌−𝟏
=
𝜕𝒓𝒌
𝜕𝒓𝒌−𝟏
0
𝐼
𝑪
𝜕𝒑(0: 2)𝒊
𝑪
𝜕𝑅 −1 (𝒓𝒌𝒌−𝟏 )
𝑪𝒌−𝟏
−1
𝜕𝒓𝒌−𝟏
−𝑅 (𝒓𝒌 )
𝜕𝒓𝒌−𝟏
𝑪
𝜕𝒑(3: 5)𝒊
]
𝜕𝒓𝒌−𝟏 ] [
0
?
1
0 0
𝐶
𝜕𝜙𝑖,𝑘
? = 𝜕𝑟𝑘−1
𝐶
𝜕𝜃𝑖,𝑘
[ 𝜕𝑟𝑘−1 ]
𝐶
𝜕𝜙𝑖,𝑘
𝐶
𝐶
𝜕𝒎−𝟏 (𝑅 −1 (𝒓𝑪𝒌−𝟏 )𝒎(𝜙𝑖,𝑘𝑘−1 , 𝜃𝑖,𝑘𝑘−1 ))
𝜕𝒓𝒌−𝟏
=
𝐶
𝜕𝒓𝒌−𝟏
𝜕𝜃𝑖,𝑘
[𝜕𝒓𝒌−𝟏 ]
𝐶
𝐶
Let 𝑀(𝒓𝒌−𝟏 ) = 𝑅 −1 (𝒓𝑪𝒌−𝟏 )𝒎(𝜙𝑖,𝑘𝑘−1 , 𝜃𝑖,𝑘𝑘−1 )
𝜕𝑚−1 𝜕𝑚−1 𝜕𝑀
=
∙
𝜕𝑟𝑘−1
𝜕𝑀 𝜕𝑟𝑘−1
𝑪
𝜕𝑅 −1 (𝒓𝒌𝒌−𝟏 )
𝜕𝑀
𝐶
𝐶
=
∙ 𝒎(𝜙𝑖,𝑘𝑘−1 , 𝜃𝑖,𝑘𝑘−1 )
𝜕𝑟𝑘−1
𝜕𝒓𝒌−𝟏
𝜕𝑚−1
𝜕𝑀
With respect to 𝒑𝑪𝒊
𝜕𝜙𝐶
𝜕𝜃𝐶
𝒌
𝒌
is 𝜕𝒉𝑹𝑘 and 𝜕𝒉𝑘𝑹
𝜕𝑶𝑪𝑾,𝒌
0
𝜕𝒑𝑪𝒊,𝒌−𝟏
𝜕𝑾𝑪𝑾,𝒌
𝜕𝒙𝒌
𝜕𝒑𝑪𝒊,𝒌−𝟏
0
𝜕𝒑𝑪𝒊,𝒌−𝟏
𝜕𝑐𝑘
=
0
=
𝜕𝒑𝑪𝒊,𝒌−𝟏
𝜕𝑟𝑘
0
𝜕𝒑𝑪𝒊,𝒌−𝟏
𝜕𝒑𝑪𝒊,𝒌
𝜕𝒑𝑪𝒊,𝒌
𝑪
[𝜕𝒑𝒊,𝒌−𝟏 ]
𝑪
[𝜕𝒑𝒊,𝒌−𝟏 ]
𝐶
𝜕𝑥𝑖,𝑘
𝜕𝒑𝑪𝒊,𝒌−𝟏
𝐶
𝜕𝑦𝑖,𝑘
𝑅 −1 (𝒓𝑪 )
𝜕𝒑𝑪𝒊,𝒌−𝟏
𝜕𝒑𝑪𝒊,𝒌
𝜕𝒑𝑪𝒊,𝒌−𝟏
𝜕𝒑𝑪𝒊,𝒌
𝜕𝒑𝑪𝒊,𝒌−𝟏
0
𝐶
𝜕𝜙𝑖,𝑘
𝑪
= 𝜕𝒑𝒊,𝒌−𝟏 =
𝐶
𝜕𝜃𝑖,𝑘
𝑪
[𝜕𝒑𝒊,𝒌−𝟏 ]
For the ith feature point,
0
0
=
𝐶
𝜕𝑧𝑖,𝑘
𝜕𝒑𝑪𝒊,𝒌−𝟏
𝐶
𝜕𝜌𝑖,𝑘
𝜕𝒑𝑪𝒊,𝒌−𝟏
𝐶
𝜕𝜙𝑖,𝑘
𝜕𝒑𝑪𝒊,𝒌−𝟏
𝐶
𝜕𝜃𝑖,𝑘
𝑪
[𝜕𝒑𝒊,𝒌−𝟏 ]
0
𝐶
𝜕𝜙𝑖,𝑘
𝐶
𝜕𝜙𝑖,𝑘−1
𝐶
𝜕𝜃𝑖,𝑘
0
𝐶
𝜕𝜙𝑖,𝑘−1
[
𝐶
𝜕𝜌𝑖,𝑘
0
=
𝜕𝒑(𝟑: 𝟓)𝑪𝒊,𝒌−𝟏
𝐶
𝜕𝜙𝑖,𝑘
0
[
𝜕𝒑(𝟑: 𝟓)𝑪𝒊,𝒌−𝟏
𝐶
𝜕𝜃𝑖,𝑘
0
0
𝐶
𝜕𝜙𝑖,𝑘
𝐶
𝜕𝜃𝑖,𝑘−1
=
𝐶
𝜕𝜃𝑖,𝑘
𝐶
𝜕𝜃𝑖,𝑘−1
]
0
𝜕𝒑(𝟑: 𝟓)𝑪𝒊,𝒌−𝟏 ]
0
0
0
[
0
𝐶
𝜕𝜙𝑖,𝑘
𝜕𝒉𝑹
𝒊,𝒌
0
𝐶
𝜕𝜙𝑖,𝑘
𝜕𝒉𝑹
𝒊,𝒌
𝐶
𝜕𝒉𝑹
𝒊,𝒌 𝜕𝜙𝑖,𝑘−1
𝐶
𝜕𝒉𝑹
𝒊,𝒌 𝜕𝜃𝑖,𝑘−1
𝐶
𝜕𝜃𝑖,𝑘
𝐶
𝜕𝜃𝑖,𝑘
𝜕𝒉𝑹
𝒊,𝒌
𝜕𝒉𝑹
𝒊,𝒌
𝜕𝒉𝑹
𝒌
𝐶
𝜕𝜙𝑖,𝑘−1
𝐶
𝜕𝒉𝑹
𝒊,𝒌 𝜕𝜃𝑖,𝑘−1 ]
𝑇
𝑅
𝑅
−ℎ𝑥,𝑘
∙ ℎ𝑧,𝑘
2
2
2
2
2
2
2
𝑅
𝑅
𝑅
𝑅
𝑅
(ℎ𝑥,𝑘
+ ℎ𝑦,𝑘
+ ℎ𝑧,𝑘
) √ℎ𝑥,𝑘
+ ℎ𝑦,𝑘
𝜕𝜙𝑘𝐶
𝜕𝒉𝑹
𝒌
=
𝜕𝜙𝑘𝐶
[ 𝑅
𝜕ℎ𝑥,𝑘
𝜕𝜙 𝐶𝑘
𝑅
𝜕ℎ𝑦,𝑘
𝜕𝜙 𝐶𝑘
𝑅 ]
𝜕ℎ𝑧,𝑘
𝑅
𝐶
−ℎ𝑦,𝑘
∙ ℎ𝑧,𝑘
=
2
2
2
𝑅
𝑅
𝑅
𝑅
𝑅
(ℎ𝑥,𝑘
+ ℎ𝑦,𝑘
+ ℎ𝑧,𝑘
) √ℎ𝑥,𝑘
+ ℎ𝑦,𝑘
2
𝑅
𝑅
√ℎ𝑥,𝑘
+ ℎ𝑦,𝑘
2
𝜕𝒉𝑹
𝒌
= [−
𝑅
ℎ𝑦,𝑘
2
𝑅
𝑅
ℎ𝑥,𝑘
+ ℎ𝑦,𝑘
2
2
𝑅
𝑅
𝑅
(ℎ𝑥,𝑘
+ ℎ𝑦,𝑘
+ ℎ𝑧,𝑘
)
[
𝜕𝜃𝑘𝐶
2
𝑅
ℎ𝑥,𝑘
2
2
𝑅
𝑅
ℎ𝑥,𝑘
+ ℎ𝑦,𝑘
2
0]
𝐶
𝐶
− cos(𝜃𝑘−1
) sin(𝜙𝑘−1
)
𝜕𝒉𝑹
𝒌
𝐶
𝐶
−1 𝑪
= 𝑅 (𝒓 ) [ − sin(𝜙𝑘−1 ) sin(𝜃𝑘−1 ) ]
𝐶
𝜕𝜙𝑘−1
𝐶
cos(𝜙𝑘−1
)
𝐶
𝐶
− cos(𝜙𝑘−1
) sin(𝜃𝑘−1
)
𝜕𝒉𝑹
𝒌
−1 𝑪
𝐶
𝐶
=
𝑅
(𝒓
)
[
cos(𝜙𝑘−1 ) cos(𝜃𝑘−1 ) ]
𝐶
𝜕𝜃𝑘−1
0
2.4.
Measurement Model
2.4.1. Method 1
Measurement are the image pixel location of the feature 𝑼 = [𝑢 𝑣]𝑇
𝑥𝑘𝐶
𝑐𝑥𝐶
𝒉𝑪𝒌 = 𝑅 −1 (𝒓𝑪𝒌 ) 𝜌𝑘 ([𝑦 𝐶𝑘 ] − [𝑐𝑦𝐶 ]) + 𝒎(𝜙 𝐶𝑘 , 𝜃𝑘𝐶 )
𝑧 𝐶𝑘
𝑐𝑧𝐶
(
)
𝐶
𝑠𝑥 ℎ𝑦,𝑘
𝒉𝑼
𝒌
𝐶
ℎ𝑥,𝑘
𝑢𝑘
= [𝑣 ] =
𝐶
𝑘
𝑠𝑦 ℎ𝑧,𝑘
𝐶
[ ℎ𝑥,𝑘 ]
2.4.2. Method 2
The measurement model for the ith feature point is given by simple vector summation:
𝑥𝑘𝐶
𝒉𝑪𝒌 = 𝜌𝑘 [𝑦 𝐶𝑘 ] + 𝒎(𝜙 𝐶𝑘 , 𝜃𝑘𝐶 )
𝑧 𝐶𝑘
]
𝐶
𝑠𝑥 ℎ𝑦,𝑘
𝐶
ℎ𝑥,𝑘
𝑢𝑘
𝒉𝑼
𝒌 = [𝑣 ] =
𝐶
𝑘
𝑠𝑦 ℎ𝑧,𝑘
𝐶
[ ℎ𝑥,𝑘 ]
2.5.
Measurement Matrix Linearization
2.5.1. Method 1
𝜕𝒉(𝒙)
𝜕𝒉(𝒙)
𝜕𝒉(𝒙) 𝜕𝒉(𝒙) 𝜕𝒉(𝒙)
= [𝟎𝟐𝒏×𝟔
]
…
𝑪
𝑪
𝜕𝒙
𝜕𝒑𝟏
𝜕𝒑𝒏
𝜕𝒄
𝜕𝒓
𝜕𝒉𝑼 𝜕𝒉𝑪
𝜕𝒉𝑼 𝜕𝒉𝑪
𝜕𝒉𝑼 𝜕𝒉𝑪
= [𝟎𝟐𝒏×𝟔
∙
∙
∙
𝜕𝒉𝑪 𝜕𝒑𝟏
𝜕𝒉𝑪 𝜕𝒄𝑪
𝜕𝒉𝑪 𝜕𝒓𝑪
𝜕𝒉𝑼
𝒌
𝜕𝒉𝑪𝒌
−
=
[
−
𝐶
𝑠𝑥 ℎ𝑦,𝑘
𝐶 2
ℎ𝑥,𝑘
𝐶
𝑠𝑦 ℎ𝑧,𝑘
𝐶 2
ℎ𝑥,𝑘
𝑠𝑥
𝐶
ℎ𝑥,𝑘
0
…
𝜕𝒉𝑼 𝜕𝒉𝑪
]
∙
𝜕𝒉𝑪 𝜕𝒑𝒏
0
𝑠𝑦
, (in code, fx is Sx, fy is Sy)
𝐶
ℎ𝑥,𝑘
]
𝜕𝒉𝑪
= −𝑅−1 (𝒓𝑪𝒌 )𝜌𝑘
𝜕𝒄𝑪
𝜕𝒉(𝒙)
𝜕𝒉𝑪 𝜕 𝒉𝑪 𝜕 𝒉𝑪
=
[
]
𝜕𝒓𝑪
𝜕𝑟𝑥𝐶 𝜕𝑟𝑦𝐶 𝜕𝑟𝑧𝐶
𝜕𝒉𝑪
𝜕𝑟𝑥
0
𝑤
𝑤
𝑤
= [ℎ𝑥 (𝑠𝑖𝑛(𝑟𝑥 )𝑠𝑖𝑛(𝑟𝑧 ) + 𝑐𝑜𝑠(𝑟𝑥 )𝑐𝑜𝑠(𝑟𝑧 )𝑠𝑖𝑛(𝑟𝑦 )) + ℎ𝑦 (−𝑐𝑜𝑠(𝑟𝑧 )𝑠𝑖𝑛(𝑟𝑥 ) + 𝑐𝑜𝑠(𝑟𝑥 )𝑠𝑖𝑛(𝑟𝑦 )𝑠𝑖𝑛(𝑟𝑧 )) + ℎ𝑧 𝑐𝑜𝑠(𝑟𝑥 )𝑐𝑜𝑠(𝑟𝑦 )]
𝑤
𝑤
ℎ𝑥 (𝑐𝑜𝑠(𝑟𝑥 )𝑠𝑖𝑛(𝑟𝑧 ) − 𝑐𝑜𝑠(𝑟𝑧 )𝑠𝑖𝑛(𝑟𝑥 )𝑠𝑖𝑛(𝑟𝑦 )) + ℎ𝑦 (−𝑐𝑜𝑠(𝑟𝑥 )𝑐𝑜𝑠(𝑟𝑧 ) − 𝑠𝑖𝑛(𝑟𝑥 )𝑠𝑖𝑛(𝑟𝑦 )𝑠𝑖𝑛(𝑟𝑧 )) − ℎ𝑧𝑤 𝑐𝑜𝑠(𝑟𝑦 )𝑠𝑖𝑛(𝑟𝑥 )
−ℎ𝑧𝑤 𝑐𝑜𝑠(𝑟𝑦 ) − ℎ𝑥𝑤 𝑐𝑜𝑠(𝑟𝑧 ) ∗ 𝑠𝑖𝑛(𝑟𝑦 ) − ℎ𝑦𝑤 𝑠𝑖𝑛(𝑟𝑦 )𝑠𝑖𝑛(𝑟𝑧 )
𝜕𝒉𝑪
= [ −ℎ𝑧𝑤 𝑠𝑖𝑛(𝑟𝑥 )𝑠𝑖𝑛(𝑟𝑦 ) + ℎ𝑥𝑤 𝑐𝑜𝑠(𝑟𝑦 )𝑐𝑜𝑠(𝑟𝑧 )𝑠𝑖𝑛(𝑟𝑥 ) + ℎ𝑦𝑤 𝑐𝑜𝑠(𝑟𝑦 )𝑠𝑖𝑛(𝑟𝑥 )𝑠𝑖𝑛(𝑟𝑧 ) ]
𝜕𝑟𝑦
−ℎ𝑧𝑤 𝑐𝑜𝑠(𝑟𝑥 )𝑠𝑖𝑛(𝑟𝑦 ) + ℎ𝑥𝑤 𝑐𝑜𝑠(𝑟𝑥 )𝑐𝑜𝑠(𝑟𝑦 )𝑐𝑜𝑠(𝑟𝑧 ) + ℎ𝑦𝑤 𝑐𝑜𝑠(𝑟𝑥 )𝑐𝑜𝑠(𝑟𝑦 )𝑠𝑖𝑛(𝑟𝑧 )
ℎ𝑦𝑤 𝑐𝑜𝑠(𝑟𝑦 )𝑐𝑜𝑠(𝑟𝑧 ) − ℎ𝑥𝑤 𝑐𝑜𝑠(𝑟𝑦 )𝑠𝑖𝑛(𝑟𝑧 )
𝑪
𝜕𝒉
= [ℎ𝑥𝑤 (−𝑐𝑜𝑠(𝑟𝑥 )𝑐𝑜𝑠(𝑟𝑧 ) − 𝑠𝑖𝑛(𝑟𝑥 )𝑠𝑖𝑛(𝑟𝑦 )𝑠𝑖𝑛(𝑟𝑧 )) + ℎ𝑦𝑤 (−𝑐𝑜𝑠(𝑟𝑥 )𝑠𝑖𝑛(𝑟𝑧 ) + 𝑐𝑜𝑠(𝑟𝑧 )𝑠𝑖𝑛(𝑟𝑥 )𝑠𝑖𝑛(𝑟𝑦 ))]
𝜕𝑟𝑧
ℎ𝑥𝑤 (𝑐𝑜𝑠(𝑟𝑧 )𝑠𝑖𝑛(𝑟𝑥 ) − 𝑐𝑜𝑠(𝑟𝑥 )𝑠𝑖𝑛(𝑟𝑦 )𝑠𝑖𝑛(𝑟𝑧 )) + ℎ𝑦𝑤 (𝑠𝑖𝑛(𝑟𝑥 )𝑠𝑖𝑛(𝑟𝑧 ) + 𝑐𝑜𝑠(𝑟𝑥 )𝑐𝑜𝑠(𝑟𝑧 )𝑠𝑖𝑛(𝑟𝑦 ))
𝜕𝒉𝑪
𝜕𝒑𝒊
𝜕𝒉𝑪
= [𝜕𝑥 𝐶
𝑖
𝜕𝒉𝑪
𝜕𝒉𝑪
𝜕𝑦𝑖𝐶
𝜕𝑧𝑖𝐶
𝜕𝒉𝑪
𝜕𝜌𝑖
𝜕𝒉𝑪
𝜕𝜃𝑖
𝜕𝒉𝑪
],
𝜕𝜙𝑖
𝑹𝑾𝟐𝑪 = 𝑅 −1 (𝒓𝑪𝒌 )
𝜕𝒉𝑪
[ 𝐶
𝜕𝑥𝑖
𝜕𝒉𝑪
𝜕𝑦𝑖𝐶
𝜕𝒉𝑪
] = 𝑹𝑾𝟐𝑪 𝜌𝑘
𝜕𝑧𝑖𝐶
3x6
𝜕𝒉𝑪
= −𝑹𝑾𝟐𝑪 ∙ 𝒄𝒌
𝜕𝜌𝑖
−cos(𝜙𝑖 ) sin(𝜃𝑖 )
𝜕𝒉𝑪
= 𝑹𝑾𝟐𝑪 [ cos(𝜙𝑖 ) cos(𝜃𝑖 ) ]
𝜕𝜃𝑖
0
−cos(𝜙𝑖 ) sin(𝜃𝑖 )
𝜕𝒉𝑪
= 𝑹𝑾𝟐𝑪 [ −sin(𝜙𝑖 )sin(𝜃𝑖 ) ]
𝜕𝜙𝑖
cos(𝜙𝑖 )
2.5.2. Method 2
𝜕𝒉𝑼
𝒌
𝜕𝒉𝑪𝒌
−
=
𝐶
𝑠𝑥 ℎ𝑦,𝑘
𝐶 2
ℎ𝑥,𝑘
𝐶
𝑠𝑦 ℎ𝑧,𝑘
−
𝐶 2
[ ℎ𝑥,𝑘
𝑠𝑥
𝐶
ℎ𝑥,𝑘
0
0
𝑠𝑦
𝐶
ℎ𝑥,𝑘
]
𝜕𝒉𝑪𝒌
= 𝜌𝑘
𝑥𝑘𝐶
𝜕 [𝑦 𝐶𝑘 ]
𝑧 𝐶𝑘
𝜕𝒉𝑪𝒌
𝜕𝒉𝑪𝒌 𝜕𝒉𝑪𝒌
=[ 𝐶
]
𝜕[𝜙 𝐶𝑘 , 𝜃𝑘𝐶 ]𝑇
𝜕𝜙 𝑘 𝜕𝜃 𝐶𝑘
𝐶
𝐶
− cos(𝜃𝑘−1
) sin(𝜙𝑘−1
)
𝜕𝒉𝑪𝒌
𝐶
𝐶
= [ − sin(𝜙𝑘−1 ) sin(𝜃𝑘−1 ) ]
𝜕𝜙 𝐶𝑘
𝐶
cos(𝜙𝑘−1
)
𝐶
𝐶
− cos(𝜙𝑘−1
) sin(𝜃𝑘−1
)
𝜕𝒉𝑪𝒌
𝐶
𝐶
= [ cos(𝜙𝑘−1 ) cos(𝜃𝑘−1 ) ]
𝜕𝜃 𝐶𝑘
0
2.6.
Additional Step for Method 1
Update step corrects the camera motion and feature location in camera frame k-1. To continue to the
next cycle of tracking, all parameter must be transform to camera frame k. World reference point
coordinate and orientation from k-1 to k is related by
𝐶
𝐶
𝐶
𝑂𝑥 𝑘−1
𝑐𝑥 𝑘−1
𝑂𝑥 𝑘
𝑪
𝐶
𝐶
[ 𝑂𝑦𝐶 ] = 𝑅 −1 (𝒓𝒌𝒌−𝟏 ) [𝑂𝑦 𝑘−1 ] − [𝑐𝑦 𝑘−1 ]
𝐶𝑘−1
𝐶
𝑂𝑧𝐶 𝑘
𝑐𝑧 𝑘−1 𝑘 )
( 𝑂𝑧
𝑘
𝐶
𝐶
𝑊𝑥 𝑘
𝑊𝑥 𝑘−1
𝐶
𝐶
= [ 𝑊𝑦 𝑘−1 ] − 𝒓𝑪𝒌−𝟏
[ 𝑊𝑦 𝑘 ]
𝐶𝑘
𝑊𝑧
𝐶𝑘−1
𝑘−1
𝑊𝑧
𝑘
Feature parameters are related from one step to the next by
𝐶
𝐶
𝑥𝑖 𝑘
𝐶
𝑥𝑖 𝑘−1
𝑐𝑥 𝑘−1
𝐶
[𝑦𝑖𝐶𝑘 ] = 𝑅 −1 (𝒓𝑪𝒌−𝟏 ) [𝑦𝑖𝐶𝑘−1 ] − [𝑐𝑦 𝑘−1 ]
𝐶
𝑧𝑖 𝑘
𝐶
𝑘−1
( 𝑧𝑖
𝑘
𝐶
𝑐𝑧 𝑘−1 𝑘 )
𝑘
𝐶
𝜙𝑖 𝑘
𝐶
𝐶
𝒎−𝟏 (𝑅−1 (𝒓𝑪𝒌−𝟏 ) 𝒎(𝜙𝑖,𝑘𝑘−1 , 𝜃𝑖,𝑘𝑘−1 )
[ 𝜃 𝐶𝑘 ] = (
)
𝑖
𝜌𝑖,𝑘
𝜌𝑖 𝑘
𝐶
𝐶
where 𝒎(𝜙𝑖,𝑘𝑘−1 , 𝜃𝑖,𝑘𝑘−1 ) is the unit vector pointing from the initialization point to the feature seen by
the camera at step k-1
The covariance matrix is also affected by this transform. Therefore must be updated. The new
covariance matrix is related to the old one by
𝐶
𝐶
𝑃𝑘 𝑘 = 𝐽𝐶𝑘−1 →𝐶𝑘 𝑃𝑘 𝑘−1 𝐽𝐶𝑇𝑘−1 →𝐶𝑘
The calculation of 𝐽𝐶𝑘−1 →𝐶𝑘 is the same as the linearization of prediction matrix in section 2.3 Method 2.
2.7.
State Vector Initialization and Derivatives
The initialization function extracts the feature’s parameters from feature coordinates in image plane.
First, a vector pointing from camera optical center to feature can be defined by
1
ℎ𝐶 = [𝑢 ∙ 𝑠𝑥 ]
𝑣 ∙ 𝑠𝑦
Then because the feature is referenced to camera origin at initialization, the parameters [𝑥𝑖𝐶 𝑦𝑖𝐶 𝑧𝑖𝐶 ] is
the optical center [0 0 0], and the elevation-azimuth pair [𝜙𝑖𝐶 , 𝜃𝑖𝐶 ] can be directly calculated from ℎ𝐶
ℎ𝑧𝐶
𝜙 = arctan
2
2
𝐶
√ 𝐶
( ℎ𝑥 + ℎ𝑦 )
𝜃 = arctan (
ℎ𝑦𝐶
ℎ𝑥𝐶
)
𝜌 is initialized to be 0.1 because we are dealing with long distance objects, with variance of 1.
For every new feature added, the new covariance matrix becomes
𝑷
𝑷𝒏𝒆𝒘 = 𝑱 [ 𝒐𝒍𝒅
𝟎
𝟎 𝑻
]𝑱
𝑹
where 𝑷𝒐𝒍𝒅 is the covariance matrix of the existing state vector, and
𝜎𝑥 𝐶
𝑖
𝜎𝑦 𝐶
𝟎
𝑖
𝜎𝑧 𝐶
𝑹=
𝑖
𝜎𝜌0
𝜎𝑖𝑚𝑎𝑔𝑒
𝟎
𝜎𝑖𝑚𝑎𝑔𝑒 ]
[
𝜎𝑥 𝐶 , 𝜎𝑦𝐶 , 𝜎𝑧𝐶 is the uncertainty of the camera optical center position. 𝜎𝑖𝑚𝑎𝑔𝑒 is the pixel variance,
𝑖
𝑖
𝑖
𝜎𝑖𝑚𝑎𝑔𝑒 = 1, and 𝜎𝜌0 is the initial uncertainty of the inverse distance, initialized to 1 to cover any
distance from 0 to infinity. 𝑱 is the jacobian matrix for the initialization formula.
𝑰
𝑱=
𝝏𝒑𝒊
[𝝏𝑶𝑿𝑪𝑾
𝝏𝒑𝒊
𝝏𝒄𝑪
𝝏𝒑𝒊
𝝏𝒓𝑪
𝟎
⋮
𝟎
𝝏𝒑𝒊
𝟎 … 𝟎
𝝏𝒈𝒊 ]
Since 𝒑𝒊 is not a function of 𝑶𝑿𝑪𝑾 , 𝒄𝑪 , 𝒓𝑪 .Then it follows
𝝏𝒑𝒊
𝝏𝑶𝑿𝑪𝑾
𝝏𝒑𝒊
= 𝟎𝟔×𝟑
𝝏𝒄𝑪
= 𝟎𝟔×𝟔
𝝏𝒑𝒊
= 𝟎𝟔×𝟑
𝝏𝒓𝑪
𝑱 can be simplified as
𝑰
𝟎
𝝏𝒑
𝒊]
𝑱 = [𝟎
𝝏𝒈𝒊
where 𝒈𝒊 = [𝑥𝑖
𝑦𝑖
𝑧𝑖
𝜌𝑖
𝑢𝑖
𝑣𝑖 ]. Then
𝑰𝟑×𝟑
𝝏𝒑𝒊
=
𝝏𝒈𝒊
𝟎
[
𝜕𝜌
𝜕𝜌
𝜕𝜑
𝜕𝜌
𝜕𝜃
𝜕𝜌
𝟎
𝜕𝜌
𝜕𝑢
𝜕𝜑
𝜕𝑢
𝜕𝜃
𝜕𝑢
𝑰𝟑×𝟑
𝜕𝜌
𝜕𝑣
𝜕𝜑 =
𝟎
𝜕𝑣
𝜕𝜃
[
𝜕𝑣]
𝟎
0
0
𝜕𝜑 𝜕𝜑
0
𝜕𝑢 𝜕𝑣
𝜕𝜃 𝜕𝜃
0
𝜕𝑢 𝜕𝑣 ]
1
where
𝜕𝜑
𝜕𝜑 𝜕𝒉𝑪
= 𝑪
𝜕𝑢 𝜕𝒉 𝜕𝑢
𝜕𝜃
𝜕𝜃 𝜕𝒉𝑪
= 𝑪
𝜕𝑢 𝜕𝒉 𝜕𝑢
𝜕𝜑
𝜕𝜑 𝜕𝒉𝑪
= 𝑪
𝜕𝑣 𝜕𝒉 𝜕𝑣
𝜕𝜃
𝜕𝜃 𝜕𝒉𝑪
= 𝑪
𝜕𝑣 𝜕𝒉 𝜕𝑣
and
𝑇
−ℎ𝑥𝐶 ∙ ℎ𝑧𝐶
2
2
2
2
(ℎ𝑥𝐶 + ℎ𝑦𝐶 + ℎ𝑧𝐶 )√ℎ𝑥𝐶 + ℎ𝑦𝐶
2
−ℎ𝑦𝑊 ∙ ℎ𝑧𝑊
𝜕𝜑
=
2
2
2
2
2
𝜕𝒉𝑪
(ℎ𝑥𝐶 + ℎ𝑦𝐶 + ℎ𝑧𝐶 )√ℎ𝑥𝐶 + ℎ𝑦𝐶
√ℎ𝑥𝐶 2 + ℎ𝑦𝐶 2
[
2
2
2
(ℎ𝑥𝐶 + ℎ𝑦𝐶 + ℎ𝑧𝐶 )
ℎ𝑦𝐶
𝜕𝜃
−
= [ 𝐶2
2
𝜕𝒉𝑪
ℎ𝑥 + ℎ𝑦𝐶
𝜕𝒉𝑪
𝜕𝒉𝑪
=[
𝜕[𝑢 𝑣]
𝜕𝑢
]
ℎ𝑥𝐶
2
ℎ𝑥𝐶 + ℎ𝑦𝐶
0
𝜕𝒉𝑪
] = [𝑠𝑥
𝜕𝑣
0
2
0]
0
0]
𝑠𝑦