當前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

深蓝学院《从零开始手写VIO》作业一

發布時間：2025/3/20 编程问答 23 豆豆

生活随笔收集整理的這篇文章主要介紹了深蓝学院《从零开始手写VIO》作业一小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

深藍學院《從零開始手寫VIO》作業一

深藍學院《從零開始手寫VIO》作業一
- 1. VIO文獻閱讀
- - 1.1 視覺與IMU進行融合之后有何優勢？
  - 1.2 有哪些常見的視覺+IMU融合方案？有沒有工業界應用的例子？
  - 1.3 在學術界，VIO研究有哪些新進展？有沒有將學習方法用到VIO中的例子？
- 2. 四元數和李代數更新
- 3. 其他導數

深藍學院《從零開始手寫VIO》作業一

1. VIO文獻閱讀

閱讀VIO相關綜述文獻如A review of visual inertial odometry from filtering and
optimisation
perspectives，回答以下問題：
視覺與IMU進行融合之后有何優勢？有哪些常見的視覺+IMU融合方案？有沒有工業界應用的例子？
在學術界，VIO研究有哪些新進展？有沒有將學習方法用到VIO中的例子？

1.1 視覺與IMU進行融合之后有何優勢？

我覺得這個問題在高博的課上已經回答得很清楚了，IMU和視覺定位方案優劣勢對比：

方案IMU視覺

優勢	快速響應，不受成像質量影響，角速度普遍比較準確，可估計絕對尺度	不產生漂移，直接測量旋轉與平移
劣勢	存在零偏，低精度IMU積分定位發散，高精度價格昂貴	受圖像遮擋、運動物體干擾，單目無尺度，單目純旋轉無法估計，快速運動時丟失

那么用自己的話總結一下：
（1）IMU具備的快速響應的特點正好可以彌補視覺在快速運動時容易丟失的缺陷；
（2）低精度IMU積分定位容易發散，而視覺定位在靜止時不會產生漂移；
（3）單目相機無法進行絕對尺度估計，而IMU正好可以；
綜上所述，IMU和視覺可以達到優勢互補，在達到同等定位精度或者魯棒性的情況下，IMU和視覺的組合可以極大的降低成本，提高性能！

1.2 有哪些常見的視覺+IMU融合方案？有沒有工業界應用的例子？

