F-LOAM——快速的激光里程计和建图框架总结

F-LOAM相比与LOAM的区别：

1. 两次过程去畸变；2.利用Ceres库做scan-submap优化。

一、去畸变

1. 因为好多激光频率超过10Hz，所以帧间的时间间隔很小，所以在间隔内假设是匀速度和匀角速度模型。第一步用常速模型进行去畸变和做预测。

2. 经过位姿解算后，利用新的位姿重新对去畸变的特征？（原始特征）再次去畸变。两步运算的好处是，相比一步定位精度相似，但是运算量小了很多。

3. 注：原本LOAM在里程计部分对一帧点云利用里程计计算的Tk,L+1和匀速模型，计算出每个时间戳下点云所处的变换矩阵Tk,L+1，在优化函数中迭代优化。本文中用匀速模型去畸变后进行帧间匹配，最后在用匹配的结果重新进行去畸变。

二、Ceres优化

点，面特征根据曲率大小计算权重。分子是当前特征的，分母是所有特征。

利用Ceres库对误差函数进行优化，传入四元数；而LOAM中是手动计算误差量对欧拉角的导数。

  void addCornerConstraint(const Eigen::Vector3d& curr_point,
                           const Eigen::Vector3d& point_a,
                           const Eigen::Vector3d& point_b) {
    std::unique_lock<std::mutex> lock(mutex_);
    ceres::CostFunction* cost_function =
        new EdgeAnalyticCostFunction(curr_point, point_a, point_b);
    problem_.AddResidualBlock(cost_function, loss_function_, params_);
    ++constraint_num_;
  }

 对误差函数进行重写

class EdgeAnalyticCostFunction : public ceres::SizedCostFunction<1, 7> {
 public:
  EdgeAnalyticCostFunction(Eigen::Vector3d curr_point_,
                           Eigen::Vector3d last_point_a_,
                           Eigen::Vector3d last_point_b_)
      : curr_point(curr_point_),
        last_point_a(last_point_a_),
        last_point_b(last_point_b_) {}
  // 手动赋雅克比使用Evaluate函数
  virtual bool Evaluate(double const* const* parameters, double* residuals,
                        double** jacobians) const {
    Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);
    Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[0] + 4);
    Eigen::Vector3d lp;
    lp = q_last_curr * curr_point + t_last_curr;

    Eigen::Vector3d nu = (lp - last_point_a).cross(lp - last_point_b);
    Eigen::Vector3d de = last_point_a - last_point_b;
    double de_norm = de.norm();
    residuals[0] = nu.norm() / de_norm;

    if (jacobians != NULL) {
      if (jacobians[0] != NULL) {
        Eigen::Matrix3d skew_lp = skew(lp);
        Eigen::Matrix<double, 3, 6> dp_by_se3;
        dp_by_se3.block<3, 3>(0, 0) = -skew_lp;
        (dp_by_se3.block<3, 3>(0, 3)).setIdentity();
        Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor>> J_se3(
            jacobians[0]);
        J_se3.setZero();
        Eigen::Matrix3d skew_de = skew(de);
        J_se3.block<1, 6>(0, 0) =
            -nu.transpose() / nu.norm() * skew_de * dp_by_se3 / de_norm;
      }
    }
    return true;
  }

 private:
  Eigen::Vector3d curr_point;
  Eigen::Vector3d last_point_a;
  Eigen::Vector3d last_point_b;
};

三、实验效果

经过使用ceres优化后，相比于LOAM时间代价降低大约20%。