VINS_MONO marginalization机制

本文详细解析了视觉里程计中滑动窗口优化策略,包括如何根据帧间视差判断关键帧,以及如何更新滑窗内的帧。讨论了Estimator模块中不同条件下帧的处理流程,以及特征管理中视差检查的重要性。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

假设WINDOW_SIZE=5(代码中为10),则buffer大小为6(WINDOW_SIZE+1),最新来的帧放到WINDOW_SIZE的位置,记作X,但是WINDOW_SIZE位置的帧不参与优化。如图:
1418770-20181031153707540-305917454.png
estimator::processImage()中首先检查新的到帧与前一帧之间的视差,如果视差大于阈值,则新来的帧会作为LoopClosure中的KeyFrame,把滑窗中最早的一帧移出去丢掉,X加入优化的滑窗,marginalization_flag记作MARGIN_OLD。
移除之后buffer状态如图:
1418770-20181031154211048-746529120.png
如果新来的帧与前一帧间的视差较小,新来的帧不作为KeyFrame,把WINDOW_SIZE-1位置处的帧丢掉,X加入优化的滑窗,marginalization_flag记作MARGIN_SECOND_NEW。如图:
1418770-20181031154558432-547706941.png

需要注意的是,放进LoopClsoure buffer中的KeyFrame是WINDOWS_SIZE-2处的帧,因为WINDOW_SIZE-1处的帧不一定是关键帧。
具体代码参考:
Estimator::processImage()
Estimator::slideWindow()
FeatureManager::addFeatureCheckParallax()。

比较奇怪的一点是Estimator::slideWindow()中对MARGIN_OLD,移除最早一帧时的循环里面不是用赋值而是用swap,虽然逻辑上没有问题,但是会更消耗资源吧,不知道是基于什么原因考虑的。

转载于:https://www.cnblogs.com/glxin/p/9883642.html

