前言

指数形式的线性化：

若存在一个无穷小量x0x_0x0,则对指数ex0e^{x_0}ex0泰勒展开：
ex0=1+x0+x022!+...+x0nn!e^{x_0}=1+x_0+ \frac {x_0^2} {2!}+...+\frac {x_0^n} {n!} ex0=1+x0+2!x02+...+n!x0n
我们取一阶的泰勒展开：ex0≈1+x0e^{x_0}\approx 1+x_0ex0≈1+x0
所以对于无穷小量δϕ\delta \phiδϕ,δξ\delta \xiδξ有：
exp⁡(δϕ)⋅C≈(1+δϕ)⋅Cexp⁡(δξ)⋅C≈(1+δξ)⋅C\exp (\delta \phi)\cdot \bm C\approx (1+\delta \phi)\cdot \bm C\\ \exp (\delta \xi)\cdot \bm C\approx (1+\delta \xi)\cdot \bm C exp(δϕ)⋅C≈(1+δϕ)⋅Cexp(δξ)⋅C≈(1+δξ)⋅C
上述就是旋转线性化和位姿线性化

一、旋转矩阵SO(3)群

考虑SO(3)旋转群，设关于θ\thetaθ的函数：
c(θ)=R(θ)⋅p\small \bm c(\theta) = \bm R(\theta)\cdot p c(θ)=R(θ)⋅p
对于SO(3),其BCH近似为：

左乘： exp⁡(Δϕ∧)⋅exp⁡(ϕ∧)=exp⁡((ϕ+Jl−1(ϕ)Δϕ)∧)\exp(\Delta \phi ^ \land)\cdot \exp(\phi ^ \land) =\exp((\phi+\bm J_l^{-1}(\phi)\Delta \phi)^\land)exp(Δϕ∧)⋅exp(ϕ∧)=exp((ϕ+Jl−1(ϕ)Δϕ)∧)
右乘： exp⁡(ϕ∧)⋅exp⁡(Δϕ∧)=exp⁡((ϕ+Jr−1(ϕ)Δϕ)∧)\exp(\phi ^ \land) \cdot \exp(\Delta \phi ^ \land)=\exp((\phi+\bm J_r^{-1}(\phi)\Delta \phi)^\land)exp(ϕ∧)⋅exp(Δϕ∧)=exp((ϕ+Jr−1(ϕ)Δϕ)∧)

根据BCH 近似以及旋转线性化：
c(θ+Δθ)=exp⁡((Jl⋅Δθ)∧)⋅exp⁡(θ∧)=(1+(JlΔθ)∧)R(θ)⋅p=c(θ)+(−R(θ)⋅p)∧Jl⋅Δθ(1)\small \begin{aligned} \bm c(\theta+\Delta\theta)&=\exp((\bm J_l\cdot \Delta \theta)^\land)\cdot \exp(\theta^\land)\\~\\ &= (1+(\bm J_l\Delta\theta)^\land)\bm R(\theta)\cdot p\\ ~\\ &=\bm c(\theta) + (-\bm R(\theta)\cdot p)^\land \bm J_l\cdot \Delta \theta \end{aligned}\tag{1} c(θ+Δθ) =exp((Jl⋅Δθ)∧)⋅exp(θ∧)=(1+(JlΔθ)∧)R(θ)⋅p=c(θ)+(−R(θ)⋅p)∧Jl⋅Δθ(1)
根据泰勒展开式：
c(θ+Δθ)=c(θ)+∂R(θ)⋅p∂θ⋅Δθ(2)\small \bm c(\theta+\Delta\theta) = \bm c(\theta)+\frac {\partial{\bm R(\theta)\cdot p}} {\partial \theta} \cdot \Delta \theta\tag{2} c(θ+Δθ)=c(θ)+∂θ∂R(θ)⋅p⋅Δθ(2)
对比（1）（2）两式可以得到：
∂R(θ)⋅p∂θ=(−R(θ)⋅p)∧Jl\small \frac {\partial{\bm R(\theta)\cdot p}} {\partial \theta} =(-\bm R(\theta)\cdot p)^\land \bm J_l ∂θ∂R(θ)⋅p=(−R(θ)⋅p)∧Jl
这样就求出了c(θ)c(\theta)c(θ)对θ\thetaθ的导数，但其中包含了一个Jl(ϕ)\bm J_l(\phi)Jl(ϕ)，加大了计算的复杂度；

