定位通常在已有地图中估计机器人状态；SLAM同时估计轨迹与地图。测量的位置依赖机器人当时在哪，机器人在哪又要靠地图中的观测约束，两者因此耦合。里程计长期积累的误差，需要新的独立约束才能纠正。

地图并不只有一种

占据栅格保存每个单元是否被障碍占据的信念，适合导航，但分辨率越细，计算与存储成本越大。常以log-odds累加测量证据：L=log(p/(1-p))。在先验0.5、假设观测条件独立的简化模型下，一次对应占据概率0.7的证据增加约0.847；重复三次得到约0.927的占据概率。若三次其实来自同一条相关观测，这样累加会过度自信。实际系统还需上下界和动态物体策略。

射线在击中点之前提供空闲证据，在末端提供占据证据；未知区域与空闲区域必须区分。距离最大值、无效反射和传感器盲区要按设备含义处理，不能全都当作清空障碍。

地标地图保存点、线或其他特征；稠密/半稠密地图记录表面。导航所需的可通行性与三维重建的表面精度并非同一个目标。建图系统还可能另外输出位姿图，供全局一致性优化。SLAM建模见Georgia Tech的SLAM章节。

因子图把什么连接起来

变量节点可以是关键帧位姿、地标或偏置，因子表示里程计、观测、先验或回环残差。优化目标通常把残差按测量协方差加权。协方差越小，权重越大；把不可靠回环设得非常确定，会把整张地图拉错。

前端负责提取、匹配、数据关联与局部估计，后端负责融合这些约束。后端求解器收敛，并不能证明前端对应正确。回环候选还需要几何验证、时间与外观检查，以及对错误约束的防护。

没有锚定参考坐标时，整组位姿一起平移或旋转，许多相对残差仍不变，这叫规范自由度。需要固定一个参考位姿或加入适当先验，才能得到唯一坐标表示。单目视觉还可能涉及尺度自由度。

一维回环实验

pose_graph_1d.py用七个位姿表示“前进三步再返回原点”。每次里程计都加0.12米偏差，最后误差累计到0.72米。脚本用NumPy最小二乘求解，再加入“末位姿与起点相同”的带噪回环约束：

python pose_graph_1d.py

2026-10-24在Python 3.12.14、NumPy 2.3.5执行，未锚定时七个变量的矩阵秩为六；加入锚定后，回环前RMSE约0.4327米，回环后约0.0173米，终点仍保留约0.0288米误差。回环有有限权重，所以并非把终点强制改成零。原始结果可用于重算。

这个实验只验证一维线性位姿图，没有图像、激光、数据关联、非线性旋转或真实SLAM系统。练习：把回环测量故意改为1米，观察错误约束如何影响整个轨迹；再改变回环标准差，解释为何结果改变。面试自查：为什么全局优化后的地图校正可能使map→odom变化，而局部控制仍希望odom连续？