basalt（一）3D点参数化
basalt 使用
2D-dir和inverse-depth对3D点进行参数化，相比VINS只优化逆深度的方式，这种参数化方式更加合理，host帧对点的方向观测确实应该优化一下
1、3D点参数化
XYZ形式的3D点参数化：
P
=
[
X
Y
Z
1
]
P=\begin{bmatrix} X\\ Y\\ Z \\ 1 \end{bmatrix}
P=⎣⎢⎢⎡XYZ1⎦⎥⎥⎤
表示成逆深度的形式：
P
=
[
X
/
d
Y
/
d
Z
/
d
1
/
d
]
,
d
=
X
2
+
Y
2
+
Z
2
P=\begin{bmatrix} X/d \\ Y/d \\ Z/d \\ 1/d \end{bmatrix}, d=\sqrt{X^2+Y^2+Z^2}
P=⎣⎢⎢⎡X/dY/dZ/d1/d⎦⎥⎥⎤,d=X2+Y2+Z2
前三维是单位向量，表示3D点的方向，后一维是逆深度，如果想使用这种方式参数化3D点，显然有一个*度是多于的，有两种处理方式：
1）采用类似VINS的方法，3D点的方向由host frame的对该3D点的观测来构造，这种方式导致无法对3D点的观测方向进行优化
2）通过stereographic projection将前三维单位向量参数化成2D-dir的形式，从而能够对2D-dir进行优化
P
(
x
,
y
,
z
)
\mathbf{P}(x,y,z)
P(x,y,z) 是3D点单位化的向量（unit-length bearing vector），位于上图黑色的单位球上
p
(
u
,
v
)
\mathbf{p}(u,v)
p(u,v) 是对
P
(
x
,
y
,
z
)
\mathbf{P}(x,y,z)
P(x,y,z) 通过stereographic投影得到的2D-dir（注意，
(
u
,
v
)
(u,v)
(u,v) 不是像素坐标），位于上图中蓝色2D平面上
1.1 stereographic投影
将空间中一个3D点
P
\mathbf{P}
P归一化到单位圆上，然后根据三角形相似，将单位球上的一点投影到平面上，即：
p
=
π
(
P
)
=
[
x
/
d
1
+
z
/
d
y
/
d
1
+
z
/
d
]
=
[
x
d
+
z
y
d
+
z
]
\begin{aligned} \mathbf{p}=\pi(\mathbf{P}) &= \begin{bmatrix} {\frac{x/d}{1 + z/d}} \\ {\frac{y/d}{1 + z/d}} \\ \end{bmatrix} = \begin{bmatrix} {\frac{x}{d + z}} \\ {\frac{y}{d + z}} \\ \end{bmatrix} \end{aligned}
p=π(P)=[1+z/dx/d1+z/dy/d]=[d+zxd+zy]
其中 d = x 2 + y 2 + z 2 d = \sqrt{x^2 + y^2 + z^2} d=x2+y2+z2 .
代码里对3D点齐次坐标
P
h
o
m
o
g
e
n
e
o
u
s
P_{homogeneous}
Phomogeneous（
4
×
1
4 \times 1
4×1）做stereographic projection时，直接取了
P
h
o
m
o
g
e
n
e
o
u
s
P_{homogeneous}
Phomogeneous的
2-norm
\textrm{2-norm}
2-norm，显然它不是深度（应该是手误的bug…），而是：
2-norm
=
(
x
/
d
)
2
+
(
y
/
d
)
2
+
(
z
/
d
)
2
+
(
1
/
d
)
2
=
1
+
(
1
/
d
)
2
\textrm{2-norm}=\sqrt{(x/d)^2 + (y/d)^2 + (z/d)^2 + (1/d)^2}=\sqrt{1+ (1/d)^2}
2-norm=(x/d)2+(y/d)2+(z/d)2+(1/d)2
=1+(1/d)2
其中，代码里三角化得到的3D点为 P n o r m a l i z e = [ x / d , y / d , z / d , 1 / d ] T P_{normalize}=[x/d,y/d,z/d,1/d]^T Pnormalize=[x/d,y/d,z/d,1/d]T，前三维是已经归一化后3D点坐标.
1.2 stereographic反投影
反投影得到
P
s
p
h
e
r
e
=
π
−
1
(
p
)
=
2
1
+
u
2
+
v
2
[
u
v
1
]
−
[
0
0
1
]
\begin{aligned} \mathbf{P_{sphere}} = \pi^{-1}(\mathbf{p}) &= \frac{2}{1 + u^2 + v^2} \begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix}-\begin{bmatrix} 0 \\ 0 \\ 1 \\ \end{bmatrix} \end{aligned}
Psphere=π−1(p)=1+u2+v22⎣⎡uv1⎦⎤−⎣⎡001⎦⎤
反投影函数对 p \mathbf{p} p 的雅可比矩阵记为 J u p 4 × 2 J_{up}^{4 \times 2} Jup4×2，最后一行只与逆深度有关，补零即可.
2、重投影雅可比
由于参数化后的3D点由2D-dir和一个inverse-depth构成，所以先要恢复出3D点:
P
n
o
r
m
l
i
z
e
=
[
P
s
p
h
e
r
e
,
1
/
d
]
T
P_{normlize}=[P_{sphere},1/d]^T
Pnormlize=[Psphere,1/d]T
其中，前三维直接由stereographic反投影得到，最后一维为逆深度
接着，根据相机模型构造如下重投影误差：
e
=
π
c
a
m
−
m
o
d
e
l
(
T
t
h
P
n
o
r
m
l
i
z
e
)
−
[
u
o
b
s
v
o
b
s
]
=
π
c
a
m
−
m
o
d
e
l
(
T
t
h
[
π
s
t
e
r
e
o
g
r
a
p
h
i
c
−
1
(
p
2
D
−
d
i
r
)
1
/
d
]
)
−
[
u
o
b
s
v
o
b
s
]
=
K
f
normalize-plane
(
T
t
h
[
π
s
t
e
r
e
o
g
r
a
p
h
i
c
−
1
(
p
2
D
−
d
i
r
)
1
/
d
]
)
−
[
u
o
b
s
v
o
b
s
]
\begin{aligned} e & = \pi_{cam-model} \left( T_{th} P_{normlize} \right) -\begin{bmatrix} u_{obs} \\ v_{obs} \end{bmatrix} \\ & = \pi_{cam-model} \left( T_{th} \begin{bmatrix} \pi^{-1}_{stereographic}(p_{2D-dir}) \\ 1/d \end{bmatrix} \right) -\begin{bmatrix} u_{obs} \\ v_{obs} \end{bmatrix} \\ & = K f_\textrm{normalize-plane}\left( T_{th} \begin{bmatrix} \pi^{-1}_{stereographic}(p_{2D-dir}) \\ 1/d \end{bmatrix} \right) -\begin{bmatrix} u_{obs} \\ v_{obs} \end{bmatrix} \end{aligned}
e=πcam−model(TthPnormlize)−[uobsvobs]=πcam−model(Tth[πstereographic−1(p2D−dir)1/d])−[uobsvobs]=Kfnormalize-plane(Tth[πstereographic−1(p2D−dir)1/d])−[uobsvobs]
对
T
t
h
T_{th}
Tth求雅可比：
∂
e
∂
ξ
t
h
=
∂
e
∂
P
t
∗
∂
P
t
∂
ξ
t
h
\frac{\partial e}{\partial \xi_{th}} = \frac{\partial e}{\partial P_t} * \frac{\partial P_t}{\partial \xi_{th}}
∂ξth∂e=∂Pt∂e∗∂ξth∂Pt
对
p
2
D
−
d
i
r
p_{2D-dir}
p2D−dir的雅可比：
∂
e
∂
p
2
D
−
d
i
r
=
∂
e
∂
P
t
∗
∂
P
t
∂
P
n
o
r
m
l
i
z
e
∗
∂
P
n
o
r
m
l
i
z
e
∂
p
2
D
−
d
i
r
=
∂
e
∂
P
t
∗
T
t
h
3
×
4
∗
J
u
p
4
×
2
\frac{\partial e}{\partial p_{2D-dir}} = \frac{\partial e}{\partial P_t} * \frac{\partial P_t}{\partial P_{normlize}} * \frac{\partial P_{normlize}}{\partial p_{2D-dir}} = \frac{\partial e}{\partial P_t} * T_{th}^{3 \times 4} * J_{up}^{4 \times 2}
∂p2D−dir∂e=∂Pt∂e∗∂Pnormlize∂Pt∗∂p2D−dir∂Pnormlize=∂Pt∂e∗Tth3×4∗Jup4×2
其中， ∂ P t ∂ P n o r m l i z e = T t h \frac{\partial P_t}{\partial P_{normlize}} = T_{th} ∂Pnormlize∂Pt=Tth
对inverse-depth的雅可比：
∂
e
∂
i
d
=
∂
e
∂
P
t
∗
∂
P
t
∂
P
n
o
r
m
l
i
z
e
∗
∂
P
n
o
r
m
l
i
z
e
∂
i
d
=
∂
e
∂
P
t
∗
T
t
h
∗
[
0
0
0
1
]
=
∂
e
∂
P
t
∗
T
t
h
c
o
l
3
\frac{\partial e}{\partial id} = \frac{\partial e}{\partial P_t} * \frac{\partial P_t}{\partial P_{normlize}} * \frac{\partial P_{normlize}}{\partial id} = \frac{\partial e}{\partial P_t} * T_{th} * \begin{bmatrix} 0\\ 0\\ 0\\ 1 \end{bmatrix} = \frac{\partial e}{\partial P_t} * T_{th}^{col 3}
∂id∂e=∂Pt∂e∗∂Pnormlize∂Pt∗∂id∂Pnormlize=∂Pt∂e∗Tth∗⎣⎢⎢⎡0001⎦⎥⎥⎤=∂Pt∂e∗Tthcol3