我们重新定义增量方程，利用⊕\oplus⊕来定义，并规定其具有运算的顺序性，所以下面的方程为左乘扰动：
c(Δθ⊕θ)=exp⁡(Δθ∧)⋅exp⁡(θ∧)p=exp⁡((θ+Jl−1(θ)Δθ)∧)p=(1+Δθ∧)exp⁡(θ∧)p=R(θ)p+[−(R(θ)p)]∧Δθ(3)\small \begin{aligned} \bm c(\Delta \theta \oplus \theta)&=\exp(\Delta \theta^\land)\cdot \exp(\theta^\land)p =\exp((\theta+\bm J_l^{-1}(\theta)\Delta \theta)^\land)p\\~\\ &=(1+\Delta\theta^\land)\exp(\theta^\land)p\\~\\ &=\bm R(\theta)p+[-(\bm R(\theta)p)]^\land\Delta\theta \end{aligned}\tag{3}c(Δθ⊕θ) =exp(Δθ∧)⋅exp(θ∧)p=exp((θ+Jl−1(θ)Δθ)∧)p=(1+Δθ∧)exp(θ∧)p=R(θ)p+[−(R(θ)p)]∧Δθ(3)
进行泰勒展开得：
c(Δθ⊕θ)=exp⁡((θ+Jl−1(θ)Δθ)∧)p=c(Jl−1(θ)Δθ+θ)=c(θ)+∂R(θ)⋅p∂θ⋅Jl−1(θ)Δθ(4)\small \begin{aligned} \bm c(\Delta \theta \oplus \theta) &=\exp((\theta+\bm J_l^{-1}(\theta)\Delta \theta)^\land)p=c(\bm J_l^{-1}(\theta)\Delta \theta+\theta)\\~\\ & =\bm c(\theta)+\frac {\partial{\bm R(\theta)\cdot p}} {\partial \theta} \cdot \bm J_l^{-1}(\theta)\Delta \theta \tag{4} \end{aligned}c(Δθ⊕θ) =exp((θ+Jl−1(θ)Δθ)∧)p=c(Jl−1(θ)Δθ+θ)=c(θ)+∂θ∂R(θ)⋅p⋅Jl−1(θ)Δθ(4)
结合公式（3）和（4），可以得到：
[−(R(θ)p)]∧=∂R(θ)⋅p∂θ⋅Jl−1(θ)\small [-(\bm R(\theta)p)]^\land= \frac {\partial{\bm R(\theta)\cdot p}} {\partial \theta} \cdot \bm J_l^{-1}(\theta) [−(R(θ)p)]∧=∂θ∂R(θ)⋅p⋅Jl−1(θ)
右边按照定义：
∂R(θ)⋅p∂θ⋅Jl−1(θ)=lim⁡Δθ→0R(Δθ+θ)p−R(θ)pΔθ⋅Jl−1(θ)=lim⁡Δθ→0(1+(Jl(θ)Δθ)∧)exp⁡(θ∧)p−exp⁡(θ∧)pΔθ⋅Jl−1(θ)=lim⁡Δθ→0(Jl(θ)Δθ)∧⋅exp⁡(θ∧)pΔθ⋅Jl−1(θ)=lim⁡Δθ→0−(exp⁡(θ∧)p)∧JlΔθJlΔθ=−(exp⁡(θ∧)p)∧(5)\small \begin{aligned} \frac {\partial{\bm R(\theta)\cdot p}} {\partial \theta} \cdot \bm J_l^{-1}(\theta) &= \lim_{\Delta \theta \to 0} \frac {R(\Delta \theta+\theta) p-R(\theta)p} {\Delta \theta}\cdot \bm J_l^{-1}(\theta)\\ ~\\ &= \lim_{\Delta \theta \to 0} \frac {(1+(\bm J_l(\theta)\Delta \theta)^\land)\exp(\theta^\land)p-\exp(\theta^\land)p} {\Delta \theta } \cdot \bm J_l^{-1}(\theta)\\ ~\\ &= \lim_{\Delta \theta \to0} \frac {(\bm J_l(\theta)\Delta \theta)^\land\cdot \exp(\theta^\land)p} {\Delta \theta} \cdot \bm J_l^{-1}(\theta) \\ ~\\ &= \lim_{\Delta \theta \to0} \frac {-(\exp(\theta^\land)p)^\land \bm J_l \Delta \theta} {\bm J_l \Delta \theta} \\ ~\\ &= {{-(\exp(\theta^\land)p)^\land } } \tag{5} \end{aligned}∂θ∂R(θ)⋅p⋅Jl−1(θ) =Δθ→0limΔθR(Δθ+θ)p−R(θ)p⋅Jl−1(θ)=Δθ→0limΔθ(1+(Jl(θ)Δθ)∧)exp(θ∧)p−exp(θ∧)p⋅Jl−1(θ)=Δθ→0limΔθ(Jl(θ)Δθ)∧⋅exp(θ∧)p⋅Jl−1(θ)=Δθ→0limJlΔθ−(exp(θ∧)p)∧JlΔθ=−(exp(θ∧)p)∧(5)
到这里就是我们定义的左扰动模型求导，省去了复杂的Jl\bm J_lJl:
∂c(Δθ⊕θ)∂δθ=∂R(θ)⋅p∂θ⋅Jl−1(θ)=−(exp⁡(θ∧)p)∧\frac {\partial{\bm c(\Delta \theta \oplus \theta) }} {\partial \delta \theta} = \frac {\partial{\bm R(\theta)\cdot p}} {\partial \theta} \cdot \bm J_l^{-1}(\theta) ={{-(\exp(\theta^\land)p)^\land } } ∂δθ∂c(Δθ⊕θ)=∂θ∂R(θ)⋅p⋅Jl−1(θ)=−(exp(θ∧)p)∧
这就是我们线性化旋转矩阵的雅克比矩阵:Jδθ=∂c(Δθ⊕θ)∂δθ=−(Rp)∧\bm J_{\delta \theta}=\frac {\partial{\bm c(\Delta \theta \oplus \theta) }} {\partial \delta \theta}=-(\bm Rp)^\landJδθ=∂δθ∂c(Δθ⊕θ)=−(Rp)∧

