左扰动与右更新
cpp// 计算雅可比矩阵 J // 在右乘扰动模型下误差 e 对相机位姿 (R, t) 的导数 Matrixdouble, 3, 6 J; // 对旋转的雅可比: J_R - [R * p]_× J.block3,3(0,0) -skew(R * p); // 左扰动形式的雅可比 // 对平移的雅可比: J_t I J.block3,3(0,3) Matrix3d::Identity(); // 构建并求解增量方程 (H lambda * I) * dx b Vector6d dx (H lambda * I).ldlt().solve(b); // 提取旋转增量和平移增量 Vector3d dR dx.segment3(0); Vector3d dt dx.segment3(3); // ★ 关键右乘更新旋转矩阵 R R * Sophus::SO3d::exp(dR).matrix(); // 右乘 t t dt; // 平移直接加这段代码存在一个常见的一致性错误雅可比矩阵的形式与位姿更新方式不匹配。问题在哪雅可比 J 的旋转部分cppJ.block3,3(0,0) -skew(R * p); // 左扰动形式的雅可比这是左扰动模型下的推导结果。推导对旋转施加左扰动 ΔR⋅R误差 e(Rpt)−q 的导数为 −⌊Rp⌋×。位姿更新部分cppR R * Sophus::SO3d::exp(dR).matrix(); // 右乘这里却用了右乘更新它对应的是右扰动模型R←R⋅exp(dR)。后果求解出的增量dx是基于左扰动假设得到的却用右乘去更新会导致优化方向错误甚至无法收敛。两种正确的修正方案方案一统一使用左扰动推荐与代码里的 J 一致保持雅可比不变更新改为左乘cpp// 雅可比不变 J.block3,3(0,0) -skew(R * p); // 左扰动 J.block3,3(0,3) Matrix3d::Identity(); // 求解 dx 不变 Vector6d dx (H lambda * I).ldlt().solve(b); Vector3d dR dx.segment3(0); Vector3d dt dx.segment3(3); // ★ 改成左乘更新 R Sophus::SO3d::exp(dR).matrix() * R; // 左乘 t t dt;方案二统一使用右扰动保持你原来的右乘更新雅可比必须推导为右扰动形式即 JR−R⋅⌊p⌋×JR−R⋅⌊p⌋×cpp// 右扰动形式的雅可比 J.block3,3(0,0) -R * skew(p); // 注意这里是 R * skew(p)不是 skew(R*p) J.block3,3(0,3) Matrix3d::Identity(); Vector6d dx (H lambda * I).ldlt().solve(b); Vector3d dR dx.segment3(0); Vector3d dt dx.segment3(3); // 右乘更新与右扰动雅可比匹配 R R * Sophus::SO3d::exp(dR).matrix(); t t dt;为什么必须匹配增量方程求解出的dR本质上是所选扰动模型下的李代数增量。左扰动模型ΔRexp(dR) 作用为 R←exp(dR)⋅R右扰动模型ΔRexp(dR) 作用为 R←R⋅exp(dR)如果混用相当于用“苹果”的度量去修正“橘子”结果必然错误。
