SLAM中的非线性优-3D图优化之相对位姿Between Factor位姿图优化（十三）

本节是相对位姿图优化，最后一篇实战篇，将之前理论讲解用代码形式展现，此处只展现了左扰动跟右扰动，至于SO3/t与四元数的形式，还没有完全调试好，并且利用G2O作为优化器，ceres-sover优化也作了实现，但是效果不太稳定，时好时坏，待后续升级，G2O版本的左右扰动较为稳定，接下来看实际效果

完整代码依然存储在码云上https://gitee.com/zl_vslam/slam_optimizer/blob/master/3d_optimize/apps/main_ch13.cpp

一.效果展示

如上图所示，仿真生成1km的矩形路径，包含了帧间边跟回环边，然后利用之前左右雅克比的推导，实现位姿图优化，效果如下

上图中绿色为真值，红色为里程计，蓝色为优化后的位姿，紫色为回环约束，左右雅克比去掉真值显示如下

可以看到经过位姿图优化后被重新拉成了环型；

二.代码展示

复制代码

    void computeJacobianLeftDisturbance(Matrix6d& jacobian_i, Matrix6d& jacobian_j, const Sophus::SE3d& v1, Sophus::SE3d& v2) {
        Matrix6d J = JRInv(SE3d::exp(_error));
        jacobian_i = -J*v2.inverse().Adj();
        jacobian_j = J*v2.inverse().Adj();
    }

    void computeJacobianRightDisturbance(Matrix6d& jacobian_i, Matrix6d& jacobian_j, const Sophus::SE3d& v1, Sophus::SE3d& v2) {
        Matrix6d J = Matrix6d::Identity();
        // J = JRInv(SE3d::exp(_error));
        jacobian_i = -J* (v2.inverse()*v1).Adj();
        jacobian_j = J * Matrix6d::Identity();
    }

    void computeJacobianRightDisturbanceGTSAM(Matrix6d& jacobian_i, Matrix6d& jacobian_j, const Sophus::SE3d& v1, Sophus::SE3d& v2) {
        // 转换为 GTSAM Pose3
        gtsam::Pose3 gtsam_pose_i(gtsam::Rot3(v1.rotationMatrix()), 
                            gtsam::Point3(v1.translation()));
        gtsam::Pose3 gtsam_pose_j(gtsam::Rot3(v2.rotationMatrix()), 
                            gtsam::Point3(v2.translation()));
        gtsam::Pose3 measured(gtsam::Rot3(_measurement.rotationMatrix()), 
                            gtsam::Point3(_measurement.translation()));
    
        // 创建 between factor（这里噪声模型不重要，因为我们只调用 evaluateError）
        auto model = gtsam::noiseModel::Isotropic::Sigma(6, 1.0);
        gtsam::BetweenFactor<gtsam::Pose3> factor(
            (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))->id(), 
            (static_cast<VertexSE3LieAlgebra*> (_vertices[1]))->id(), 
            measured, 
            model);
    
        // 直接调用 evaluateError 计算残差和雅可比
        gtsam::Matrix gtsam_jacobian_i, gtsam_jacobian_j;
        gtsam::Matrix gtsam_residual;
    
        boost::optional<gtsam::Matrix&> H1(gtsam_jacobian_i);
        boost::optional<gtsam::Matrix&> H2(gtsam_jacobian_j);
        // 调用 evaluateError
        gtsam_residual = factor.evaluateError(gtsam_pose_i, gtsam_pose_j, H1, H2);

        /////////////////////////////////////////////////
        /////////////////////////////////////////////////
        /////////////////////////////////////////////////
        /////////////////////////////////////////////////
        std::cout << " gtsam_jacobian_i : " << (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))->id() << " \n" 
                  << gtsam_jacobian_i << std::endl;
        std::cout << " gtsam_jacobian_j : " << (static_cast<VertexSE3LieAlgebra*> (_vertices[1]))->id() << " \n" 
                  << gtsam_jacobian_j << std::endl;
        std::cout << " gtsam_residual : " << gtsam_residual.transpose() << std::endl;


        Matrix6d J = Matrix6d::Identity();
        // J = JRInv(SE3d::exp(_error));
        std::cout << " jacobian_i : " << (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))->id() << " \n" 
                  << -J* (v2.inverse()*v1).Adj() << std::endl;
        std::cout << " jacobian_j : " << (static_cast<VertexSE3LieAlgebra*> (_vertices[1]))->id() << " \n" 
                  << J * Matrix6d::Identity() << std::endl;
        std::cout << " residual : " << _error.transpose() << std::endl;

        /////////////////////////////////////////////////
        /////////////////////////////////////////////////
        /////////////////////////////////////////////////
        /////////////////////////////////////////////////

        jacobian_i = gtsam_jacobian_i;
        jacobian_j = gtsam_jacobian_j;


        // 交换左上角 3x3 块和右下角 3x3 块
        Eigen::Matrix3d temp_i_top = jacobian_i.block<3, 3>(0, 3);  // 右上角
        Eigen::Matrix3d temp_i_bottom = jacobian_i.block<3, 3>(3, 0);  // 左下角

        // 交换
        jacobian_i.block<3, 3>(0, 3) = temp_i_bottom;
        jacobian_i.block<3, 3>(3, 0) = temp_i_top;

        // 对 jacobian_j 做同样的操作
        Eigen::Matrix3d temp_j_top = jacobian_j.block<3, 3>(0, 3);
        Eigen::Matrix3d temp_j_bottom = jacobian_j.block<3, 3>(3, 0);

        jacobian_j.block<3, 3>(0, 3) = temp_j_bottom;
        jacobian_j.block<3, 3>(3, 0) = temp_j_top;

        std::cout << " final gtsam_jacobian_i : " << (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))->id() << " \n" 
                  << jacobian_i << std::endl;
        std::cout << " final gtsam_jacobian_j : " << (static_cast<VertexSE3LieAlgebra*> (_vertices[1]))->id() << " \n" 
                  << jacobian_j << std::endl;
    }

完整代码请自行看代码吧，上述代码就是左右雅可比

总结

本节展示了完整版位姿图优化效果，用实践证明了理论的正确性，其中G2O优化效果较好,因为其对位姿的表示天然支持SE3,而ceres-solver效果较差，其对四元数支持较好，此处暂时并未实现，本节博客后期会持续更新修复这些问题，修复ceres-solver优化问题，相对位姿雅克比已经花费较多时间，下一讲讲解，位姿图中的平面约束，用以解决地面机器人Z方向飘移问题；