下面对位姿进行线性化求解

二、位姿矩阵SE(3)群

考虑SE(3)旋转群，设关于ξ\xiξ的函数：
H(ξ)=T(ξ)⋅v,(v为列向量,T∈SE(3))\small \bm H(\xi) = \bm T(\xi)\cdot v,{(v为列向量,T \in SE(3))} H(ξ)=T(ξ)⋅v,(v为列向量,T∈SE(3))
ξ\xiξ是一个6维的向量，由于只有3维向量有反对称矩阵，所以我们定义这样的
ξ∧=[ϕ∧ρ0T0]4×4,ρ∈R3,ϕ∈so(3)\xi^\land= \begin{bmatrix} \bm \phi^\land & \bm \rho\\ \\ \bm 0^T &\bm 0 \end{bmatrix}_{4 \times 4},\rho \in {\mathbb{R}^3},\phi \in so(3) ξ∧=⎣⎡ϕ∧0Tρ0⎦⎤4×4,ρ∈R3,ϕ∈so(3)

同样是BCH公式：
左乘： exp⁡(Δξ∧)⋅exp⁡(ξ∧)=exp⁡((ξ+Jl−1(ξ)Δξ)∧)\exp(\Delta \xi ^ \land)\cdot \exp(\xi ^ \land) =\exp((\xi+\bm J_l^{-1}(\xi)\Delta \xi)^\land)exp(Δξ∧)⋅exp(ξ∧)=exp((ξ+Jl−1(ξ)Δξ)∧)
右乘： exp⁡(ξ∧)⋅exp⁡(Δξ∧)=exp⁡((ξ+Jr−1(ξ)Δξ)∧)\exp(\xi ^ \land) \cdot \exp(\Delta \xi ^ \land)=\exp((\xi+\bm J_r^{-1}(\xi)\Delta \xi)^\land)exp(ξ∧)⋅exp(Δξ∧)=exp((ξ+Jr−1(ξ)Δξ)∧)

