视觉残差函数及雅可比公式推导

【约定符号】：
特征点在相机坐标系下的坐标为 [ x , y , z ] T [x,y,z]^T [x,y,z]T；
特征点在归一化相机坐标系下的坐标为 [ μ , ν , 1 ] T [\mu,\nu,1]^T [μ,ν,1]T或 [ μ , ν ] T [\mu,\nu]^T [μ,ν]T
特征点的这两种坐标之间的关系：
[ x y z ] = 1 λ [ μ ν 1 ] \begin{bmatrix} x\\ y\\ z \end{bmatrix}= \frac{1}{\lambda} \begin{bmatrix} \mu\\ \nu\\ 1 \end{bmatrix} ⎣⎡xyz⎦⎤=λ1⎣⎡μν1⎦⎤
其中， λ = 1 / z \lambda=1/z λ=1/z，称为逆深度。

【定义概念】视觉重投影误差
假设预测的（估计的） 特征点的坐标为 [ x , y , z ] T [x,y,z]^T [x,y,z]T（相机坐标系），观测到的 特征点的坐标为 [ μ , ν ] T [\mu,\nu]^T [μ,ν]T（归一化相机坐标系），则视觉重投影误差定义为：
r c = [ x z − μ y z − ν ] r_c=\begin{bmatrix} \frac{x}{z}-\mu\\ \frac{y}{z}-\nu \end{bmatrix} rc=[zx−μzy−ν]
基于以上内容，开始推导。

已知第 i i i帧中某特征点的坐标 [ μ i , ν i ] T [\mu_i,\nu_i]^T [μi,νi]T（归一化相机坐标系）及逆深度 λ i \lambda_i λi，可以预测该特征点在第 j j j帧的相机坐标系下的坐标 [ x c j , y c j , z c j ] T [x_{c_j},y_{c_j},z_{c_j}]^T [xcj,ycj,zcj]T为：
(1-1) [ x c j y c j z c j 1 ] = T b c − 1 T w b j − 1 T w b i T b c [ 1 λ c i μ 1 λ c i ν 1 λ c i 1 ] \begin{bmatrix} x_{c_j}\\ y_{c_j}\\ z_{c_j}\\1 \end{bmatrix}= T^{-1}_{bc}T^{-1}_{wb_j} T_{wb_i}T_{bc} \begin{bmatrix} \frac{1}{\lambda_{c_i}}\mu\\ \frac{1}{\lambda_{c_i}}\nu\\ \frac{1}{\lambda_{c_i}} \\1 \end{bmatrix} \tag{1-1} ⎣⎢⎢⎡xcjycjzcj1⎦⎥⎥⎤=Tbc−1Twbj−1TwbiTbc⎣⎢⎢⎢⎡λci1μλci1νλci11⎦⎥⎥⎥⎤(1-1)
【注】关于 T w b i T_{wb_i} Twbi和 T w b j T_{wb_j} Twbj，此时我们有一个粗略的值。
同时，该特征点在第 j j j帧确实被观测到了，坐标为 [ μ c j , ν c j ] T [\mu_{c_j},\nu_{c_j}]^T [μcj,νcj]T，则不难构建重投影误差（抄过来）如下：
r c = [ x c j z c j − μ c j y c j z c j − ν c j ] ≜ [ r c 1 r c 2 ] r_c=\begin{bmatrix} \frac{x_{c_j}}{z_{c_j}}-\mu_{c_j}\\ \frac{y_{c_j}}{z_{c_j}}-\nu_{c_j} \end{bmatrix}\triangleq \begin{bmatrix} r_{c1}\\ r_{c2} \end{bmatrix} rc=⎣⎡zcjxcj−μcjzcjycj−νcj⎦⎤≜[rc1rc2]
这就是残差函数。
残差函数构成损失函数，在使用LM算法优化过程中，需要使用残差函数的Jacobian矩阵（一阶泰勒展开） ∂ r c ∂ s t a t e = ∂ r c ∂ f c j ⋅ ∂ f c j ∂ s t a t e \frac{\partial r_c}{\partial state}=\frac{\partial r_c}{\partial f_{c_j}}\cdot \frac{\partial f_{c_j}}{\partial state} ∂state∂rc=∂fcj∂rc⋅∂state∂fcj。【具体详见LM算法】

