【基于gtsam的残差构建和位姿优化作业】

目录

文章目录

前言

一、作业要求

二、利用gtsam完成点面残差优化作业

总结
 

---

前言

这是视觉工坊的苏赟老师的LVI-SAM下的课程作业讲解记录，方便以后复习。仅供学习交流，侵删。作业是老师改的代码框架然后挖空的形式，挺用心的，有需要作业说明和代码包的评论下我看到了会私发bdu云链接，就不在这里发出来了，感觉不太好。

一、作业要求

要求：基于所给的SLAM代码框架，完成基于gtsam的残差构建和位姿优化；
目的：加深对SLAM残差构建与位姿优化的理解，以及熟悉对非线性优化库的使用；

说明：
1.给出的代码中已完成ceres版本的激光SLAM，可以通过aloam_ceres.launch文件直接运行；
2.基于gtsam版本的激光SLAM已完成laserOdometryGtsam节点，laserMappingGtsam.cpp文件中也已完成角点因子的构建，可以直接运行，但是结果不正确；
3.仿照已完成的角点因子的构建过程，需要完善lidarFactorGtsam.hpp文件中面点残差和雅克比的计算(作业_1)，laserMappingGtsam.cpp文件中的面点因子的构建(作业_2)，并进行位姿优化(作业_3)，需要添加代码的部分已经通过作业_123标记出来，只需要在start和end之间添加代码即可；

二、利用gtsam完成点面残差优化作业

本次作业前提需要理清：激光雷达提取线面之间的几何关系，以及构建残差时的导数关系。

作业part 1是利用gtsam计算点面残差，并计算雅克比矩阵。线面特征的残差构建需要输入的变量：当前需要优化的pose的id，当前的激光点，平面的法向量、一些其他参数和噪声。噪声是协方差，也就是权重，这里噪声没有用到，因为是纯激光。把当前点根据pose转到世界坐标系下，将转换到世界坐标系下的点根据观测模型构建残差，即根据法向量和一些平面参数投影过去计算点到面的距离，也就是残差，最后希望点和面重合，即残差为0。需要的是残差关于pose的导数，根据链式求导法则可分为面对点的导数和点对pose的导数相乘，作业部分实现如下：

gtsam::Vector evaluateError(const Pose3 &pose, boost::optional<Matrix &> H1 = boost::none) const
  {
    gtsam::Vector1 residual;
    // 作业_1：
    // TODO: 利用gtsam计算点面残差，并计算雅克比；（仿照LidarPose3PlaneFactor）
    // start_1:

    //当前激光的面点
    gtsam::Point3 cp(curr_point_(0),curr_point_(1),curr_point_(2));
    //当前激光面点的法向量
    gtsam::Point3 norm(plane_unit_norm_(0),plane_unit_norm_(1),plane_unit_norm_(2));

    //当前激光点根据传入的pose转换到世界坐标系下为point_w，
    gtsam::Matrix36 Dpose;
    gtsam::Point3 point_w = pose.transform_from(cp,H1?&Dpose:0);
    //将世界坐标系下的点根据法向量和参数投影过去，计算点到面的距离，也就是残差，希望其为0
    gtsam::Matrix13 Dnorm,Dpoint;
    residual(0) = norm.dot(point_w,Dnorm,Dpoint) + negative_OA_dot_norm_;

    //如果我们需要雅可比矩阵 所以H1是雅可比矩阵？
    if(H1)
    {
      H1->resize(1,6);
      *H1 << Dpoint * Dpose;
    }
    // end_1

    return residual;
  }

构建完点面残差后，要讲改因子加入图优化模型中，作业part 2是利用gtsam构建点面约束因子，并添加到graph中，代码实现如下：

if (planeValid)
              {
                // 作业_2：
                // TODO: 利用gtsam构建点面约束因子，并添加到graph中；（仿照laserOdometryGtsam.cpp）
                // start_2:

                //首先给出一个噪声模型，也就是协方差矩阵
                gtsam::noiseModel::Diagonal::shared_ptr surf_gaussian_model = gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(1) << 1.0).finished());
                gtsam::noiseModel::Robust::shared_ptr surf_noise_model = gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::Huber::Create(0.1), edge_gaussian_model);
                //根据刚才factor的结构形式输入，0是pose id，因为只有一个优化变量激光雷达相对于世界坐标系下的pose
                //所以优化目标id为0即可+当前观测到的点+平面法向量+一些参数+噪声模型（没有用到）
                gtsam::LidarPose3EdgeFactor factor(0, curr_point, norm, negative_OA_dot_norm, surf_noise_model);
                //将面特征factor push到graph中进行优化
                graph->push_back(factor);

                // end_2

                surf_num++;
              }

将构建的factor加入graph后要用优化算法进行优化，优化完成后将结果（pose)拿出 可仿照里程计节点进行，作业part 3要利用gtsam进行位姿优化，实现如下：

// 作业_3：
          // TODO: 利用gtsam进行位姿优化；（仿照laserOdometryGtsam.cpp）
          // start_3:
          //构建完graph后要用优化算法进行优化，优化完成后将结果（pose)拿出 可仿照里程计节点
          //构建优化器参数对象,设置参数优化方式、停止阈值等
          gtsam::LevenbergMarquardtParams optimizer_params;
          optimizer_params.setLinearSolverType("MULTIFRONTAL_QR");
          optimizer_params.setRelativeErrorTol(1e-4);

          //优化算法选LM算法，将因子图，初始估计(可选匀速模型或者放入上一帧数据)，优化参数放入
          gtsam::LevenbergMarquardtOptimizer optimizer(*graph,initial_estimate,optimizer_params);
          //返回优化结果
          gtsam::Values result = optimizer.optimize();
          //只有一个优化变量，返回第一个（下标0） 里程计节点优化的是前后帧的运动增量，
          //mapping优化的当前激光相对于世界坐标系下的pose，是绝对值
          T_w_curr = result.at<gtsam::Pose>();
          
          // T_w_curr = result.at<gtsam::Pose3>(0);

          // end_3

          t_solve_twice += t_solver.toc();

---

总结

提示：这里对文章进行总结：
例如：以上就是今天要讲的内容，本文仅仅简单介绍了pandas的使用，而pandas提供了大量能使我们快速便捷地处理数据的函数和方法。