这里的JlJ_lJl和JrJ_rJr都是6×66\times 66×6的矩阵，这里不再展开；

根据对旋转矩阵的线性化分析，我们继续利用⊕\oplus⊕来定义，所以下面的方程为左乘扰动：
H(Δξ⊕ξ)=exp⁡(Δξ∧)⋅exp⁡(ξ∧)v=exp⁡((ξ+Jl−1(ξ)Δξ)∧)v=(1+Δξ∧)exp⁡(ξ∧)v=T(ξ)v+[−(T(ξ)v)]4×6∧Δξ6×1=H(ξ)+[−(T(ξ)v)]4×6∧Δξ6×1(6)\small \begin{aligned} \bm H(\Delta \xi \oplus \xi)&=\exp(\Delta \xi^\land)\cdot \exp(\xi^\land)v =\exp((\xi+\bm J_l^{-1}(\xi)\Delta \xi)^\land)v\\~\\ &=(1+\Delta\xi^\land)\exp(\xi^\land)v\\~\\ &=\bm T(\xi)v+[-(\bm T(\xi)v)]_{4\times 6}^\land\Delta\xi_{6\times1}\\~\\ &=\bm H(\xi)+[-(\bm T(\xi)v)]_{4\times 6}^\land\Delta\xi_{6\times1} \end{aligned} \tag{6}H(Δξ⊕ξ)   =exp(Δξ∧)⋅exp(ξ∧)v=exp((ξ+Jl−1(ξ)Δξ)∧)v=(1+Δξ∧)exp(ξ∧)v=T(ξ)v+[−(T(ξ)v)]4×6∧Δξ6×1=H(ξ)+[−(T(ξ)v)]4×6∧Δξ6×1(6)
进行泰勒展开得：
H(Δξ⊕ξ)=exp⁡((ξ+Jl−1(ξ)Δξ)∧)p=H(Jl−1(ξ)Δξ+ξ)=H(ξ)+∂T(ξ)⋅v∂ξ⋅Jl−1(ξ)Δξ6×1(7)\small \begin{aligned} \bm H(\Delta \xi \oplus \xi) &=\exp((\xi+\bm J_l^{-1}(\xi)\Delta \xi)^\land)p=H(\bm J_l^{-1}(\xi)\Delta \xi+\xi)\\~\\ & =\bm H(\xi)+\frac {\partial{T(\xi)\cdot v}} {\partial \xi} \cdot \bm J_l^{-1}(\xi)\Delta \xi_{6\times1} \tag{7} \end{aligned}H(Δξ⊕ξ) =exp((ξ+Jl−1(ξ)Δξ)∧)p=H(Jl−1(ξ)Δξ+ξ)=H(ξ)+∂ξ∂T(ξ)⋅v⋅Jl−1(ξ)Δξ6×1(7)
结合公式（6）和（7），可以得到：
[−(T(ξ)v)]∧=（∂T(ξ)⋅v∂ξ）4×6⋅Jl−1(ξ)\small [-(\bm T(\xi)v)]^\land= （\frac {\partial{\bm T(\xi)\cdot v}} {\partial \xi}）_{4 \times 6} \cdot \bm J_l^{-1}(\xi) [−(T(ξ)v)]∧=（∂ξ∂T(ξ)⋅v）4×6⋅Jl−1(ξ)
右边按照定义：
∂T(ξ)⋅v∂ξ⋅Jl−1(ξ)=lim⁡Δξ→0T(Δξ+ξ)v−T(ξ)vΔξ⋅Jl−1(ξ)=lim⁡Δξ→0(1+(Jl(ξ)Δξ)∧)exp⁡(ξ∧)v−exp⁡(ξ∧)vΔξ⋅Jl−1(ξ)=lim⁡Δξ→0(Jl(ξ)Δξ)∧⋅exp⁡(ξ∧)vΔξ⋅Jl−1(ξ)=lim⁡Δξ→0(Jl(ξ)Δξ)∧⋅exp⁡(ξ∧)vJlΔξ=[I3×3−(T(ξ)v)1:3∧0T0T]4×6(8)\small \begin{aligned} \frac {\partial{\bm T(\xi)\cdot v}} {\partial \xi} \cdot \bm J_l^{-1}(\xi) &= \lim_{\Delta \xi \to 0} \frac {\bm T(\Delta \xi+\xi) v-T(\xi)v} {\Delta \xi}\cdot \bm J_l^{-1}(\xi)\\ ~\\&= \lim_{\Delta \xi \to 0} \frac {(1+(\bm J_l(\xi)\Delta \xi)^\land)\exp(\xi^\land)v-\exp(\xi^\land)v} {\Delta \xi } \cdot \bm J_l^{-1}(\xi)\\ ~\\&= \lim_{\Delta \xi \to0} \frac {(\bm J_l(\xi)\Delta \xi)^\land\cdot \exp(\xi^\land)v} {\Delta \xi} \cdot \bm J_l^{-1}(\xi) \\ ~\\&= \lim_{\Delta \xi\to0} \frac {(\bm J_l(\xi)\Delta \xi)^\land\cdot \exp(\xi^\land)v} {\bm J_l \Delta \xi} \\~\\ \\ &= {\begin{bmatrix}\bm I_{3\times3} &-(\bm T(\xi)v)_{1 :3}^\land \\ \\\bm 0^T& \bm 0^T \\ \end{bmatrix} }_{4\times6}\tag{8} \end{aligned}∂ξ∂T(ξ)⋅v⋅Jl−1(ξ)    =Δξ→0limΔξT(Δξ+ξ)v−T(ξ)v⋅Jl−1(ξ)=Δξ→0limΔξ(1+(Jl(ξ)Δξ)∧)exp(ξ∧)v−exp(ξ∧)v⋅Jl−1(ξ)=Δξ→0limΔξ(Jl(ξ)Δξ)∧⋅exp(ξ∧)v⋅Jl−1(ξ)=Δξ→0limJlΔξ(Jl(ξ)Δξ)∧⋅exp(ξ∧)v=[I3×30T−(T(ξ)v)1:3∧0T]4×6(8)
到这里我们定义的左扰动模型求导，省去了复杂的Jl\bm J_lJl:
∂H(Δξ⊕ξ)∂δξ=∂T(ξ)⋅v∂ξ⋅Jl−1(ξ)=[I3×3−(T(ξ)v)1:3∧0T0T]4×6\small \frac {\partial{\bm H(\Delta \xi \oplus \xi) }} {\partial \delta \xi} = \frac {\partial{\bm T(\xi)\cdot v}} {\partial \xi} \cdot \bm J_l^{-1}(\xi) \\~\\ \\~\\ ={\begin{bmatrix}\bm I_{3\times3} &-(\bm T(\xi)v)_{1 :3}^\land \\ \\\bm 0^T& \bm 0^T \\ \end{bmatrix} }_{4\times6} ∂δξ∂H(Δξ⊕ξ)=∂ξ∂T(ξ)⋅v⋅Jl−1(ξ)  =[I3×30T−(T(ξ)v)1:3∧0T]4×6
这就是我们线性化 位姿矩阵 的雅克比矩阵:Jδξ=∂H(Δξ⊕ξ)∂δξ=(Tv)⊙\bm J_{\delta \xi}=\frac {\partial{\bm H(\Delta \xi \oplus \xi) }} {\partial \delta \xi}=(\bm Tv)^\odotJδξ=∂δξ∂H(Δξ⊕ξ)=(Tv)⊙
(Tv)⊙=[I3×3−(Tv)1:3∧0T0T]4×6(\bm Tv)^\odot={\begin{bmatrix}\bm I_{3\times3} &-(\bm Tv)_{1 :3}^\land \\ \\\bm 0^T& \bm 0^T \\ \end{bmatrix} }_{4\times6} (Tv)⊙=⎣⎡I3×30T−(Tv)1:3∧0T⎦⎤4×6