求残差函数的Jacobian矩阵
首先，明确 r c r_c rc需要对哪些变量求偏导。
共四大部分：1. i i i时刻的位移和姿态，2. j j j时刻的位移和姿态，3. imu和相机的外参，4. 逆深度。

应用链式法则， ∂ r c ∂ s t a t e = ∂ r c ∂ f c j ⋅ ∂ f c j ∂ s t a t e \frac{\partial r_c}{\partial state}=\frac{\partial r_c}{\partial f_{c_j}}\cdot \frac{\partial f_{c_j}}{\partial state} ∂state∂rc=∂fcj∂rc⋅∂state∂fcj

第一步，先求 ∂ r c ∂ f c j \frac{\partial r_c}{\partial f_{c_j}} ∂fcj∂rc得：
∂ r c ∂ f c j = [ ∂ r c 1 ∂ x c j ∂ r c 1 ∂ y c j ∂ r c 1 ∂ z c j ∂ r c 2 ∂ x c j ∂ r c 2 ∂ y c j ∂ r c 2 ∂ z c j ] = [ 1 z c j 0 − x c j z c j 2 0 1 z c j − y c j z c j 2 ] \begin{aligned} \frac{\partial r_c}{\partial f_{c_j}} &= \begin{bmatrix} \frac{\partial r_{c1}}{\partial x_{c_j}} & \frac{\partial r_{c1}}{\partial y_{c_j}} & \frac{\partial r_{c1}}{\partial z_{c_j}} \\ \frac{\partial r_{c2}}{\partial x_{c_j}} & \frac{\partial r_{c2}}{\partial y_{c_j}} & \frac{\partial r_{c2}}{\partial z_{c_j}} \end{bmatrix} \\ &= \begin{bmatrix} \frac{1}{z_{c_j}} & 0 & -\frac{x_{c_j}}{ z^2_{c_j}} \\ 0 & \frac{1}{z_{c_j}} & -\frac{y_{c_j}}{ z^2_{c_j}} \end{bmatrix} \\ \end{aligned} ∂fcj∂rc=[∂xcj∂rc1∂xcj∂rc2∂ycj∂rc1∂ycj∂rc2∂zcj∂rc1∂zcj∂rc2]=⎣⎡zcj100zcj1−zcj2xcj−zcj2ycj⎦⎤

第二步：求 ∂ f c j ∂ s t a t e \frac{\partial f_{c_j}}{\partial state} ∂state∂fcj。

在开始第二部分的求导之前，对 f c j f_{c_j} fcj做一些等价变形。
公式（1-1）的等价形式：公式（1-2） 将四维齐次形式改写，拆成三维形式，并做一些符号简化：
(1-2) f c j ≜ [ x c j y c j z c j ] = R b c T R w b j T R w b i R b c 1 λ c i [ μ c j ν c i 1 ] + R b c T ( R w b j T ( ( R w b i p b c + p w b i ) − p w b j ) − p b c ) \begin{aligned} f_{c_j} &\triangleq \begin{bmatrix} x_{c_j}\\ y_{c_j}\\ z_{c_j} \end{bmatrix} \\ & = R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc}\frac{1}{\lambda_{c_i}} \begin{bmatrix} \mu_{c_j}\\ \nu_{c_i}\\ 1 \end{bmatrix}\\ &+R^{T}_{bc}(R^{T}_{wb_j}(( R_{wb_i}p_{bc}+p_{wb_i})-p_{wb_j})-p_{bc}) \end{aligned} \tag{1-2} fcj≜⎣⎡xcjycjzcj⎦⎤=RbcTRwbjTRwbiRbcλci1⎣⎡μcjνci1⎦⎤+RbcT(RwbjT((Rwbipbc+pwbi)−pwbj)−pbc)(1-2)

f b i ≜ R b c f c i + p b c f w ≜ R w b i f b i + p w b i f b j ≜ R w b j T ( f w − p w b j ) f c j ≜ R b c T ( f b j − p b c ) \begin{aligned} f_{b_i} &\triangleq R_{bc}f_{c_i}+p_{bc}\\ f_{w} &\triangleq R_{wb_i}f_{b_i}+p_{wb_i}\\ f_{b_j} &\triangleq R^T_{wb_j}(f_{w}-p_{wb_j})\\ f_{c_j} &\triangleq R^T_{bc}(f_{b_j}-p_{bc}) \end{aligned} fbifwfbjfcj≜Rbcfci+pbc≜Rwbifbi+pwbi≜RwbjT(fw−pwbj)≜RbcT(fbj−pbc)
不难看出，上面四个式子依次给出了特征点在 c i , b i , w , b j , c j c_i,b_i,w,b_j,c_j ci,bi,w,bj,cj坐标系下的坐标。将四个式子依次从上到下代入，展开即可得到公式（1-2）的结果。