常見的視覺+IMU融合方案有：
VINS (單目+IMU、雙目+IMU)
OKVIS (單目+IMU、雙目+IMU)
ROVIO (單目+IMU）
RKSLAM (單目+IMU）
ORB_SLAM-IMU（單目+IMU）

工業界應用的例子：
工業界里面在AR/VR，自動駕駛，無人機等很多地方都有用到的，太多了，這里舉兩個例子好了
凌感馬賡宇：基于視覺+慣性傳感器的空間定位方法
全球首款采用VIO定位技術的Mark無人機

參考：
快看，那個學SLAM 的崩潰了!
SLAM中VIO的優勢及入門姿勢

1.3 在學術界，VIO研究有哪些新進展？有沒有將學習方法用到VIO中的例子？

有篇文章叫VINet : Visual-inertial odometry as a sequence-to-sequence learning problem就是將VIO與深度學習結合，而知乎上有大佬對此做出了解釋深度學習SLAM ：最新的基于深度學習的deepvo，VINet，大家怎樣評價？

在HKUST Aerial Robotics Group——Publications里面可以關注到沈老師那邊的最新進展，像Real‐time dense mapping for online processing and navigation這篇文章就是基于VIO（但不是完全基于）進行了大尺度的稠密3D重建

2. 四元數和李代數更新

課件提到了可以使用四元數或旋轉矩陣存儲旋轉變量。當我們用計算出來的 $ω$ 對某旋轉更新時，有兩種不同方式： $R←Rexp?(ω∧)q←q[1,12ω]T\begin{array}{l}{\mathbf{R} \leftarrow \mathbf{R} \exp \left(\boldsymbol{\omega}^{\wedge}\right)} \\ {\mathbf{q} \leftarrow \mathbf{q}\left[1, \frac{1}{2} \boldsymbol{\omega}\right]^{\mathrm{T}}}\end{array}$ 請編程驗證對于小量 $ω = [0.01, 0.02, 0.03]^T$ ，兩種方法得到的結果非常接近，實踐當中可視為等同。因此，在后文提到旋轉時，我們并不刻意區分旋轉本身是 $q$ 還是 $R$ ，也不區分其更新方式為上式的哪一種。

我的驗證思路是這樣的，分別計算Eigen下的旋轉矩陣相乘，Eigen下四元數相乘，Sophus下李代數擾動的三個結果進行比較（不知道用庫是不是算偷懶，應該不會讓自己寫四元數相乘來驗證這么狠吧…），代碼如下：

#include <iostream> #include <Eigen/Core> #include <Eigen/Geometry> #include <sophus/so3.h>using namespace std;int main() {//生成原始旋轉矩陣R和四元數qEigen::Matrix3d R = Eigen::Matrix3d::Identity();Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1));R = rotation_vector.toRotationMatrix();Eigen::Quaterniond q;q = R;cout<<"R is"<<endl<<R<<endl<<endl;//生成小量w的李代數Eigen::Vector3d w;w<<0.1,0.2,0.3;//將李代數w轉換成旋轉矩陣R_w和四元數q_wSophus::SO3 SO3_w = Sophus::SO3::exp(w);Eigen::Matrix3d R_w = SO3_w.matrix();Eigen::Quaterniond q_w;q_w = R_w;//[驗證方式一]：在Eigen下進行計算Eigen::Matrix3d R_res = R*R_w;Eigen::Quaterniond q_res = q*q_w;Eigen::Matrix3d R_q_res = q_res.toRotationMatrix();cout<<"the result of calculation by R is:"<<endl<<R_res<<endl<<endl;cout<<"the result of calculation by q is:"<<endl<<R_q_res<<endl<<endl;//[驗證方式二]：在Sophus下進行計算Sophus::SO3 SO3_R(R);Sophus::SO3 SO3_res = SO3_R*SO3_w;cout<<"the result of calculation by so3 is:"<<endl<<SO3_res.matrix()<<endl<<endl; }

結果如下

R is0.707107 -0.707107 00.707107 0.707107 00 0 1the result of calculation by R is: 0.447473 -0.87239 0.196733 0.875884 0.471934 0.100523 -0.18054 0.127335 0.97529the result of calculation by q is: 0.447473 -0.87239 0.196733 0.875884 0.471934 0.100523 -0.18054 0.127335 0.97529the result of calculation by so3 is: 0.447473 -0.87239 0.196733 0.875884 0.471934 0.100523 -0.18054 0.127335 0.97529

結果可以看出無論是通過旋轉矩陣還是四元數還是李代數，乘以小量的結果都是一致的，因此符合題意

做題中間稍微耽誤了點時間的地方是安裝Sophus庫的時候先安裝的最新版本，但是新版本的類和舊版本有了較大改動，不太熟悉，因此又卸了重裝…

3. 其他導數

使用右乘 $so(3)\mathfrak{s o}(3)$ ，推導以下導數 $d(R?1p)dR\frac{\mathrmozvdkddzhkzd\left(\mathbf{R}^{-1} \mathbf{p}\right)}{\mathrmozvdkddzhkzd \mathbf{R}}$ $dln?(R1R2?1)dR2\frac{\operatorname{dln}\left(\mathbf{R}_{1} \mathbf{R}_{2}^{-1}\right)}{\mathrmozvdkddzhkzd \mathbf{R}_{2}}$

（1）證明如下 $d(R?1p)dR=lim?φ→0(R?exp?(φ∧))?1p?R?1pφ=(exp?(φ∧))?1R?1p?R?1pφ=((exp?(φ∧))?1?1)R?1pφ≈(1?φ∧?1)R?1pφ=?φ∧R?1pφ=(R?1p)∧φφ=(R?1p)∧\begin{aligned} \frac{d\left(R^{-1} p\right)}{d R}&=\lim _{\varphi \rightarrow 0} \frac{\left(\operatorname{R} \exp\left(\varphi^{\wedge}\right)\right)^{-1} p-R^{-1} p}{\varphi} \\&=\frac{\left(\exp \left(\varphi^{\wedge}\right)\right)^{-1} R^{-1} p-R^{-1} p}\varphi \\&=\frac{\left(\left(\exp \left(\varphi^{\wedge}\right)\right)^{-1}-1\right) R^{-1} p}{\varphi} \\&\approx\frac{\left(1-\varphi^{\wedge}-1\right) R^{-1} p}{\varphi} \\&=\frac{-\varphi^{\wedge} R^{-1} p}{\varphi} \\&=\frac{\left(R^{-1} p\right)^{\wedge} \varphi}{\varphi} \\&=\left(R^{-1} p\right)^{\wedge} \end{aligned}$ 其中第二行到第三行是《機器人學中的狀態估計》的第220頁的性質推導： $C?1≡CT≡∑n=0∞1n!(??∧)n≈1??∧C^{-1} \equiv C^{\mathrm{T}} \equiv \sum_{n=0}^{\infty} \frac{1}{n !}\left(-\phi^{\wedge}\right)^{n} \approx 1-\phi^{\wedge}$ 倒數第二行到倒數第一行是叉乘的性質

（2）證明如下：
這里我改用了嘗試兩種推導方案：
方案一：
$dln?(R1R2?1)dR2=lim??→0ln?(R1(R2exp?(?∧))?1)?ln?(R1R2?1)?=lim??→0ln?(R1(exp?(?∧))?1R2?1)?ln?(R1R2?1)?=lim??→0ln?((exp?(?∧)R1?1)?1R2?1)?ln?(R1R2?1)?=lim??→0ln?((R1?1R1exp?(?∧)R1?1)?1R2?1)?ln?(R1R2?1)?=lim??→0ln?((R1?1exp?((R1?)∧))?1R2?1)?ln?(R1R2?1)?=lim??→0ln?(exp?((R1?)∧)?1R1R2?1)?ln?(R1R2?1)?≈lim??→0ln?(exp?(?(R1?)∧)R1R2?1)?ln?(R1R2?1)?=lim??→0Jl?1(ln?(R1R2?1))(?(R1?))+ln?(R1R2?1)?ln?(R1R2?1)?=Jl?1(ln?(R1R2?1))(?R1)\begin{aligned} \frac{d \ln \left(R_{1} R_{2}^{-1}\right)}{d R_{2}} &=\lim _{\phi \rightarrow 0} \frac{\ln \left(R_{1}\left(R_{2} \exp \left(\phi^{\wedge}\right)\right)^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\phi \rightarrow 0} \frac{\ln \left(R_{1}\left(\exp \left(\phi^{\wedge}\right)\right)^{-1} R_{2}^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\phi \rightarrow 0} \frac{\ln \left(\left(\exp \left(\phi^{\wedge}\right) R_{1}^{-1}\right)^{-1} R_{2}^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\phi \rightarrow 0} \frac{\ln \left(\left(R_{1}^{-1} R_{1} \exp \left(\phi^{\wedge}\right) R_{1}^{-1}\right)^{-1} R_{2}^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\phi \rightarrow 0} \frac{\ln \left(\left(R_{1}^{-1} \exp \left(\left(R_{1} \phi\right)^{\wedge}\right)\right)^{-1} R_{2}^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\phi \rightarrow 0} \frac{\ln \left(\exp \left(\left(R_{1} \phi\right)^{\wedge}\right)^{-1} R_{1} R_{2}^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&\approx\lim _{\phi \rightarrow 0} \frac{\ln \left(\exp \left(-\left(R_{1} \phi\right)^{\wedge}\right) R_{1} R_{2}^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\phi \rightarrow 0} \frac{J_{l}^{-1}\left(\ln \left(R_{1} R_{2}^{-1}\right)\right)\left(-\left(R_{1} \phi\right)\right)+\ln \left(R_{1} R_{2}^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=J_{l}^{-1}\left(\ln \left(R_{1} R_{2}^{-1}\right)\right)\left(-R_{1}\right) \end{aligned}$

方案二： $dln?(R1R2?1)dR2=lim??→0ln?(R1(R2exp?(?∧))?1)?ln?(R1R2?1)?=lim??→0ln?(R1(exp?(?∧))?1R2?1)?ln?(R1R2?1)?=lim?φ→0ln?(R1R2?1R2(exp?(?∧))?1R2?1)?ln?(R1R2?1)?=lim??→0ln?(R1R2?1(R2(exp?(?∧))R2?1)?1)?ln?(R1R2?1)?=lim?θ→0ln?(R1R2?1(R2(exp?(?∧))R2T)?1)?ln?(R1R2?1)?=lim?t→0ln?(R1R2?1(exp?(R2?)∧)?1)?ln?(R1R2?1)?=lim?t→0ln?(R1R2?1exp?(?(R2?)∧))?ln?(R1R2?1)?=lim??→0ln?(R1R2?1)?Jr?1(ln?(R1R2?1))R2??ln?(R1R2?1)?=Jr?1(ln?(R1R2?1))(?R2)\begin{aligned} \frac{d \ln \left(R_{1} R_{2}^{-1}\right)}{d R_{2}} &=\lim _{\phi \rightarrow 0} \frac{\ln \left(R_{1}\left(R_{2} \exp \left(\phi^{\wedge}\right)\right)^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\phi \rightarrow 0} \frac{\ln \left(R_{1}\left(\exp \left(\phi^{\wedge}\right)\right)^{-1} R_{2}^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\varphi \rightarrow 0} \frac{\ln \left(R_{1} R_{2}^{-1} R_{2}\left(\exp \left(\phi^{\wedge}\right)\right)^{-1} R_{2}^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\phi \rightarrow 0} \frac{\ln \left(R_{1} R_{2}^{-1}\left(R_{2}\left(\exp \left(\phi^{\wedge}\right)\right) R_{2}^{-1}\right)^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\theta \rightarrow 0} \frac{\ln \left(R_{1} R_{2}^{-1}\left(R_{2}\left(\exp \left(\phi^{\wedge}\right)\right) R_{2}^{T}\right)^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{t \rightarrow 0} \frac{\ln \left(R_{1} R_{2}^{-1}\left(\exp \left(R_{2}\phi\right)^{\wedge}\right)^{-1}\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{t \rightarrow 0} \frac{\ln \left(R_{1} R_{2}^{-1}\exp \left(-\left(R_{2}\phi\right)^{\wedge}\right)\right)-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=\lim _{\phi \rightarrow 0} \frac{\ln \left(R_{1} R_{2}^{-1}\right)-J_{r}^{-1}(\ln \left(R_{1} R_{2}^{-1}\right)) R_{2} \phi-\ln \left(R_{1} R_{2}^{-1}\right)}{\phi} \\&=J_{r}^{-1}\left(\ln \left(R_{1} R_{2}^{-1}\right)\right)\left(-R_{2}\right) \end{aligned}$

與50位技術專家面對面20年技術見證，附贈技術全景圖

總結

以上是生活随笔為你收集整理的深蓝学院《从零开始手写VIO》作业一的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。

上一篇： VIO在走廊弱纹理环境下的优化——VIN
下一篇：深蓝学院《从零开始手写VIO》作业三