三、利用高斯牛顿法求解

参考书籍：机器人学中的状态估计（高博等译）–P292-305

由线性化位姿方程：
H(Δξ⊕ξ)=H(Jl−1Δξ+ξ)=H(ξ)+JδξΔξ6×1\small \bm H(\Delta \xi \oplus \xi) =\bm H(\bm J_l^{-1}\Delta \xi+\xi) =\bm H(\xi)+\bm J_{\delta \xi} \Delta \xi_{6\times1} H(Δξ⊕ξ)=H(Jl−1Δξ+ξ)=H(ξ)+JδξΔξ6×1
这里的Jδξ\bm J_{\delta \xi}Jδξ为：
Jδξ=∂T(ξ)⋅v∂ξ⋅Jl−1(ξ)\small \bm J_{\delta \xi}=\frac {\partial{\bm T(\xi)\cdot v}} {\partial \xi} \cdot \bm J_l^{-1}(\xi) Jδξ=∂ξ∂T(ξ)⋅v⋅Jl−1(ξ)

定义误差函数e，z为观测数据:
e(ξ)=z−H(ξ)=z−T(ξ)⋅v\small e(\xi) = z-\bm H(\xi)=z-T(\xi)\cdot v e(ξ)=z−H(ξ)=z−T(ξ)⋅v
所以扰动模型：
e(Δξ⊕ξ)=z−H(Δξ⊕ξ)\small e(\Delta \xi \oplus \xi) =z-\bm H(\Delta \xi \oplus \xi) e(Δξ⊕ξ)=z−H(Δξ⊕ξ)
求导：
∂e(Δξ⊕ξ)∂δξ=−∂H(Δξ⊕ξ)∂δξ=Je=−Jδξ\frac {\partial{\bm e(\Delta \xi \oplus \xi)}} {\partial \delta \xi}=- \frac {\partial{\bm H(\Delta \xi \oplus \xi)}} {\partial \delta \xi}=J_e=-J_{\delta\xi} ∂δξ∂e(Δξ⊕ξ)=−∂δξ∂H(Δξ⊕ξ)=Je=−Jδξ
再由我上一篇文章：SLAM–非线性优化
JeT⋅Je(Δξ)=−JeT⋅e(ξ)\small J_{e}^T\cdot J_{e}(\Delta \xi) = -J_{e}^T\cdot e(\xi) JeT⋅Je(Δξ)=−JeT⋅e(ξ)
这样我们就可以求出Δξ\Delta \xiΔξ:
(Δξ)=−(JeT⋅Je)−1(JeT⋅e(ξ))\small (\Delta \xi) = -(J_{e}^T\cdot J_{e})^{-1}(J_{e}^T\cdot e(\xi)) (Δξ)=−(JeT⋅Je)−1(JeT⋅e(ξ))
令JeT⋅Je=He,−JeT⋅e(ξ)=beJ_{e}^T\cdot J_{e}=\bm H_e,-J_{e}^T\cdot e(\xi)=b_eJeT⋅Je=He,−JeT⋅e(ξ)=be
Δx=He−1⋅be\small \Delta x=H_e^{-1}\cdot b_eΔx=He−1⋅be
所以利用高斯迭代法，对于第K+1次更新位姿：
T(ξk+1)=exp⁡(Δx)⋅T(ξ0)=exp⁡(Δξ)⋅T(ξ0)\small T(\xi_{k+1}) = \exp(\Delta x)\cdot T(\xi_0)=\exp (\Delta \xi)\cdot T(\xi_0) T(ξk+1)=exp(Δx)⋅T(ξ0)=exp(Δξ)⋅T(ξ0)
迭代到e(ξk+1)e(\xi_{k+1})e(ξk+1)足够小时停止；