问： p w c j p_{wc_j} pwcj与 f c j f_{c_j} fcj含义相同吗？
答：不相同， f c j f_{c_j} fcj表示特征点在 c j c_j cj相机坐标系下的坐标；
p w c j p_{wc_j} pwcj表示相机 c j c_j cj在世界坐标系下的坐标！

已知公式（1-2）：
f c j = R b c T R w b j T R w b i R b c f c i + R b c T ( R w b j T ( ( R w b i p b c + p w b i ) − p w b j ) − p b c ) \begin{aligned} f_{c_j} & = R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc} f_{c_i} \\ &+R^{T}_{bc}(R^{T}_{wb_j}(( R_{wb_i}p_{bc}+p_{wb_i})-p_{wb_j})-p_{bc}) \end{aligned} fcj=RbcTRwbjTRwbiRbcfci+RbcT(RwbjT((Rwbipbc+pwbi)−pwbj)−pbc)
1.1 i i i时刻的位移：
即 p w b i : = p w b i + δ p b i b i ′ p_{wb_i}:=p_{wb_i}+\delta p_{b_ib'_i} pwbi:=pwbi+δpbibi′，不难写出：
∂ f c j ∂ δ p b i b i ′ = R b c T R w b j T \frac{\partial f_{c_j}}{\partial \delta p_{b_ib'_i}}=R^{T}_{bc}R^{T}_{wb_j} ∂δpbibi′∂fcj=RbcTRwbjT

1.2 i i i时刻的姿态：
即 R w b i : = R w b i ( I + [ δ θ b i b i ′ ] × ) R_{wb_i}:=R_{wb_i}(I+[\delta \theta_{b_ib'_i}]_\times) Rwbi:=Rwbi(I+[δθbibi′]×)
f c j f_{c_j} fcj中与 R w b i R_{wb_i} Rwbi有关的项有两部分，可合成简化为：
f c j = R b c T R w b j T R w b i R b c f c i + R b c T R w b j T R w b i p b c + ( . . . ) = R b c T R w b j T R w b i f b i + ( . . . ) \begin{aligned} f_{c_j} & = R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc} f_{c_i} +R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}p_{bc}+(...)\\ &= R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}f_{b_i} +(...) \end{aligned} fcj=RbcTRwbjTRwbiRbcfci+RbcTRwbjTRwbipbc+(...)=RbcTRwbjTRwbifbi+(...)
则：
∂ f c j ∂ δ θ b i b i ′ = R b c T R w b j T R w b i ( I + [ δ θ b i b i ′ ] × ) f b i δ θ b i b i ′ = − R b c T R w b j T R w b i [ f b i ] × \begin{aligned} \frac{\partial f_{c_j}}{\partial \delta \theta_{b_ib'_i}} &=\frac{R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}(I+[\delta \theta_{b_ib'_i}]_\times)f_{b_i} }{\delta \theta_{b_ib'_i}} \\ &=-R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}[f_{b_i}]_\times \end{aligned} ∂δθbibi′∂fcj=δθbibi′RbcTRwbjTRwbi(I+[δθbibi′]×)fbi=−RbcTRwbjTRwbi[fbi]×
【注】这里有一个写法上的简化。

2.1 j j j时刻的位移：
即 p w b j : = p w b j + δ p b j b j ′ p_{wb_j}:=p_{wb_j}+\delta p_{b_jb'_j} pwbj:=pwbj+δpbjbj′，不难写出：
∂ f c j ∂ δ p b j b j ′ = − R b c T R w b j T \frac{\partial f_{c_j}}{\partial \delta p_{b_jb'_j}}=-R^{T}_{bc}R^{T}_{wb_j} ∂δpbjbj′∂fcj=−RbcTRwbjT

2.2 j j j时刻的姿态：
即 R w b j : = R w b j ( I + [ δ θ b j b j ′ ] × ) R_{wb_j}:=R_{wb_j}(I+[\delta \theta_{b_jb'_j}]_\times) Rwbj:=Rwbj(I+[δθbjbj′]×)
f c j f_{c_j} fcj中与 R w b j R_{wb_j} Rwbj有关的项有两部分，可合成简化为：
f c j = R b c T R w b j T R w b i R b c f c i + R b c T ( R w b j T ( ( R w b i p b c + p w b i ) − p w b j ) − p b c ) = R b c T R w b j T ( R w b i ( R b c f c i + p b c ) + p w b i − p w b j ) + ( . . . ) = R b c T R w b j T ( f w − p w b j ) + ( . . . ) \begin{aligned} f_{c_j} & = R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc} f_{c_i} \\ &+R^{T}_{bc}(R^{T}_{wb_j}(( R_{wb_i}p_{bc}+p_{wb_i})-p_{wb_j})-p_{bc}) \\ &=R^{T}_{bc}R^{T}_{wb_j}(R_{wb_i}(R_{bc} f_{c_i}+p_{bc})+p_{wb_i}-p_{wb_j})+(...) \\ &=R^{T}_{bc}R^{T}_{wb_j}(f_w-p_{wb_j})+(...) \end{aligned} fcj=RbcTRwbjTRwbiRbcfci+RbcT(RwbjT((Rwbipbc+pwbi)−pwbj)−pbc)=RbcTRwbjT(Rwbi(Rbcfci+pbc)+pwbi−pwbj)+(...)=RbcTRwbjT(fw−pwbj)+(...)
则：
∂ f c j ∂ δ θ b j b j ′ = R b c T [ R w b j ( I + [ δ θ b j b j ′ ] × ) ] T ( f w − p w b j ) δ θ b j b j ′ = R b c T ( I − [ δ θ b j b j ′ ] × ) R w b j T ( f w − p w b j ) δ θ b j b j ′ = R b c T ( I − [ δ θ b j b j ′ ] × ) f b j δ θ b j b j ′ = R b c T [ f b j ] × \begin{aligned} \frac{\partial f_{c_j}}{\partial \delta \theta_{b_jb'_j}} &=\frac{ R^{T}_{bc}[R_{wb_j}(I+[\delta \theta_{b_jb'_j}]_\times)]^T(f_w-p_{wb_j}) }{\delta \theta_{b_jb'_j}} \\ &=\frac{ R^{T}_{bc}(I-[\delta \theta_{b_jb'_j}]_\times)R_{wb_j}^T(f_w-p_{wb_j}) }{\delta \theta_{b_jb'_j}} \\ &=\frac{ R^{T}_{bc}(I-[\delta \theta_{b_jb'_j}]_\times)f_{b_j} }{\delta \theta_{b_jb'_j}} \\ &=R^{T}_{bc}[f_{b_j}]_\times \end{aligned} ∂δθbjbj′∂fcj=δθbjbj′RbcT[Rwbj(I+[δθbjbj′]×)]T(fw−pwbj)=δθbjbj′RbcT(I−[δθbjbj′]×)RwbjT(fw−pwbj)=δθbjbj′RbcT(I−[δθbjbj′]×)fbj=RbcT[fbj]×

3.1 imu和相机之间外参中的位移：
即 p b c : = p b c + δ p c c ′ p_{bc}:=p_{bc}+\delta p_{cc'} pbc:=pbc+δpcc′，不难写出：
∂ f c j ∂ δ p c c ′ = R b c T ( R w b j T R w b j T − I 3 × 3 ) \frac{\partial f_{c_j}}{\partial \delta p_{cc'} } =R^{T}_{bc} (R^{T}_{wb_j} R^{T}_{wb_j}-I_{3\times 3}) ∂δpcc′∂fcj=RbcT(RwbjTRwbjT−I3×3)
3.2 imu和相机之间外参中的姿态：
即 R b c : = R b c ( I + [ δ θ c c ′ ] × ) R_{bc}:=R_{bc}(I+[\delta \theta_{cc'}]_\times) Rbc:=Rbc(I+[δθcc′]×)
f c j f_{c_j} fcj中与 R c c ′ R_{cc'} Rcc′有关的项有两部分，且不容易简化，故分为两部分求解：
第一部分：
f c j [ 1 ] ≜ R b c T R w b j T R w b i R b c f c i f^{[1]}_{c_j} \triangleq R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc} f_{c_i} fcj[1]≜RbcTRwbjTRwbiRbcfci
则：
∂ f c j [ 1 ] ∂ δ θ c c ′ = ( I − [ δ θ c c ′ ] × ) R b c T R w b j T R w b i R b c ( I + [ δ θ c c ′ ] × ) f c i δ θ c c ′ ≈ − [ δ θ c c ′ ] × R b c T R w b j T R w b i R b c f c i + R b c T R w b j T R w b i R b c [ δ θ c c ′ ] × f c i δ θ c c ′ = [ R b c T R w b j T R w b i R b c f c i ] × − R b c T R w b j T R w b i R b c [ f c i ] × \begin{aligned} \frac{\partial f^{[1]}_{c_j}}{\partial \delta \theta_{cc'}} &=\frac{ (I-[\delta \theta_{cc'}]_\times)R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc}(I+[\delta \theta_{cc'}]_\times) f_{c_i} }{\delta \theta_{cc'}} \\ &\approx \frac{ -[\delta \theta_{cc'}]_\times R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc} f_{c_i} + R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc} [\delta \theta_{cc'}]_\times f_{c_i}}{\delta \theta_{cc'}} \\ &=[R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc} f_{c_i}]_{\times}-R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc} [f_{c_i}]_{\times} \end{aligned} ∂δθcc′∂fcj[1]=δθcc′(I−[δθcc′]×)RbcTRwbjTRwbiRbc(I+[δθcc′]×)fci≈δθcc′−[δθcc′]×RbcTRwbjTRwbiRbcfci+RbcTRwbjTRwbiRbc[δθcc′]×fci=[RbcTRwbjTRwbiRbcfci]×−RbcTRwbjTRwbiRbc[fci]×
第二部分：
f c j [ 2 ] = R b c T ( R w b j T ( ( R w b i p b c + p w b i ) − p w b j ) − p b c ) f^{[2]}_{c_j} = R^{T}_{bc}(R^{T}_{wb_j}(( R_{wb_i}p_{bc}+p_{wb_i})-p_{wb_j})-p_{bc}) fcj[2]=RbcT(RwbjT((Rwbipbc+pwbi)−pwbj)−pbc)
则：
∂ f c j [ 2 ] ∂ δ θ c c ′ = ( I − [ δ θ c c ′ ] × ) R b c T ( R w b j T ( ( R w b i p b c + p w b i ) − p w b j ) − p b c ) δ θ c c ′ = [ R b c T ( R w b j T ( ( R w b i p b c + p w b i ) − p w b j ) − p b c ) ] × \begin{aligned} \frac{\partial f^{[2]}_{c_j}}{\partial \delta \theta_{cc'}} &=\frac{ (I-[\delta \theta_{cc'}]_\times)R^{T}_{bc}(R^{T}_{wb_j}(( R_{wb_i}p_{bc}+p_{wb_i})-p_{wb_j})-p_{bc})}{\delta \theta_{cc'}} \\ & = [R^{T}_{bc}(R^{T}_{wb_j}(( R_{wb_i}p_{bc}+p_{wb_i})-p_{wb_j})-p_{bc})]_{\times} \end{aligned} ∂δθcc′∂fcj[2]=δθcc′(I−[δθcc′]×)RbcT(RwbjT((Rwbipbc+pwbi)−pwbj)−pbc)=[RbcT(RwbjT((Rwbipbc+pwbi)−pwbj)−pbc)]×
两部分相加，即 ∂ f c j ∂ δ θ c c ′ = ∂ f c j [ 1 ] ∂ δ θ c c ′ + ∂ f c j [ 2 ] ∂ δ θ c c ′ \frac{\partial f_{c_j}}{\partial \delta \theta_{cc'}}=\frac{\partial f^{[1]}_{c_j}}{\partial \delta \theta_{cc'}}+\frac{\partial f^{[2]}_{c_j}}{\partial \delta \theta_{cc'}} ∂δθcc′∂fcj=∂δθcc′∂fcj[1]+∂δθcc′∂fcj[2]

4.逆深度：
即 λ c i : = λ c i + δ λ c i \lambda_{c_i}:=\lambda_{c_i}+\delta \lambda_{c_i} λci:=λci+δλci， f c j f_{c_j} fcj中仅 f c i f_{c_i} fci与 λ c i \lambda_{c_i} λci有关，链式法则 ∂ f c j ∂ δ λ c i = ∂ f c j ∂ δ f c i ⋅ ∂ f c i ∂ δ λ c i \frac{\partial f_{c_j}}{\partial \delta \lambda_{c_i}}=\frac{\partial f_{c_j}}{\partial \delta f_{c_i}}\cdot \frac{\partial f_{c_i}}{\partial \delta \lambda_{c_i}} ∂δλci∂fcj=∂δfci∂fcj⋅∂δλci∂fci：
其中，
f c j = R b c T R w b j T R w b i R b c f c i f_{c_j} = R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc} f_{c_i} fcj=RbcTRwbjTRwbiRbcfci
则：
∂ f c j ∂ δ f c i = R b c T R w b j T R w b i R b c \frac{\partial f_{c_j}}{\partial \delta f_{c_i}} =R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc} ∂δfci∂fcj=RbcTRwbjTRwbiRbc
又有，
f c i = 1 λ c i [ μ c j ν c i 1 ] f_{c_i}=\frac{1}{\lambda_{c_i}} \begin{bmatrix} \mu_{c_j}\\ \nu_{c_i}\\ 1 \end{bmatrix}\\ fci=λci1⎣⎡μcjνci1⎦⎤
则：
∂ f c i ∂ δ λ c i = − 1 λ c i 2 [ μ c j ν c i 1 ] = − 1 λ c i f c i \frac{\partial f_{c_i}}{\partial \delta \lambda_{c_i}} =-\frac{1}{\lambda^2_{c_i}} \begin{bmatrix} \mu_{c_j}\\ \nu_{c_i}\\ 1 \end{bmatrix}= -\frac{1}{\lambda_{c_i}} f_{c_i} ∂δλci∂fci=−λci21⎣⎡μcjνci1⎦⎤=−λci1fci
至此，推导完成！

视觉残差函数及雅可比公式推导相关推荐

IMU残差函数及雅可比公式推导（二）
根据IMU残差函数及雅可比公式推导(一)已知: αbibk+1=αbibk+βbibkδt+12aδt2qbibk+1=qbibk⊗[112ωδt]βbibk+1=βbibk+aδt\begin{al ...
【机器学习】【逻辑回归】Logistic函数/Sigmoid函数的详细公式推导
sigmoid函数的数学公式 sigmoid函数的因变量x取值范围是-∞到+∞,(-∞,+∞),但是sigmoid函数的值域是(0, 1). 不管x取什么值其对应的sigmoid函数值一定会落到(0, ...
VIO残差函数的构建以及IMU预积分和协方差传递
基于滑动窗口的 VIO Bundle Adjustment,为了节约计算量采用滑动窗口形式的 Bundle Adjustment,在 i 时刻, 滑动窗口内待优化的系统状态量定义如下: χ=[Xn,X ...
python求向量函数的雅可比矩阵_python – scipy中最小二乘函数的雅可比行列式的方法签名...
这是我使用的指数衰减拟合: import numpy as np from scipy.optimize import leastsq def f(var,xs): return var[0]*np. ...
在 VSLAM 的后端优化中的重投影误差的雅可比计算详细推导
对于相机位姿的变换可以通过旋转矩阵或者四元数进行表示,对于旋转矩阵的定义满足: R{∣R∣=1RRT=IR \begin{cases} |R| = 1 \\ RR^T = I\\ \end{cases ...
多传感器融合定位四-3D激光里程计其四：点云线面特征提取
多传感器融合定位四-3D激光里程计其四:点云线面特征提取 1. 点云线面特征提取 1.1 按线数分割 1.2 计算曲率(重要!) 1.3 按曲率大小筛选特征点 2. 基于线面特征的位姿变化 2.1 帧 ...
VINS-mon代码解析——启动文件lauch文件与参数配置文件yaml介绍
文章目录前言启动文件launch 参数配置文件yaml 1.通用参数 2.相机的基础信息 3.imu和相机之间的外参 4.在节点/feature_traker中需要用到的参数 5.在节点/vins ...
VINS-Mono之后端非线性优化 (目标函数中视觉残差和IMU残差，及其对状态量的雅克比矩阵、协方差递推方程的推导)
文章目录 1. 前言 2. 非线性最小二乘 2.1 Guass-Newton 和 Levenberg-Marquardt 2.2 鲁棒核函数下状态量增量方程的构建 3. 局部Bundle Adjust ...
ceres优化参数的传递、残差构建时状态量的传递、evaluate函数的使用
误差函数中的参数包括: 已知参数和待优化参数两部分, 1. 添加待优化参数 ceres::LocalParameterization *local_parameterization = new Pos ...

视觉残差函数及雅可比公式推导

视觉残差函数及雅可比公式推导相关推荐

最新文章

热门文章