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

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

1. 完成Bundle Adjustment求解器

完成单目 Bundle Adjustment 求解器 problem.cc 中的部分代码。
• 完成 Problem::MakeHessian() 中信息矩阵 H 的计算。
• 完成 Problem::SolveLinearSystem() 中 SLAM 问题的求解

MakeHessian() 补充代码如下:

// TODO:: home work. 完成 H index 的填写.
H.block(index_i,index_j, dim_i, dim_j).noalias() += hessian;
if (j != i) {
	// 对称的下三角
	// TODO:: home work. 完成 H index 的填写.
	H.block(index_j,index_i, dim_j, dim_i).noalias() += hessian.transpose();

SolveLinearSystem()补充代码如下:

// TODO:: home work. 完成矩阵块取值,Hmm,Hpm,Hmp,bpp,bmm
MatXX Hmm = Hessian_.block(reserve_size,reserve_size, marg_size, marg_size);
MatXX Hmp = Hessian_.block(reserve_size,0, marg_size, reserve_size);
MatXX Hpm = Hessian_.block(0, reserve_size, reserve_size, marg_size);
VecX bpp = b_.segment(0,reserve_size);
VecX bmm = b_.segment(reserve_size,marg_size);
// TODO:: home work. 完成舒尔补 Hpp, bpp 代码
MatXX tempH = Hpm * Hmm_inv;
H_pp_schur_ = Hessian_.block(0,0,reserve_size,reserve_size) - tempH * Hmp;
b_pp_schur_ = bpp - tempH * bmm;
// TODO:: home work. step3: solve landmark
VecX delta_x_ll(marg_size);
delta_x_ll = Hmm_inv*(bmm-Hmp*delta_x_pp);
delta_x_.tail(marg_size) = delta_x_ll;

运行结果如下:

0 order: 0
1 order: 6
2 order: 12

 ordered_landmark_vertices_ size : 20
iter: 0 , chi= 5.35099 , Lambda= 0.00597396
iter: 1 , chi= 0.0289048 , Lambda= 0.00199132
iter: 2 , chi= 0.000109162 , Lambda= 0.000663774
problem solve cost: 22.7924 ms
   makeHessian cost: 19.2487 ms

Compare MonoBA results after opt...
after opt, point 0 : gt 0.220938 ,noise 0.227057 ,opt 0.220992
after opt, point 1 : gt 0.234336 ,noise 0.314411 ,opt 0.234854
after opt, point 2 : gt 0.142336 ,noise 0.129703 ,opt 0.142666
after opt, point 3 : gt 0.214315 ,noise 0.278486 ,opt 0.214502
after opt, point 4 : gt 0.130629 ,noise 0.130064 ,opt 0.130562
after opt, point 5 : gt 0.191377 ,noise 0.167501 ,opt 0.191892
after opt, point 6 : gt 0.166836 ,noise 0.165906 ,opt 0.167247
after opt, point 7 : gt 0.201627 ,noise 0.225581 ,opt 0.202172
after opt, point 8 : gt 0.167953 ,noise 0.155846 ,opt 0.168029
after opt, point 9 : gt 0.21891 ,noise 0.209697 ,opt 0.219314
after opt, point 10 : gt 0.205719 ,noise 0.14315 ,opt 0.205995
after opt, point 11 : gt 0.127916 ,noise 0.122109 ,opt 0.127908
after opt, point 12 : gt 0.167904 ,noise 0.143334 ,opt 0.168228
after opt, point 13 : gt 0.216712 ,noise 0.18526 ,opt 0.216866
after opt, point 14 : gt 0.180009 ,noise 0.184249 ,opt 0.180036
after opt, point 15 : gt 0.226935 ,noise 0.245716 ,opt 0.227491
after opt, point 16 : gt 0.157432 ,noise 0.176529 ,opt 0.157589
after opt, point 17 : gt 0.182452 ,noise 0.14729 ,opt 0.182444
after opt, point 18 : gt 0.155701 ,noise 0.182258 ,opt 0.155769
after opt, point 19 : gt 0.14646 ,noise 0.240649 ,opt 0.14677
------------ pose translation ----------------
translation after opt: 0 :-0.000478009   0.00115904  0.000366508 || gt: 0 0 0
translation after opt: 1 :-1.06959  4.00018 0.863877 || gt:  -1.0718        4 0.866025
translation after opt: 2 :-4.00232  6.92678 0.867244 || gt:       -4   6.9282 0.866025

2. 完成测试函数

完成滑动窗口算法测试函数。
• 完成 Problem::TestMarginalize() 中的代码,并通过测试。

补充代码如下:

// TODO:: home work. 将变量移动到右下角
/// 准备工作: move the marg pose to the Hmm bottown right
// 将 row i 移动矩阵最下面
Eigen::MatrixXd temp_rows = H_marg.block(idx, 0, dim, reserve_size);
Eigen::MatrixXd temp_botRows = H_marg.block(idx + dim, 0, reserve_size - idx - dim, reserve_size);
H_marg.block(idx, 0, dim, reserve_size) = temp_botRows;
H_marg.block(idx + dim, 0, reserve_size - idx - dim, reserve_size) = temp_rows;
// TODO:: home work. 完成舒尔补操作
Eigen::MatrixXd Arm = H_marg.block(0,n2,n2,m2);
Eigen::MatrixXd Amr = H_marg.block(n2,0,m2,n2);
Eigen::MatrixXd Arr = H_marg.block(0,0,n2,n2);

运行结果如下:

---------- TEST Marg: before marg------------
     100     -100        0
    -100  136.111 -11.1111
       0 -11.1111  11.1111
---------- TEST Marg: 将变量移动到右下角------------
     100        0     -100
       0  11.1111 -11.1111
    -100 -11.1111  136.111
---------- TEST Marg: after marg------------
 26.5306 -8.16327
-8.16327  10.2041

3. 论文总结

论文我没有完全看懂,但是我还是结合贺博的讲解尝试将其中提到的对于H矩阵的不同操作方式总一个总结:
论文和课件中由一丢丢不同的是,课件中讨论的是视觉SLAM中的操作方式,而论文中讨论的是VIO中的操作方式,这两者本质不同在于起不可观量不同,视觉SLAM中不可观量由七个,而VIO中只有四个,论文中提到的不同操作方式一共由三种,分别是:
(1)gauge fixation
这种方式的操作是固定第一个相机的位置和yaw轴这四个状态量,在论文中给出的公式为: p 0 = p 0 0 , Δ ϕ 0 z ≐ e z ⊤ Δ ϕ 0 = 0 \mathbf{p}_{0}=\mathbf{p}_{0}^{0}, \quad \Delta \phi_{0 z} \doteq \mathbf{e}_{z}^{\top} \Delta \phi_{0}=0 p0=p00,Δϕ0zezΔϕ0=0类比到课件中的操作方式就是:


在这里插入图片描述

(2)gauge prior
这种方式的操作是添加先验,增加系统可观性,公式如下: ∥ r 0 P ∥ Σ 0 P 2 ,  where  r 0 P ( θ ) ≐ ( p 0 − p 0 0 , Δ ϕ 0 z ) \left\|\mathbf{r}_{0}^{P}\right\|_{\Sigma_{0}^{P}}^{2}, \quad \text { where } \quad \mathbf{r}_{0}^{P}(\boldsymbol{\theta}) \doteq\left(\mathbf{p}_{0}-\mathbf{p}_{0}^{0}, \Delta \phi_{0 z}\right) r0PΣ0P2, where r0P(θ)(p0p00,Δϕ0z)类比到课件中的操作方式是:


在这里插入图片描述

(3)free gauge
这种才操作方式是不对可观性进行操作,但是为了防止奇异矩阵的产生,采用伪逆或者增加一些阻尼项(列文伯格法),因此这种方法实施后可能会对可观性造成影响,也就是会是系统零空间发生变换,对应到课件中操作方式是:


在这里插入图片描述

论文中的结论是三种方法的准确性和计算时间是相似的,free gauge的方法会稍微快一些,另外因为free gauge方法会使得零空间发生变化,因此它求解的结果会有很大不同。,下面这幅图说明了三种方法之间的不同,我的个人理解是,黑线 M θ \mathcal{M}_{\theta} Mθ是最小损失值的曲线, 蓝色的线是四个自由度的流形空间的曲线,也是gauge fixation方法代表的曲线,因为gauge fixation中仅仅是固定了第一个状态的位置,因此它的结果会沿着流形空间变换,而 绿色的线是gauge prior方法代表的曲线,因为其增加了先验约束,可以说其损失函数发生了些许变换,因此其最后的状态结果和gauge fixation方法也会由些许不同,而 红色的线是free gauge方法代表的曲线,对于给定的损失函数,free gauge方法使用伪逆选择最小尺寸的参数步骤,因此垂直于成本的等值线移动(最后这一点我不太明白),但是可以看出来其结果与前两者会有很大区别。

在这里插入图片描述

<think>好的,我现在要帮助用户了解如何从零开始实现视觉惯性里程计(VIO)。首先,我需要回忆VIO的基本概念和组成部分。VIO结合了视觉(如摄像头)和惯性测量单元(IMU)的数据,用于估计设备的运动轨迹和周围环境的结构。用户提到他们想手写代码实现,所以需要从基础开始逐步构建。 根据用户提供的引用,特别是引用[1]、[3]、[4],VIO的实现涉及初始化、传感器数据融合、状态估计、后端优化等。首先,我需要确定VIO的框架,比如松耦合还是紧耦合。引用[4]提到松耦合和紧耦合的分类,而通常紧耦合效果更好,但实现更复杂。因此,可能需要从松耦合开始,逐步深入。 接下来,步骤可能包括传感器数据的同步和时间戳对齐,这在引用[3]中提到了时间戳标定的重要性。然后是IMU和视觉数据的预处理,比如IMU的零偏校正和视觉特征点的提取与跟踪。 然后是状态估计,可能需要使用滤波方法(如卡尔曼滤波)或者优化方法(如非线性最小二乘)。引用[2]提到了高斯牛顿法用于曲线拟合,这可能在后端优化中使用。因此,后端优化部分需要实现如Bundle Adjustment结合IMU数据的方法。 此外,初始化过程是关键,引用[1]和[4]都提到需要正确的初始化和外参标定。可能需要使用视觉信息来初始化IMU的参数,比如尺度、重力方向等。 最后,代码实现部分需要分模块,比如数据读取、预处理、状态预测、优化、地图管理等。可能需要使用C++或Python,但用户提供的示例代码是Python,但实际VIO系统可能更常用C++以提高效率。 在实现过程中,需要注意IMU和视觉数据的同步,时间戳的处理,以及如何处理传感器丢失的情况,如引用[3]提到的视觉丢失时IMU顶替。此外,累积误差的问题需要通过回环检测或者全局优化来解决,但可能在初始实现中先处理局部优化。 可能还需要参考现有的开源项目,如VINS-Mono或OKVIS,了解它们的结构,但用户要求手写代码,所以需要从零开始构建基本框架。 总结步骤可能为: 1. 传感器数据读取与同步 2. IMU数据预处理(去噪、零偏校正) 3. 视觉特征提取与匹配 4. 初始化和外参标定 5. 紧耦合的滤波或优化方法实现 6. 后端非线性优化(如基于高斯牛顿法或LM算法) 7. 尺度估计和重力对齐 8. 处理累积误差和闭环检测 需要确保每个模块的正确性,逐步集成测试。例如,先实现IMU的积分和预测,再处理视觉的位姿估计,然后将两者融合。可能出现的问题包括时间不同步、参数标定不准确、优化过程中的数值不稳定等,需要通过调试和测试来解决。</think>### 从零开始实现视觉惯性里程计(VIO)的步骤详解 #### 一、基础理论与框架设计 1. **VIO核心原理** VIO通过融合相机(视觉)和IMU(惯性)数据,估计载体在短时间内的相对运动。视觉提供环境特征点约束,IMU提供高频运动预测,两者互补: - IMU适合快速运动但存在零偏漂移[^3] - 视觉适合慢速运动但能校正IMU零偏并提供尺度信息[^4] 2. **系统框架选择** 推荐从**紧耦合**框架入手(直接融合原始数据),尽管实现复杂度高于松耦合,但精度更高: ```text VIO Pipeline: 传感器数据 → 时间同步 → 视觉特征提取 + IMU预积分 → 紧耦合优化 → 位姿输出 ``` --- #### 二、核心模块实现步骤 ##### 1. 传感器数据同步与预处理 - **时间戳对齐**:使用线性插值或滑动窗口对齐相机与IMU的时间戳[^1] - **IMU预处理**: - 零偏校正:通过静止状态下的均值计算初始零偏 - 角速度积分公式(四元数更新): $$ q_{t+1} = q_t \otimes \left[1, \frac{1}{2}\omega\Delta t\right]^T $$ 其中$\omega$为陀螺仪测量值 ##### 2. 视觉前端实现 ```cpp // 示例:ORB特征提取(OpenCV) cv::Ptr<cv::ORB> orb = cv::ORB::create(500); std::vector<cv::KeyPoint> kps; cv::Mat descriptors; orb->detectAndCompute(img, cv::Mat(), kps, descriptors); ``` - **光流跟踪**:使用LK光流法匹配相邻帧特征点 - **关键帧选择**:基于平移/旋转量阈值判断 ##### 3. IMU预积分 - 计算两帧图像间的IMU运动增量: $$ \Delta v = \sum_{k=i}^{j-1} (R_k a_k - g)\Delta t $$ $$ \Delta p = \sum \Delta v_k \Delta t $$ 其中$R_k$为姿态矩阵,$a_k$为加速度计测量值[^4] ##### 4. 紧耦合非线性优化 使用**滑动窗口优化**最小化视觉重投影误差与IMU预积分误差: $$ \min_{\mathbf{x}} \left( \sum \rho \left( \| r_p \|^2_{\Sigma_p} \right) + \sum \| r_I \|^2_{\Sigma_I} \right) $$ - 残差项: - 视觉残差$r_p = z - \pi(T_{cw}X)$ - IMU残差$r_I = \log\left( \Delta \hat{T}_{ij}^{-1} T_i^{-1} T_j \right)$ - 求解方法:高斯牛顿法或Levenberg-Marquardt算法[^2] --- #### 三、关键问题解决方案 1. **初始化** - **视觉-IMU外参标定**:通过手眼标定法计算$T_{imu}^{cam}$ - **尺度恢复**:单目视觉通过SFM计算特征点深度,结合IMU加速度约束求解尺度因子 2. **零偏在线估计** 将加速度计和陀螺仪零偏作为状态量加入优化变量: $$ \mathbf{x} = [T_{wb}, v, b_a, b_g, \lambda_1, ..., \lambda_n] $$ 3. **关键代码示例(C++伪代码)** ```cpp class VIOOptimizer { public: void addImuResidual(const ImuData& imu) { // 添加IMU预积分残差块 problem.AddResidualBlock( new ImuCostFunction(imu.delta_p, imu.delta_v, imu.delta_q), nullptr, pose_i, pose_j, bias_a, bias_g ); } void addVisionResidual(const Feature& feat) { // 添加视觉重投影残差块 problem.AddResidualBlock( new ReprojectionCost(feat.uv_observed), new CauchyLoss(0.5), pose, landmark ); } }; ``` --- #### 四、测试与调试建议 1. **仿真数据验证** 使用EuRoC MAV数据集,对比轨迹的ATE(绝对轨迹误差) 2. **真机调试技巧** - 优先调试IMU积分:静止状态下积分速度应趋近于零 - 可视化特征跟踪:确保匹配点分布均匀且跟踪稳定 3. **性能优化方向** - 使用Schur补加速H矩阵求逆 - 引入FEJ(First Estimate Jacobians)避免线性化点不一致 ---
评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值