位姿协方差换坐标系：Adjoint 不能只旋转对角块

定位模块给出 base_link 位姿协方差，融合节点却要在雷达坐标系里使用。我以前见过一种很顺手的写法：平移块和旋转块分别乘 $R Σ R^{⊤}$ ，交叉块原样保留。传感器与机体原点重合时它看不出问题；只要有杠杆臂，转角误差就会变成位置误差，这种写法会系统性低估平移不确定性。

先固定本文约定： $T_{A B}$ 把 $B$ 系坐标变到 $A$ 系，

p_{A} = R_{A B} p_{B} + t_{A B}

扰动采用左乘，切向量按 Sophus 的顺序写成 $ξ = [υ^{⊤}, ω^{⊤}]^{⊤}$ ，即平移在前、旋转在后。此时有

Ad T ξ = T ξ^T^{- 1}, Ad T = [R [t]_{\times} R \0 R]

所以把 $B$ 系中的小扰动协方差搬到 $A$ 系，应写成

Σ_{A} = Ad T A B Σ_{B} Ad T A B^{⊤}

右上角的 $[t]_{\times} R$ 就是杠杆臂项。假设传感器在机体 $x$ 轴前方 $1 m$ ，只有绕 $z$ 轴的标准差 $0.1 rad$ ，那么换到机体原点后， $y$ 向位置也会出现约 $0.1 m$ 的一阶标准差。只转两个 $3 \times 3$ 对角块会把它完全漏掉。

在 Sophus 里直接做

Sophus::SE3d::Adj() 的定义正好对应上面的平移优先顺序：

cpp

using Matrix6d = Eigen::Matrix<double, 6, 6>;

Matrix6d transform_covariance(
    const Sophus::SE3d& T_AB,
    const Matrix6d& Sigma_B) {
  const Matrix6d Ad = T_AB.Adj();
  Matrix6d Sigma_A = Ad * Sigma_B * Ad.transpose();
  return 0.5 * (Sigma_A + Sigma_A.transpose());
}

最后一行只清理浮点运算造成的轻微非对称，不会把一个本来非半正定的矩阵“修好”。如果最小特征值明显为负，应该回头查协方差来源、单位和坐标约定。

一个能抓住漏项的单测

下面只给 $B$ 系的 yaw 扰动方差，并设置 $1 m$ 杠杆臂。按 Sophus 顺序，yaw 是索引 5， $y$ 平移是索引 1：

cpp

Matrix6d Sigma_B = Matrix6d::Zero();
Sigma_B(5, 5) = 0.01;  // (0.1 rad)^2

Sophus::SE3d T_AB(
    Sophus::SO3d(),
    Eigen::Vector3d(1.0, 0.0, 0.0));
const Matrix6d Sigma_A = transform_covariance(T_AB, Sigma_B);

assert(std::abs(Sigma_A(1, 1) - 0.01) < 1e-12);
assert(std::abs(Sigma_A(5, 5) - 0.01) < 1e-12);
assert((Sigma_A - Sigma_A.transpose()).norm() < 1e-12);

把 T_AB 误写成 T_BA，或者把切向量顺序当成旋转在前，这个测试会立即失败。它比只测单位变换有用得多。

边界不在矩阵乘法里

左扰动、右扰动对应的协方差不是同一个对象；有的库把 twist 排成 $[ω, υ]$ ，伴随矩阵的块位置也会随之改变。接入 GTSAM、Sophus 或自研状态量时，我会先写下 T 的方向、扰动在左还是在右、六维向量顺序，再做转换。

这套传播还是局部线性近似。姿态分布很宽、接近 $π$ 奇异区，或者协方差其实来自欧拉角参数时，直接套 $6 \times 6$ 伴随矩阵并不可靠。另一个常见错误是只保留对角线：SLAM 后端里的平移—旋转交叉项往往正是传感器几何产生的，删掉后融合器会得到过分自信的权重。

我最后会做两层检查：先跑上面的解析单测，再从 $N (0, Σ_{B})$ 采样小扰动，经 T * Exp(xi) * T.inverse() 搬运后比较样本协方差。小噪声下两者应贴合；噪声增大后开始偏离，正好也能看出一阶近似的适用范围。

在 Sophus 里直接做

一个能抓住漏项的单测

边界不在矩阵乘法里

也可以看看