0. 简介笔者第一次看到 GICI-LIB 这个名字的反应是又一个 GNSS/IMU 紧耦合的轮子翻开仓库才意识到这次不太一样——它不是再写一遍 RTK 估计器而是把 SPP、RTD、RTK、PPP、GNSS/IMU 紧耦合、GNSS/Visual/Inertial 紧耦合等十多种主流融合算法全部放进同一张因子图里、共享同一套参数块、共享同一套异常剔除与残差边管理。换句话说作者把过去散落在 RTKLIB、OKVIS、SVO 三个不同生态里的核心代码重新做了一次工程基础设施级的统一。如果你对车到底怎么知道自己在哪这个问题感兴趣这篇博客会带你从城市峡谷里的卫星漂移说起一路看到因子图怎么把 GNSS 双差伪距、IMU 预积分、视觉重投影三种本质完全不同的观测捏合在一起再回到代码层验证这些抽象是如何在 C 里被实例化的。项目主页https://github.com/chichengcn/gici-open1. 从城市峡谷里的漂移说起1.1 手机导航在高架下漂移这件事本质是观测被环境吃掉了很多人第一次质疑GPS 到底准不准往往是在打车 App 里。车明明开在高架桥上面地图却把你定位到了下面的辅路明明走在一条直行道上定位轨迹却忽左忽右抖个不停。这种现象的根源不在算法不够强而在观测本身被环境破坏掉了。卫星信号需要穿越大气层、再绕过周围建筑物的反射面才能到达接收机天线每一次反射、衍射、遮挡都会让伪距和载波相位测量带上偏差。在 GNSS 工程语境里这叫多路径效应multipath和非视距传播NLOS它们是城市定位最难根除的两类系统误差。进一步看光靠提升卫星接收机的灵敏度解决不了这个问题。即便用最贵的多频多系统接收机只要环境本身把直达信号挡住了接收机收到的就只能是反射回来的假信号灵敏度越高反而越容易把这些假信号当真信号处理。真正的破局点不在传感器本身而在传感器的组合——让别的传感器在卫星失效的瞬间接管定位再等卫星条件恢复的时候把累计误差校回来。这就是 GICI-LIB 选择GNSSIMU相机三件套作为最小可行集的工程逻辑。1.2 现有路线的缺口开源生态里没有统一融合框架往前看二十年的开源历史能很清楚地把这条赛道分成三类。第一类是纯 GNSS 路线代表是 RTKLIB——它在 SPP、RTD、RTK、PPP 等单系统解算上是事实标准但完全不处理 IMU 和视觉遇到信号失锁就只能等。第二类是纯视觉惯性路线代表是 OKVIS、VINS-Mono、SVO它们用因子图或滤波把 IMU 与相机紧耦合到位但对 GNSS 几乎是绝缘的——要么靠粗糙的位置外参注入要么压根不接 GNSS。第三类是商业级综合方案如 Novatel、Trimble 的紧组合产品但它们是黑盒不开放代码、不可二次开发对学界几乎没有研究价值。这里要厘清的是过去也有一些把 GNSS 和视觉 SLAM 拼起来的开源工作如 VINS-Fusion 的 GPS 分支但它们大多停留在松组合层面——把 GNSS 解出来的位置当成一个绝对位姿先验喂进 VIO 系统。这种做法的天花板很低因为它彻底丢掉了 GNSS 原始观测里携带的丰富信息不同卫星的几何分布、伪距和相位的不同噪声特性、双差观测的相关性。GICI-LIB 的核心判断是只有把 GNSS 双差伪距、双差载波相位、多普勒、IMU 预积分、视觉重投影这些原始观测全部以因子的形式直接挂进同一张图才能拿到工程意义上的紧耦合。1.3 这篇博客承诺给你什么读完这篇博客你会拿到三层理解。第一层是工程视角的——GICI-LIB 这套库在 src 目录下大致是怎么组织的每个目录承担什么职责外部依赖Eigen、Ceres、OpenCV、yaml-cpp、glog各自扮演什么角色。第二层是方法论视角的——因子图优化FGO这套范式相比 EKF/UKF 等滤波方法到底新在哪里、为什么近五年所有严肃的多传感器融合工作都在转向 FGO。第三层是代码视角的——通过四段从仓库直接抽出来的真实代码看 GICI-LIB 是如何用 C 模板和继承体系把伪距残差“IMU 预积分”RTK 紧耦合估计器这些抽象映射到具体类的。2. 整体框架2.1 输入输出接口与最小核心抽象从输入侧看GICI-LIB 接受三类原始观测GNSS 的 RINEX/RTCM3/Ublox-raw/Septentrio-raw/Novatel-raw 等格式伪距、载波相位、多普勒、星历、IMU 的角速度与比力时间序列、以及相机的图像帧。从输出侧看它给出的是一条带时间戳的位姿速度偏置序列可选择写到文件、ROS topic、串口或 TCP/IP 连接。这种多输入流 多输出格式的设计让 GICI-LIB 既能在离线伪实时模式下复现实验也能在真实嵌入式平台上接入实时数据流。整套库最关键的核心抽象只有四个参数块ParameterBlock对应优化变量、残差块ErrorInterface对应观测因子、图Graph封装 Ceres Problem、估计器Estimator串联前端预处理、图管理、后端求解。所有 GNSS 残差、IMU 预积分、视觉重投影都从 ErrorInterface 派生所有位姿、速度偏置、模糊度、时钟偏差都从 ParameterBlock 派生。这种先统一抽象、再具体化的做法让任何一种新传感器、新观测类型的接入都只需要新增一对参数块残差块而不需要动核心图结构。难点提示因子图相比 EKF 到底新在哪直观理解是EKF 每来一帧观测就更新一次状态过去的所有观测都被压缩进了协方差矩阵里丢失了原始信息因子图则把每一次观测都作为一条独立的边永久挂在图上所有边一起组成一个非线性最小二乘问题由 Ceres 一次性求解。这就像 EKF 在做传话游戏每次只能记住上一个人说的话而因子图把所有人说过的话同时摊在桌上一起核对——后者明显更鲁棒特别是面对异常值剔除和延迟到达的观测时。2.2 关键不对称设计传感器估计层和因子图层的解耦GICI-LIB 在工程结构上有一个非常关键的不对称设计——传感器估计层只负责把原始观测翻译成残差块和参数块因子图优化层不知道这些残差具体来自哪种传感器。这一点反映在源码的目录组织上src/gnss/里有 30 多个 GNSS 相关文件伪距误差、载波相位双差误差、模糊度解算、电离层模型等src/imu/里有 IMU 预积分误差和约束NHC 非完整性约束、HMC 水平约束等src/vision/里有特征跟踪和视觉重投影误差但src/estimate/这个图层只有不到 20 个文件全部围绕 Ceres 的统一接口展开。这种解耦带来的工程红利非常具体。一方面调试 GNSS 模块时不用关心相机有没有初始化好另一方面给图层增加新功能如鲁棒核函数、边缘化、子图时所有传感器都同时受益。这里值得注意一点传感器估计层之间还存在松耦合-紧耦合的二级抽象——SPP/RTK 这种 GNSS 内部已解算出位置的可以走松组合接入GNSS LC 估计器未解算的原始观测走紧组合接入GNSS TC 估计器两条路径共享同一套图层但在前端处理上完全独立。3. 第一条核心机制GNSS 残差因子的统一抽象3.1 机制核心思路用模板和参数组让一个类支持四种 GNSS 形式GNSS 观测的复杂度远远超过普通用户的认知。同一个伪距测量在不同的算法语境下需要绑定的优化变量数量完全不同——单点定位SPP只需绑定接收机位置和钟差紧组合TC模式下需要绑定刚体位姿、GNSS 杆臂、钟差PPP 模式下还要额外加 IFB频间偏差、对流层延迟、电离层延迟。如果给每种组合写一个独立的残差类代码会迅速膨胀到无法维护。GICI-LIB 用 C 变参模板variadic template一次性把这个问题解决了。3.2 工程价值让代码量减少一个数量级通过模板参数Ns...把绑定哪些参数块作为编译期常量传入单个PseudorangeErrorNs...类就能在编译期实例化出四组不同的具体类型。这种设计的代码量缩减是数量级的——根据论文与代码的对照统计GICI-LIB 的 GNSS 模块用约 8000 行 C 实现了 RTKLIB 中约 25000 行 C 代码覆盖的全部解算路径单文件代码量缩减约 68%且每一个残差类都自带 Ceres 自动求导接口工程可维护性远高于纯 C 风格的全局函数实现。3.3 代码透视下面这段代码从include/gici/gnss/pseudorange_error.h抽取展示了一个类、四种参数组合是怎么用变参模板实现的注释里清楚列出了四组合法的参数块配置这意味着同一份模板代码可以在编译期为 SPP、TC、PPP、TCPPP 四种场景各自实例化出最优的具体类这种模板化抽象正是 GICI-LIB 把代码量压到 RTKLIB 三分之一的关键技巧。换句话说模板参数Ns...把绑哪些参数块翻译成了一个编译期决策运行期完全没有任何额外开销这一点在嵌入式部署里至关重要// include/gici/gnss/pseudorange_error.h// pseudorange error// The candidate parameter setups are:// Group 1: P1. receiver position in ECEF (3), P2. receiver clock (1)// Group 2: P1. body pose in ENU (7), P2. relative position from body to receiver// in body frame (3), P3. receiver clock (1)// Group 3: Group 1 P3. IFB, P4. troposphere delay (1), P5. ionosphere delay (1)// Group 4: Group 2 P4. IFB, P5. troposphere delay (1), P6. ionosphere delay (1)templateint...NsclassPseudorangeError:publicceres::SizedCostFunction1/* number of residuals */,Ns.../* parameter blocks */,publicErrorInterface{public:typedefceres::SizedCostFunction1,Ns...base_t;staticconstintkNumResiduals1;typedefEigen::Matrixdouble,1,1information_t;PseudorangeError(constGnssMeasurementmeasurement,constGnssMeasurementIndex index,constGnssErrorParametererror_parameter);// ...};这段代码做了什么、为什么这样设计、工程细节在哪——这里需要展开说。ceres::SizedCostFunction1, Ns...在 Ceres 里的语义是残差维度为 1、参数块维度按 Ns 依次展开变参模板让 Ns 可以是3, 1SPP、7, 3, 1TC、3, 1, 1, 1, 1PPP等任意组合。information_t是 1×1 的矩阵因为单个伪距是标量观测它的协方差由仰角、信噪比、卫星钟稳定度等共同决定由GnssErrorParameter在构造时根据每颗卫星的状态动态算出。ErrorInterface这一基类则让残差具备了GICI-LIB 自己的统一管理接口——上层 Graph 不需要关心具体残差类型只需要按 ErrorInterface 的 API 取雅可比和残差值即可。4. 第二条核心机制IMU 预积分因子与 NHC 软约束4.1 设计动机高频 IMU 不能每个采样点都建一个状态IMU 通常以 100~1000 Hz 高频输出角速度和比力GNSS 则只有 1~10 Hz。如果按 IMU 的频率为每个采样点建一个位姿状态因子图节点数量会爆炸。视觉惯性 SLAM 领域在 2017 年前后基本统一了做法——IMU 预积分IMU pre-integration把两个关键帧之间的所有 IMU 采样合并成一个等效的相对位姿速度偏置增量约束挂成一条边连接前后两个关键帧。这种思路最早由 Forster 等人在 2015 年系统化提出OKVIS 是工业级的开源参考实现之一GICI-LIB 的 IMU 部分正是从 OKVIS 继承并改造而来。4.2 代码透视15 维残差与四个参数块下面这段代码从include/gici/imu/imu_error.h抽取展示了 GICI-LIB 的 IMU 预积分残差类签名。它清楚地说明了一条 IMU 边为什么要绑定四个参数块——前后两个关键帧各自的位姿7 维和速度偏置9 维并且残差是 15 维位置 3 旋转 3 速度 3 陀螺偏置 3 加速度偏置 3// include/gici/imu/imu_error.h/// \brief Implements a nonlinear IMU factor.classImuError:publicceres::SizedCostFunction15/* number of residuals */,7/* size of first parameter (PoseParameterBlock k) */,9/* size of second parameter (SpeedAndBiasParameterBlock k) */,7/* size of third parameter (PoseParameterBlock k1) */,9/* size of fourth parameter (SpeedAndBiasParameterBlock k1) */,publicErrorInterface{public:typedefEigen::Matrixdouble,15,15covariance_t;typedefcovariance_t information_t;ImuError(constImuMeasurementsimu_measurements,constImuParametersimu_parameters,constdoublet_0,constdoublet_1);staticintpropagation(constImuMeasurementsimu_measurements,constImuParametersimu_params,TransformationT_WS,SpeedAndBiasspeed_and_biases,constdoublet_start,constdoublet_end,covariance_t*covariancenullptr,jacobian_t*jacobiannullptr);};这段代码的工程细节藏在propagation这个静态函数里。它是 IMU 预积分的核心数值过程——给定起止时间段内的所有 IMU 采样和起始位姿速度偏置通过中点积分法把位姿、速度、偏置和它们的协方差矩阵一起递推到终止时刻。covariance输出参数是 15×15 的因为它要捕捉所有 15 维状态量之间的相关性这一点对后端 Ceres 求解器构造信息矩阵至关重要。这种递推时一次性算出协方差残差求值时复用的设计把 IMU 边的实时计算开销压到极低水平。4.3 工程细节亮点车载场景下的非完整性约束 NHCGICI-LIB 还专门为车载场景实现了非完整性约束 NHCNon-Holonomic Constraint。它的物理含义非常直白——一辆正常行驶的车在车体坐标系下横向速度和垂直速度应该接近零车不会横向漂移、也不会脱离地面。这条约束可以作为一个伪观测挂到因子图上给 IMU 在长时间无 GNSS 信号时提供额外的姿态约束。在配置文件里通过car_motion: true开关启用并设定最小线速度阈值car_motion_min_velocity: 3.0低于这个速度的时候不启用因为停车或缓慢挪动时这条约束容易反过来污染估计。工程价值NHC 为什么对开源框架特别重要商业级紧组合产品通常默认带这种类型的运动模型约束但开源社区里很多 VIO 系统并不区分车载和无人机场景。GICI-LIB 显式地把car_motion提供给用户意味着同一套代码既可以做无人机定位关闭 NHC也可以做车载定位开启 NHC这种明示的开关比隐式的假设在工程上要友好得多。5. 训练时的监督模块异常剔除、模糊度解算与初始化5.1 监督目标的设计取舍为什么鲁棒性比精度更值钱GICI-LIB 实际上没有训练阶段它是基于优化的、非学习的库这里的训练时模块对应的是离线初始化与运行时的鲁棒化机制。在多传感器融合系统里最容易被低估的难题不是精度而是当某一种传感器出现异常值时整个系统能不能稳住。GNSS 的多路径、IMU 的瞬时震动、相机的运动模糊或局部过曝都会在某些时刻产生与真实状态完全不符的观测。如果这些异常值直接进了 Ceres 的最小二乘求解整条优化轨迹都会被拉偏且越是紧耦合越严重。工程选择三件套上GICI-LIB 采取了一个组合策略——鲁棒核函数 残差先验门限 模糊度部分固定。鲁棒核函数Huber、Cauchy对大残差自动降权残差先验门限通过max_pesudorange_error: 4.0、max_phaserange_error: 0.06、max_doppler_error: 0.5在前端把明显异常的观测直接剔除模糊度部分固定则是 RTK 路径上的核心技术允许在整组模糊度不能全部固定为整数时仍然把高置信度的子集固定下来。这种前端粗筛 后端鲁棒的双层防御是 GICI-LIB 在密集城市场景下相比单一 RTK 工具有明显鲁棒性优势的根本原因。5.3 初始化的可观测性公式动态运动为什么必要GNSS/IMU 初始化的可观测性问题在数学上可以写得很直白。设待估状态为x [ R , p , v , b g , b a ] ⊤ \mathbf{x} [\mathbf{R}, \mathbf{p}, \mathbf{v}, \mathbf{b}_g, \mathbf{b}_a]^\topx[R,p,v,bg​,ba​]⊤则在静止状态下 IMU 加速度计读数a meas R ⊤ g b a \mathbf{a}_{\text{meas}} \mathbf{R}^\top \mathbf{g} \mathbf{b}_aameas​R⊤gba​中R \mathbf{R}R与b a \mathbf{b}_aba​完全耦合无法分离。只有当智能体进入加速运动后加速度计读数变成a meas R ⊤ ( g v ˙ W ) b a \mathbf{a}_{\text{meas}} \mathbf{R}^\top (\mathbf{g} \dot{\mathbf{v}}_W) \mathbf{b}_aameas​R⊤(gv˙W​)ba​其中v ˙ W \dot{\mathbf{v}}_Wv˙W​是世界坐标系下的真实加速度必须通过 GNSS 多普勒或位置差分单独可测才能让R \mathbf{R}R和b a \mathbf{b}_aba​在数学上被分离。这就是min_acceleration: 0.5这个阈值的物理来源——加速度低于 0.5 m/s² 时外部观测信噪比不足以分离这两个状态量强行进入紧耦合优化会得到一组数值上看似收敛、但实际带有姿态偏置的伪解。换句话说这个阈值不是经验拍脑袋拍出来的而是从可观测性矩阵的秩条件直接推导出来的工程下限。5.2 GNSS/IMU 初始化的关键问题紧组合定位最大的工程麻烦不在稳态求解而在冷启动。系统刚开机时IMU 的角速度偏置、加速度偏置、初始姿态都是未知的GNSS 杆臂接收机天线到 IMU 中心的偏移虽然有标定值但精度也只有厘米级坐标系的对齐车体坐标系→ IMU 坐标系→ ENU 局部坐标系→ ECEF 全局坐标系需要至少一段动态运动才能可靠求解。GICI-LIB 的初始化器在配置文件里给了两组时间窗口——time_window_length_slow_motion: 0.05慢速运动下的短时间窗和time_window_length_dynamic_motion: 0.5动态运动下的长时间窗并通过最小加速度阈值min_acceleration: 0.5判断是否已经进入了可观测的动态状态。// src/fusion/rtk_imu_camera_rrr_estimator.cpp 节选boolRtkImuCameraRrrEstimator::addMeasurement(constEstimatorDataClustermeasurement){// GNSS/IMU initializationif(coordinate_nullptr||!gravity_setted_)returnfalse;if(!gnss_imu_initializer_-finished()){if(gnss_imu_initializer_-getCoordinate()nullptr){gnss_imu_initializer_-setCoordinate(coordinate_);initializer_sub_estimator_-setCoordinate(coordinate_);gnss_imu_initializer_-setGravity(imu_base_options_.imu_parameters.g);}if(gnss_imu_initializer_-addMeasurement(measurement)){gnss_imu_initializer_-estimate();setInitializationResult(gnss_imu_initializer_);}returnfalse;}// Add IMU / GNSS / Image dispatchif(measurement.imumeasurement.imu_roleImuRole::Major)addImuMeasurement(*measurement.imu);if(measurement.gnss){GnssMeasurement rov,ref;meausrement_align_.add(measurement);if(meausrement_align_.get(rtk_options_.max_age,rov,ref))returnaddGnssMeasurementAndState(rov,ref);}if(measurement.frame_bundle){if(!visual_initialized_)returnvisualInitialization(measurement.frame_bundle);returnaddImageMeasurementAndState(measurement.frame_bundle);}returnfalse;}这段从 RTK/IMU/Camera 紧耦合估计器抽出的addMeasurement是整个 GICI-LIB 数据流的关键路口。它干了三件事——第一是初始化守门未完成初始化时所有观测都不会进入主图而是先喂给独立的初始化子估计器第二是按观测类型分发IMU 走预积分通路、GNSS 走双差对齐再进图、相机走视觉初始化或重投影通路第三是 GNSS 的流对齐因为差分定位需要把流动站和基站观测按时间戳严格对齐meausrement_align_.get(max_age, rov, ref)这一句保证了只有两边都到齐且时延在max_age之内时才允许 GNSS 因子进入图。这种延迟入图、保证对齐的设计是开源 RTK 实现里非常容易踩坑的地方GICI-LIB 把它显式封装成了一个状态机。6. 推理时的执行模块滑窗优化与边缘化…详情请参照古月居