<think>嗯,用户这次想深入理解VINS-Mono中边缘化标志位的具体含义。从问题来看,用户应该已经对VINS-Mono的基础框架有所了解,现在想重点关注边缘化策略的细节。用户引用的资料显示ta已经查阅过相关文档,特别是关于滑动窗口维护的部分。 用户可能正在调试VINS-Mono代码,遇到了边缘化相关的逻辑需要理解。考虑到ta专门提到MARGIN_OLD和MARGIN_SECOND_NEW这两个枚举值,很可能是想在算法层面理解不同边缘化策略的触发条件及其影响。 从专业角度需要解释清楚三个关键点:首先是两种模式的根本区别在于被移除帧的类型(最旧帧vs次新帧),其次是触发条件(视差检测机制),最后是技术实现差异(是否保留先验约束)。特别要注意强调MARGIN_SECOND_NEW这种特殊场景的处理逻辑,这是很多初学者容易困惑的地方。 用户似乎对算法工程实现很感兴趣,所以准备用具体场景说明:当无人机悬停时采用MARGIN_SECOND_NEW避免退化问题,而当剧烈运动时采用MARGIN_OLD保证可观性。这种实际案例能帮助理解。 在数学表达方面,需要展示边缘化如何转化为先验项的过程,但要注意控制公式复杂度,重点用文字解释其物理意义。考虑到用户可能参考代码实现,准备补充说明在estimator.cpp中对应的代码位置。 用户之前的问题涉及solver_flag状态机,这次转向边缘化策略,说明ta在系统性地研究VINS的状态管理机制。后续问题可以引导ta思考边缘化与初始化状态的关系,或者与重定位的交互逻辑。</think>在VINS-Mono中,`marginalization_flag`的取值(`MARGIN_OLD`和`MARGIN_SECOND_NEW`)决定了滑动窗口边缘化的策略,直接影响系统精度和稳定性: ### 1. **`MARGIN_OLD`(边缘化最旧帧)** - **核心作用**:移除滑动窗口中最旧的帧 $$ \mathcal{W} = \{f_{k}, f_{k+1}, ..., f_{k+N}\} \rightarrow \{f_{k+1}, ..., f_{k+N}, f_{new}\} $$ - **触发条件**: - 当前帧与最旧帧的**视差足够大**(通常>10°) $$ \theta = \arccos\left( \frac{\mathbf{p}_{old} \cdot \mathbf{p}_{curr}}{|\mathbf{p}_{old}| \cdot |\mathbf{p}_{curr}|} \right) > \theta_{th} $$ - 场景发生显著运动,保证新观测能提供有效约束[^2] - **技术实现**: ```cpp if (marginalization_flag == MARGIN_OLD) { marginalizeOldFrame(); // 边缘化最旧帧 addNewFrame(); // 添加新帧 } ``` - **关键特性**: - 将最旧帧的状态转换为**先验约束**,保留其对剩余帧的几何关系 - 维持窗口尺寸不变(通常10-20帧) - 避免累积误差导致尺度漂移[^1][^2] ### 2. **`MARGIN_SECOND_NEW`(边缘化次新帧)** - **核心作用**:移除窗口中倒数第二新的帧(即最新帧的前一帧) $$ \mathcal{W} = \{f_{k}, ..., f_{N-1}, f_{N}\} \rightarrow \{f_{k}, ..., f_{N-2}, f_{N}\} $$ - **触发条件**: - 当前帧与最旧帧**视差过小**(接近纯旋转运动) - 最新两帧间**特征跟踪稳定**,可直接用最新帧替代次新帧[^2] - **技术实现**: ```cpp if (marginalization_flag == MARGIN_SECOND_NEW) { removeFrame(N-1); // 删除次新帧(索引N-1) keepFrame(N); // 保留最新帧(索引N) } ``` - **关键特性**: - 避免在**低视差场景**(如悬停、缓慢平移)中边缘化旧帧导致约束丢失 - 防止系统因运动退化而崩溃 - 维持IMU预积分连续性(次新帧与最新帧的IMU数据高度相关)[^1][^2] ### 决策逻辑(代码级说明) 在`vins_estimator/src/estimator.cpp`的`processImage()`函数中: ```cpp // 计算当前帧与最旧帧的视差 double parallax = computeParallax(last_frame, curr_frame); if (parallax > MIN_PARALLAX && window_size == MAX_WINDOW_SIZE) { marginalization_flag = MARGIN_OLD; // 移除最旧帧 } else if (window_size >= MAX_WINDOW_SIZE) { marginalization_flag = MARGIN_SECOND_NEW; // 移除次新帧 } ``` ### 两种策略的对比 | **特性** | `MARGIN_OLD` | `MARGIN_SECOND_NEW` | |------------------------|---------------------------|------------------------------| | **移除对象** | 滑动窗口最旧帧 | 倒数第二新帧(次新帧) | | **视差条件** | 大视差(>10°) | 小视差(接近纯旋转) | | **主要目的** | 引入新观测,防止窗口饱和 | 避免运动退化导致系统崩溃 | | **先验约束保留** | 完整保留旧帧约束 | 仅保留最新帧与剩余帧的约束 | | **典型场景** | 快速平移、旋转 | 悬停、缓慢移动 | ### 工程意义 1. **`MARGIN_OLD`** 在显著运动时保证历史约束的有效传递,是**精度维持的核心机制** $$ \mathcal{J}_{prior} = \| \mathbf{r}_{prior} \|_{\Sigma_{prior}}^2 $$ 其中$\mathbf{r}_{prior}$是边缘化产生的先验残差[^1] 2. **`MARGIN_SECOND_NEW`** 本质是**系统鲁棒性的保护措施**,防止在退化运动时因边缘化旧帧导致优化问题病态 3. **与初始化关联** 初始化阶段(`solver_flag==INITIAL`)禁用边缘化,避免未收敛时破坏约束[^3][^4] --- **相关问题** 1. 视差阈值`MIN_PARALLAX`的设置如何影响系统性能? 2. 在`MARGIN_SECOND_NEW`模式下如何避免IMU预积分中断? 3. 边缘化导致的信息丢失如何定量评估? 4. 为何不直接丢弃最新帧而选择保留?
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值