本节是相对位姿图优化，最后一篇实战篇，将之前理论讲解用代码形式展现，此处只展现了左扰动跟右扰动，至于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方向飘移问题；