【Basalt】nfr_mapper 中的“小 SfM/BA 后端”

"把 VIO 边缘化留下的线性先验，转换成可再次优化的非线性位姿因子；再结合重新建立的视觉匹配和三角化点，做一次局部/离线 BA。"

nfr_mapper文件定义的是一个"后端式稠化/重建 mapper"，核心作用不是做实时 VIO 前端跟踪，而是：

从 VIO 边缘化输出的 MargData 里恢复关键帧位姿约束
对这些关键帧重新做特征提取、跨帧匹配、建 track、三角化地图点
最后做一次小规模 bundle adjustment，把位姿和点一起优化

对应类在 nfr_mapper.h 和实现 nfr_mapper.cpp。

它的作用

从成员和流程看，NfrMapper 继承自 ScBundleAdjustmentBase<double>，所以它本质上是一个 BA 优化器的具体应用。nfr_mapper.h

它内部主要维护几类数据：

frame_poses：要优化的关键帧位姿
roll_pitch_factors、rel_pose_factors：从 VIO 边缘化先验里"抽"出来的非线性位姿约束
img_data：保存关键帧图像
feature_corners、feature_matches、feature_tracks：特征、匹配和轨迹
lmdb：地图点和观测，来自基类

你可以把它理解成：

"拿 VIO 已经估过的一批关键帧和图像，重新做一次更偏全局/离线的视觉建图与优化。"

怎么被调用

它不是在 sqrt_keypoint_vio.cpp 里直接 new 出来常驻运行的主路径模块；真正的直接入口主要在 demo：

典型调用顺序是：

构造 NfrMapper
读取 VIO 输出的 MargData
addMargData(...)
detect_keypoints()
match_stereo()
match_all()
build_tracks()
setup_opt()
optimize()

在 demo 里能直接看到：

detect() 调 detect_keypoints() demo/mapper.cpp
match() 调 match_stereo() + match_all() demo/mapper.cpp
tracks() 调 build_tracks() + setup_opt() demo/mapper.cpp
optimize() 调 nrf_mapper->optimize(...) demo/mapper.cpp

而 MargData 的来源，是 VIO 在边缘化时往 out_marg_queue 里塞出来的：sqrt_keypoint_vio.cpp。
MargData 结构定义在 imu_types.h。

原理分三段

1. 从边缘化结果里提取"位姿因子"

addMargData() 先做两件事：

processMargData()：把 POSE_VEL_BIAS 状态裁成只保留 pose，速度和 bias 再次边缘化掉，同时保存图像数据到 img_data

见 nfr_mapper.cpp
extractNonlinearFactors()：把边缘化后的高斯先验 abs_H / abs_b 转成更直观的非线性约束

见 nfr_mapper.cpp

这里做得很巧：

先把 abs_H 反解成协方差 cov_old
对被边缘化的关键帧 kf_id，抽出：
- roll/pitch 约束
- 它与其它关键帧之间的相对位姿约束

也就是说，它不是直接把 VIO 的线性先验原封不动搬来，而是把它"重解释"为：

一个 RollPitchFactor
多个 RelPoseFactor

误差模型在 nfr.h：

relPoseError(...)：相对位姿误差
rollPitchError(...)：roll/pitch 误差
yawError(...) / absPositionError(...)：用于构造中间 Jacobian

这个过程很像"从旧优化结果里蒸馏出更紧凑的位姿图约束"。

2. 用图像重新建视觉观测

接下来 mapper 不依赖前端已有 track，而是自己重建：

detect_keypoints()：在保存的关键帧图像上重新检测角点、算方向、算描述子、建 BoW
nfr_mapper.cpp
match_stereo()：同一时刻左右目做 stereo matching，利用已知双目外参和 essential matrix 过滤内点
nfr_mapper.cpp
match_all()：跨时间、跨视角用 HashBoW 检索候选图像对，再做描述子匹配 + RANSAC 几何验证
nfr_mapper.cpp
build_tracks()：把 pairwise matches 融合成多帧 feature track
nfr_mapper.cpp
setup_opt()：从 track 里挑两次观测做初始三角化，生成地图点和观测
nfr_mapper.cpp

所以它更像一个"小 SfM/BA 后端"。

3. 做联合优化

optimize() 是核心：nfr_mapper.cpp

每轮迭代会把三类误差一起线性化：

视觉重投影误差：来自地图点观测
相对位姿误差：rel_pose_factors
roll/pitch 误差：roll_pitch_factors

这些在线性化器 MapperLinearizeAbsReduce 里被累加到同一个 Hessian / b 中

见 nfr_mapper.h

然后用 LM 或 GN 更新：

更新所有 frame_poses
再更新地图点参数
如果 LM 步长让总代价下降就接受，否则回退并增大 lambda

所以优化目标可以概括成：

"既要满足视觉重投影，又尽量保持从 VIO 先验里提炼出来的姿态/相对位姿关系。"

一句话理解

NfrMapper 的原理就是：

"把 VIO 边缘化留下的线性先验，转换成可再次优化的非线性位姿因子；再结合重新建立的视觉匹配和三角化点，做一次局部/离线 BA。"

和 sqrt_keypoint_vio.cpp 的关系

sqrt_keypoint_vio.cpp 是实时 VIO 主估计器；它在边缘化关键帧时会输出 MargData 到 out_marg_queue。sqrt_keypoint_vio.cpp

NfrMapper 则消费这些 MargData，把它们变成更适合建图优化的因子和初值。

所以两者关系是：

SqrtKeypointVioEstimator：实时估计，产出边缘化信息
NfrMapper：利用这些边缘化信息做后端地图优化

如果你愿意，我下一条可以直接按"函数调用链"给你画一版：
MargData -> addMargData -> processMargData -> extractNonlinearFactors -> detect/match/track/setup_opt -> optimize，并把每一步输入输出都展开。