【basalt】（一）3D点参数化

basalt 使用2D-dir和inverse-depth对3D点进行参数化，相比VINS只优化逆深度的方式，这种参数化方式更加合理，host帧对点的方向观测确实应该优化一下

1、3D点参数化

XYZ形式的3D点参数化：
P=[XYZ1]P=\begin{bmatrix} X\\ Y\\ Z \\ 1 \end{bmatrix} P=⎣⎢⎢⎡XYZ1⎦⎥⎥⎤

表示成逆深度的形式：
P=[X/dY/dZ/d1/d],d=X2+Y2+Z2P=\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/d1+z/dy/d1+z/d]=[xd+zyd+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=x2+y2+z2d = \sqrt{x^2 + y^2 + z^2}d=x2+y2+z2.

代码里对3D点齐次坐标PhomogeneousP_{homogeneous}Phomogeneous（4×14 \times 14×1）做stereographic projection时，直接取了PhomogeneousP_{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点为Pnormalize=[x/d,y/d,z/d,1/d]TP_{normalize}=[x/d,y/d,z/d,1/d]^TPnormalize=[x/d,y/d,z/d,1/d]T，前三维是已经归一化后3D点坐标.

1.2 stereographic反投影

反投影得到
Psphere=π−1(p)=21+u2+v2[uv1]−[001]\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 的雅可比矩阵记为 Jup4×2J_{up}^{4 \times 2}Jup4×2，最后一行只与逆深度有关，补零即可.

2、重投影雅可比

由于参数化后的3D点由2D-dir和一个inverse-depth构成，所以先要恢复出3D点:
Pnormlize=[Psphere,1/d]TP_{normlize}=[P_{sphere},1/d]^T Pnormlize=[Psphere,1/d]T

其中，前三维直接由stereographic反投影得到，最后一维为逆深度

接着，根据相机模型构造如下重投影误差：
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]\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]

对TthT_{th}Tth求雅可比：
∂e∂ξth=∂e∂Pt∗∂Pt∂ξth\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

对p2D−dirp_{2D-dir}p2D−dir的雅可比：
∂e∂p2D−dir=∂e∂Pt∗∂Pt∂Pnormlize∗∂Pnormlize∂p2D−dir=∂e∂Pt∗Tth3×4∗Jup4×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

其中，∂Pt∂Pnormlize=Tth\frac{\partial P_t}{\partial P_{normlize}} = T_{th}∂Pnormlize∂Pt=Tth

对inverse-depth的雅可比：
∂e∂id=∂e∂Pt∗∂Pt∂Pnormlize∗∂Pnormlize∂id=∂e∂Pt∗Tth∗[0001]=∂e∂Pt∗Tthcol3\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

@leatherwang

二零二零年六月十五日