至此，线性化的估计位姿推导完成。

SLAM--线性化求解估计位姿相关推荐

SLAM中姿态估计的图优化方法比较（g2o/Ceres/GTSAM/SE-Sync）
编辑 | 深蓝AI 点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取后台回复[SLAM综述]获取视觉SLAM.激光SLAM.RGBD-SLAM等多篇综述! 本 ...
3d激光SLAM：LIO-SAM框架---位姿融合输出
3d激光SLAM:LIO-SAM框架---位姿融合输出前言 Eigen::Affine3f 位姿融合输出 result 前言 LIO-SAM的全称是:Tightly-coupled Lidar In ...
基于图像的三维重建——P3P求解相机位姿(5)
文章目录 P3P简介直接线性变换法 Kneip算法算法原理及流程基于RANSAC的Kneip算法 P3P简介相机位姿求解是SFM中必要的一步,而P3P问题的目的就是通过三维点(3对)对应关系确 ...
反光板导航SLAM（四）如何通过两个反光柱估计位姿
通常而言确定空间中的一个点的坐标需要三个点,也就是俗称的三点定位.但是在某些时候,场景内可能不一定能够采样到足够多的反光柱,比如只有两个.这个时候看起来似乎参数是不足的,毕竟机器人的参数有三个,两个方 ...
论文精读 | slam中姿态估计的图优化方法比较
一. 摘要对于位置环境中的自主导航问题,同步定位与建图(Simultaneous localization and mapping, SLAM)是一个非常重要的工具框架.根据SLAM字面含义可以得知 ...
基于直线的最小非线性SLAM相机姿态估计方法
Minimal Non-linear Camera Pose Estimation Method Using Lines for SLAM Applications 1. 介绍 2. 非线性优化方法 ...
ArUco估计位姿原理
ArUco使用 PnP [OpenCV/aruco]校准相机(Camera Calibration) Demo -------------------------------------------- ...
6D 位姿估计位姿测量常用方法总结（适用于单目标场景）
在机器人的应用中,往往会涉及到基于视觉的 6D 位姿估计或者位姿测量的问题,现在 Pose Estimation 的论文现在非常多,也有很大的创新,不过目前SOTA的工作一般面向几个特定的数据集,比如 ...
无人机利用视觉slam实现位置估计
无人机利用视觉slam实现室内位置估计自己近期所做的以及思考的东西我们实验室买了阿木的p200(带t265)无人机用于无人机的控制研究,通过阿木实验室的ros功能包px4_command可以无脑的 ...
使用C++ OpenCV实现椭圆区域检测与Aruco码的生成与检测并估计位姿
前景概要很多机器视觉的定位与识别场景,如无人车.无人机,都会用Aruco码或特定的标志物来实现,Aruco码的优点在于,xxxx(自行搜索). 对于像在低成本轻量级的无人机这种嵌入式系统上,搭载深度 ...

SLAM--线性化求解估计位姿

目录

前言

指数形式的线性化：

一、旋转矩阵SO(3)群

二、位姿矩阵SE(3)群

三、利用高斯牛顿法求解

SLAM--线性化求解估计位姿相关推荐

最新文章

热门文章