Cartographer源码阅读3D-后端优化-SPA

和2D基本一致，区别：

（1）第一个submap只优化yaw角，不优化平移；

（2）可选是否优化z轴，如果优化，则加入IMU相关的残差：线加速度和角速度；如果不优化，则和2D类似，加入轮速计残差和LocalSLAM的sequence边。

struct NodeSpec3D {
  common::Time time;
  transform::Rigid3d local_pose;
  transform::Rigid3d global_pose;
};

struct SubmapSpec3D {
  transform::Rigid3d global_pose;
};

struct TrajectoryData {
    double gravity_constant = 9.8;
    // imu轨迹的四元数
    std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};
    common::optional<transform::Rigid3d> fixed_frame_origin_in_map;
  };

class OptimizationProblem3D
    : public OptimizationProblemInterface<NodeSpec3D, SubmapSpec3D,
                                          transform::Rigid3d> {
 public:
  explicit OptimizationProblem3D(
      const optimization::proto::OptimizationProblemOptions& options);
  ~OptimizationProblem3D();

  OptimizationProblem3D(const OptimizationProblem3D&) = delete;
  OptimizationProblem3D& operator=(const OptimizationProblem3D&) = delete;

  void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override;
  void AddOdometryData(int trajectory_id,
                       const sensor::OdometryData& odometry_data) override;
  void AddTrajectoryNode(int trajectory_id,
                         const NodeSpec3D& node_data) override;
  void InsertTrajectoryNode(const NodeId& node_id,
                            const NodeSpec3D& node_data) override;
  void TrimTrajectoryNode(const NodeId& node_id) override;
  void AddSubmap(int trajectory_id,
                 const transform::Rigid3d& global_submap_pose) override;
  void InsertSubmap(const SubmapId& submap_id,
                    const transform::Rigid3d& global_submap_pose) override;
  void TrimSubmap(const SubmapId& submap_id) override;
  void SetMaxNumIterations(int32 max_num_iterations) override;

  void Solve(
      const std::vector<Constraint>& constraints,
      const std::set<int>& frozen_trajectories,
      const std::map<std::string, LandmarkNode>& landmark_nodes) override;

  const MapById<NodeId, NodeSpec3D>& node_data() const override {
    return node_data_;
  }
  const MapById<SubmapId, SubmapSpec3D>& submap_data() const override {
    return submap_data_;
  }
  const std::map<std::string, transform::Rigid3d>& landmark_data()
      const override {
    return landmark_data_;
  }
  const sensor::MapByTime<sensor::ImuData>& imu_data() const override {
    return imu_data_;
  }
  const sensor::MapByTime<sensor::OdometryData>& odometry_data()
      const override {
    return odometry_data_;
  }

  void AddFixedFramePoseData(
      int trajectory_id,
      const sensor::FixedFramePoseData& fixed_frame_pose_data);
  void SetTrajectoryData(
      int trajectory_id,
      const PoseGraphInterface::TrajectoryData& trajectory_data);
  const sensor::MapByTime<sensor::FixedFramePoseData>& fixed_frame_pose_data()
      const {
    return fixed_frame_pose_data_;
  }
  const std::map<int, PoseGraphInterface::TrajectoryData>& trajectory_data()
      const {
    return trajectory_data_;
  }

 private:
  // Computes the relative pose between two nodes based on odometry data.
  std::unique_ptr<transform::Rigid3d> CalculateOdometryBetweenNodes(
      int trajectory_id, const NodeSpec3D& first_node_data,
      const NodeSpec3D& second_node_data) const;

  optimization::proto::OptimizationProblemOptions options_;
  MapById<NodeId, NodeSpec3D> node_data_;
  MapById<SubmapId, SubmapSpec3D> submap_data_;
  std::map<std::string, transform::Rigid3d> landmark_data_;
  sensor::MapByTime<sensor::ImuData> imu_data_;
  sensor::MapByTime<sensor::OdometryData> odometry_data_;
  sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_;
  std::map<int, PoseGraphInterface::TrajectoryData> trajectory_data_;
};
// 入口函数
void OptimizationProblem3D::Solve(
    const std::vector<Constraint>& constraints,
    const std::set<int>& frozen_trajectories,
    const std::map<std::string, LandmarkNode>& landmark_nodes) {
  if (node_data_.empty()) {
    // Nothing to optimize.
    return;
  }

  ceres::Problem::Options problem_options;
  ceres::Problem problem(problem_options);

  const auto translation_parameterization =
      [this]() -> std::unique_ptr<ceres::LocalParameterization> {
    return options_.fix_z_in_3d()
               ? common::make_unique<ceres::SubsetParameterization>(
                     3, std::vector<int>{2})
               : nullptr;
  };

  // Set the starting point.
  CHECK(!submap_data_.empty());
  MapById<SubmapId, CeresPose> C_submaps;
  MapById<NodeId, CeresPose> C_nodes;
  std::map<std::string, CeresPose> C_landmarks;
  bool first_submap = true;
  bool freeze_landmarks = !frozen_trajectories.empty();
  for (const auto& submap_id_data : submap_data_) {
    const bool frozen =
        frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
    if (first_submap) {
      first_submap = false;
      // Fix the first submap of the first trajectory except for allowing
      // gravity alignment.
      // 第一个submap只优化yaw角，这个是和2D有差别的
      C_submaps.Insert(
          submap_id_data.id,
          CeresPose(submap_id_data.data.global_pose,
                    translation_parameterization(),
                    common::make_unique<ceres::AutoDiffLocalParameterization<
                        ConstantYawQuaternionPlus, 4, 2>>(),
                    &problem));
      // 不优化平移
      problem.SetParameterBlockConstant(
          C_submaps.at(submap_id_data.id).translation());
    } else {
      C_submaps.Insert(
          submap_id_data.id,
          CeresPose(submap_id_data.data.global_pose,
                    translation_parameterization(),
                    common::make_unique<ceres::QuaternionParameterization>(),
                    &problem));
    }
    if (frozen) {
      problem.SetParameterBlockConstant(
          C_submaps.at(submap_id_data.id).rotation());
      problem.SetParameterBlockConstant(
          C_submaps.at(submap_id_data.id).translation());
    }
  }
  for (const auto& node_id_data : node_data_) {
    const bool frozen =
        frozen_trajectories.count(node_id_data.id.trajectory_id) != 0;
    C_nodes.Insert(
        node_id_data.id,
        CeresPose(node_id_data.data.global_pose, translation_parameterization(),
                  common::make_unique<ceres::QuaternionParameterization>(),
                  &problem));
    if (frozen) {
      problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).rotation());
      problem.SetParameterBlockConstant(
          C_nodes.at(node_id_data.id).translation());
    }
  }
  // Add cost functions for intra- and inter-submap constraints.
  // 回环残差
  for (const Constraint& constraint : constraints) {
    problem.AddResidualBlock(
        SpaCostFunction3D::CreateAutoDiffCostFunction(constraint.pose),
        // Only loop closure constraints should have a loss function.
        constraint.tag == Constraint::INTER_SUBMAP
            ? new ceres::HuberLoss(options_.huber_scale())
            : nullptr /* loss function */,
        C_submaps.at(constraint.submap_id).rotation(),
        C_submaps.at(constraint.submap_id).translation(),
        C_nodes.at(constraint.node_id).rotation(),
        C_nodes.at(constraint.node_id).translation());
  }
  // Add cost functions for landmarks.
  AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_,
                           &C_nodes, &C_landmarks, &problem);
  // Add constraints based on IMU observations of angular velocities and
  // linear acceleration.
  // 添加IMU残差，以为着z轴可以被优化，这个是和2D有差别的地方，IMU线加速度不一定准，一般不用，暂不分析
  if (!options_.fix_z_in_3d()) {
    // 遍历所有的node
    for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
      // 对于当前的轨迹，取出轨迹的id
      const int trajectory_id = node_it->id.trajectory_id;
      // 取出当前轨迹的Node数据
      const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
      // 如果是frozen的状态，则没必要优化
      if (frozen_trajectories.count(trajectory_id) != 0) {
        // We skip frozen trajectories.
        node_it = trajectory_end;
        continue;
      }
      // 取出当前轨迹的trajectory_data
      TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);
      // 将IMU的外参加入优化变量
      problem.AddParameterBlock(trajectory_data.imu_calibration.data(), 4,
                                new ceres::QuaternionParameterization());
      CHECK(imu_data_.HasTrajectory(trajectory_id));
      // 取出当前轨迹的imu数据
      const auto imu_data = imu_data_.trajectory(trajectory_id);
      // 判断不为空
      CHECK(imu_data.begin() != imu_data.end());
      // 取出第一帧Imu数据
      auto imu_it = imu_data.begin();
      // 赋值当前的node为pre_node_it
      auto prev_node_it = node_it;
      // 计算下一个Node
      for (++node_it; node_it != trajectory_end; ++node_it) {
        // 第一个Node的id
        const NodeId first_node_id = prev_node_it->id;
        // 第一个node的位姿和时间
        const NodeSpec3D& first_node_data = prev_node_it->data;
        // 迭代
        prev_node_it = node_it;
        // 第二个node的id
        const NodeId second_node_id = node_it->id;
        // 第二个node的位姿和时间
        const NodeSpec3D& second_node_data = node_it->data;
        // 保证两个node是连续的
        if (second_node_id.node_index != first_node_id.node_index + 1) {
          continue;
        }

        // Skip IMU data before the node.如注释
        while (std::next(imu_it) != imu_data.end() &&
               std::next(imu_it)->time <= first_node_data.time) {
          ++imu_it;
        }

        auto imu_it2 = imu_it;
        // IMU积分，得到第1-2个node之间的角度增量和线速度
        const IntegrateImuResult<double> result = IntegrateImu(
            imu_data, first_node_data.time, second_node_data.time, &imu_it);
        // 第三个node
        const auto next_node_it = std::next(node_it);
        // 对于非最终的Node
        if (next_node_it != trajectory_end &&
            next_node_it->id.node_index == second_node_id.node_index + 1) {、
          // 第三个node的id
          const NodeId third_node_id = next_node_it->id;
          // 第三个node的位姿和时间
          const NodeSpec3D& third_node_data = next_node_it->data;
          // 计算两段时间差
          const common::Time first_time = first_node_data.time;
          const common::Time second_time = second_node_data.time;
          const common::Time third_time = third_node_data.time;
          // 第1-2个node之间的时间差
          const common::Duration first_duration = second_time - first_time;
          // 第2-3个node之间的时间差
          const common::Duration second_duration = third_time - second_time;
          // 第1.5个node的时间
          const common::Time first_center = first_time + first_duration / 2;
          // 第2.5个node的时间
          const common::Time second_center = second_time + second_duration / 2;
          // 第1-1.5个node之间的IMU积分
          const IntegrateImuResult<double> result_to_first_center =
              IntegrateImu(imu_data, first_time, first_center, &imu_it2);
          // 第1.5-2.5个node之间的IMU积分                             
          const IntegrateImuResult<double> result_center_to_center =
              IntegrateImu(imu_data, first_center, second_center, &imu_it2);
          // 'delta_velocity' is the change in velocity from the point in time
          // halfway between the first and second poses to halfway between
          // second and third pose. It is computed from IMU data and still
          // contains a delta due to gravity. The orientation of this vector is
          // in the IMU frame at the second pose.
          // 第1.5-2.5个node之间速度的变化量，包含重力方向，后面需要减掉重力加速度带来的z轴速度
          const Eigen::Vector3d delta_velocity =
              (result.delta_rotation.inverse() *
               result_to_first_center.delta_rotation) *
              result_center_to_center.delta_velocity;
          // 添加IMU线加速度残差，优化变量为第2个node的旋转，第1/2/3个node的平移，外参trajectory_data.imu_calibration、重力方向
          problem.AddResidualBlock(
              AccelerationCostFunction3D::CreateAutoDiffCostFunction(
                  options_.acceleration_weight(), delta_velocity,
                  common::ToSeconds(first_duration),
                  common::ToSeconds(second_duration)),
              nullptr /* loss function */,
              C_nodes.at(second_node_id).rotation(),
              C_nodes.at(first_node_id).translation(),
              C_nodes.at(second_node_id).translation(),
              C_nodes.at(third_node_id).translation(),
              &trajectory_data.gravity_constant,
              trajectory_data.imu_calibration.data());
        }
        // 添加IMU旋转残差，优化变量为第一个和第二个node的旋转角以及外参trajectory_data.imu_calibration
        problem.AddResidualBlock(
            RotationCostFunction3D::CreateAutoDiffCostFunction(
                options_.rotation_weight(), result.delta_rotation),
            nullptr /* loss function */, C_nodes.at(first_node_id).rotation(),
            C_nodes.at(second_node_id).rotation(),
            trajectory_data.imu_calibration.data());
      }
    }
  }
  // 不优化z轴方向，添加轮速计残差和Local SLAM node之间的残差，和2D差不多
  if (options_.fix_z_in_3d()) {
    // Add penalties for violating odometry (if available) and changes between
    // consecutive nodes.
    for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
      const int trajectory_id = node_it->id.trajectory_id;
      const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
      if (frozen_trajectories.count(trajectory_id) != 0) {
        node_it = trajectory_end;
        continue;
      }

      auto prev_node_it = node_it;
      for (++node_it; node_it != trajectory_end; ++node_it) {
        const NodeId first_node_id = prev_node_it->id;
        const NodeSpec3D& first_node_data = prev_node_it->data;
        prev_node_it = node_it;
        const NodeId second_node_id = node_it->id;
        const NodeSpec3D& second_node_data = node_it->data;

        if (second_node_id.node_index != first_node_id.node_index + 1) {
          continue;
        }

        // Add a relative pose constraint based on the odometry (if available).
        const std::unique_ptr<transform::Rigid3d> relative_odometry =
            CalculateOdometryBetweenNodes(trajectory_id, first_node_data,
                                          second_node_data);
        if (relative_odometry != nullptr) {
          problem.AddResidualBlock(
              SpaCostFunction3D::CreateAutoDiffCostFunction(Constraint::Pose{
                  *relative_odometry, options_.odometry_translation_weight(),
                  options_.odometry_rotation_weight()}),
              nullptr /* loss function */, C_nodes.at(first_node_id).rotation(),
              C_nodes.at(first_node_id).translation(),
              C_nodes.at(second_node_id).rotation(),
              C_nodes.at(second_node_id).translation());
        }

        // Add a relative pose constraint based on consecutive local SLAM poses.
        const transform::Rigid3d relative_local_slam_pose =
            first_node_data.local_pose.inverse() * second_node_data.local_pose;
        problem.AddResidualBlock(
            SpaCostFunction3D::CreateAutoDiffCostFunction(
                Constraint::Pose{relative_local_slam_pose,
                                 options_.local_slam_pose_translation_weight(),
                                 options_.local_slam_pose_rotation_weight()}),
            nullptr /* loss function */, C_nodes.at(first_node_id).rotation(),
            C_nodes.at(first_node_id).translation(),
            C_nodes.at(second_node_id).rotation(),
            C_nodes.at(second_node_id).translation());
      }
    }
  }

  // Add fixed frame pose constraints.
  std::map<int, CeresPose> C_fixed_frames;
  for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
    const int trajectory_id = node_it->id.trajectory_id;
    const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
    if (!fixed_frame_pose_data_.HasTrajectory(trajectory_id)) {
      node_it = trajectory_end;
      continue;
    }

    const TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);
    bool fixed_frame_pose_initialized = false;
    for (; node_it != trajectory_end; ++node_it) {
      const NodeId node_id = node_it->id;
      const NodeSpec3D& node_data = node_it->data;

      const std::unique_ptr<transform::Rigid3d> fixed_frame_pose =
          Interpolate(fixed_frame_pose_data_, trajectory_id, node_data.time);
      if (fixed_frame_pose == nullptr) {
        continue;
      }

      const Constraint::Pose constraint_pose{
          *fixed_frame_pose, options_.fixed_frame_pose_translation_weight(),
          options_.fixed_frame_pose_rotation_weight()};

      if (!fixed_frame_pose_initialized) {
        transform::Rigid3d fixed_frame_pose_in_map;
        if (trajectory_data.fixed_frame_origin_in_map.has_value()) {
          fixed_frame_pose_in_map =
              trajectory_data.fixed_frame_origin_in_map.value();
        } else {
          fixed_frame_pose_in_map =
              node_data.global_pose * constraint_pose.zbar_ij.inverse();
        }
        C_fixed_frames.emplace(
            std::piecewise_construct, std::forward_as_tuple(trajectory_id),
            std::forward_as_tuple(
                transform::Rigid3d(
                    fixed_frame_pose_in_map.translation(),
                    Eigen::AngleAxisd(
                        transform::GetYaw(fixed_frame_pose_in_map.rotation()),
                        Eigen::Vector3d::UnitZ())),
                nullptr,
                common::make_unique<ceres::AutoDiffLocalParameterization<
                    YawOnlyQuaternionPlus, 4, 1>>(),
                &problem));
        fixed_frame_pose_initialized = true;
      }

      problem.AddResidualBlock(
          SpaCostFunction3D::CreateAutoDiffCostFunction(constraint_pose),
          nullptr /* loss function */,
          C_fixed_frames.at(trajectory_id).rotation(),
          C_fixed_frames.at(trajectory_id).translation(),
          C_nodes.at(node_id).rotation(), C_nodes.at(node_id).translation());
    }
  }
  // Solve.
  ceres::Solver::Summary summary;
  ceres::Solve(
      common::CreateCeresSolverOptions(options_.ceres_solver_options()),
      &problem, &summary);
  if (options_.log_solver_summary()) {
    LOG(INFO) << summary.FullReport();
    for (const auto& trajectory_id_and_data : trajectory_data_) {
      const int trajectory_id = trajectory_id_and_data.first;
      const TrajectoryData& trajectory_data = trajectory_id_and_data.second;
      if (trajectory_id != 0) {
        LOG(INFO) << "Trajectory " << trajectory_id << ":";
      }
      LOG(INFO) << "Gravity was: " << trajectory_data.gravity_constant;
      const auto& imu_calibration = trajectory_data.imu_calibration;
      LOG(INFO) << "IMU correction was: "
                << common::RadToDeg(2. * std::acos(imu_calibration[0]))
                << " deg (" << imu_calibration[0] << ", " << imu_calibration[1]
                << ", " << imu_calibration[2] << ", " << imu_calibration[3]
                << ")";
    }
  }

  // Store the result.
  for (const auto& C_submap_id_data : C_submaps) {
    submap_data_.at(C_submap_id_data.id).global_pose =
        C_submap_id_data.data.ToRigid();
  }
  for (const auto& C_node_id_data : C_nodes) {
    node_data_.at(C_node_id_data.id).global_pose =
        C_node_id_data.data.ToRigid();
  }
  for (const auto& C_fixed_frame : C_fixed_frames) {
    trajectory_data_.at(C_fixed_frame.first).fixed_frame_origin_in_map =
        C_fixed_frame.second.ToRigid();
  }
  for (const auto& C_landmark : C_landmarks) {
    landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();
  }
}

IMU积分

template <typename T>
struct IntegrateImuResult {
  Eigen::Matrix<T, 3, 1> delta_velocity;
  Eigen::Quaternion<T> delta_rotation;
};

template <typename T, typename RangeType, typename IteratorType>
IntegrateImuResult<T> IntegrateImu(
    const RangeType& imu_data,
    const Eigen::Transform<T, 3, Eigen::Affine>&
        linear_acceleration_calibration,
    const Eigen::Transform<T, 3, Eigen::Affine>& angular_velocity_calibration,
    const common::Time start_time, const common::Time end_time,
    IteratorType* const it) {
  // it为start_time前的一帧imu数据
  CHECK_LE(start_time, end_time);
  CHECK(*it != imu_data.end());
  CHECK_LE((*it)->time, start_time);
  if (std::next(*it) != imu_data.end()) {
    CHECK_GT(std::next(*it)->time, start_time);
  }

  common::Time current_time = start_time;

  IntegrateImuResult<T> result = {Eigen::Matrix<T, 3, 1>::Zero(),
                                  Eigen::Quaterniond::Identity().cast<T>()};
  while (current_time < end_time) {
    common::Time next_imu_data = common::Time::max();
    if (std::next(*it) != imu_data.end()) {
      next_imu_data = std::next(*it)->time;
    }
    common::Time next_time = std::min(next_imu_data, end_time);
    // 计算时间增量
    const T delta_t(common::ToSeconds(next_time - current_time));
    // 计算角度增量
    const Eigen::Matrix<T, 3, 1> delta_angle =
        (angular_velocity_calibration *
         (*it)->angular_velocity.template cast<T>()) *
        delta_t;
    // 角度增量转换为旋转矩阵
    result.delta_rotation *=
        transform::AngleAxisVectorToRotationQuaternion(delta_angle);
    // 计算线速度增量，前面要乘以旋转矩阵才行
    result.delta_velocity += result.delta_rotation *
                             ((linear_acceleration_calibration *
                               (*it)->linear_acceleration.template cast<T>()) *
                              delta_t);
    current_time = next_time;
    if (current_time == next_imu_data) {
      ++(*it);
    }
  }
  return result;
}

// Returns velocity delta in map frame.
template <typename RangeType, typename IteratorType>
IntegrateImuResult<double> IntegrateImu(const RangeType& imu_data,
                                        const common::Time start_time,
                                        const common::Time end_time,
                                        IteratorType* const it) {
  return IntegrateImu<double, RangeType, IteratorType>(
      imu_data, Eigen::Affine3d::Identity(), Eigen::Affine3d::Identity(),
      start_time, end_time, it);
}

残差项

3D SPA残差

class SpaCostFunction3D {
 public:
  static ceres::CostFunction* CreateAutoDiffCostFunction(
      const PoseGraph::Constraint::Pose& pose) {
    return new ceres::AutoDiffCostFunction<
        SpaCostFunction3D, 6 /* residuals */, 4 /* rotation variables */,
        3 /* translation variables */, 4 /* rotation variables */,
        3 /* translation variables */>(new SpaCostFunction3D(pose));
  }

  template <typename T>
  bool operator()(const T* const c_i_rotation, const T* const c_i_translation,
                  const T* const c_j_rotation, const T* const c_j_translation,
                  T* const e) const {
    const std::array<T, 6> error = ScaleError(
        ComputeUnscaledError(pose_.zbar_ij, c_i_rotation, c_i_translation,
                             c_j_rotation, c_j_translation),
        pose_.translation_weight, pose_.rotation_weight);
    std::copy(std::begin(error), std::end(error), e);
    return true;
  }

 private:
  explicit SpaCostFunction3D(const PoseGraph::Constraint::Pose& pose)
      : pose_(pose) {}

  const PoseGraph::Constraint::Pose pose_;
};

template <typename T>
static std::array<T, 6> ComputeUnscaledError(
    const transform::Rigid3d& relative_pose, const T* const start_rotation,
    const T* const start_translation, const T* const end_rotation,
    const T* const end_translation) {
  const Eigen::Quaternion<T> R_i_inverse(start_rotation[0], -start_rotation[1],
                                         -start_rotation[2],
                                         -start_rotation[3]);

  const Eigen::Matrix<T, 3, 1> delta(end_translation[0] - start_translation[0],
                                     end_translation[1] - start_translation[1],
                                     end_translation[2] - start_translation[2]);
  const Eigen::Matrix<T, 3, 1> h_translation = R_i_inverse * delta;

  const Eigen::Quaternion<T> h_rotation_inverse =
      Eigen::Quaternion<T>(end_rotation[0], -end_rotation[1], -end_rotation[2],
                           -end_rotation[3]) *
      Eigen::Quaternion<T>(start_rotation[0], start_rotation[1],
                           start_rotation[2], start_rotation[3]);

  const Eigen::Matrix<T, 3, 1> angle_axis_difference =
      transform::RotationQuaternionToAngleAxisVector(
          h_rotation_inverse * relative_pose.rotation().cast<T>());

  return {{T(relative_pose.translation().x()) - h_translation[0],
           T(relative_pose.translation().y()) - h_translation[1],
           T(relative_pose.translation().z()) - h_translation[2],
           angle_axis_difference[0], angle_axis_difference[1],
           angle_axis_difference[2]}};
}

IMU相关残差

线加速度残差：

class AccelerationCostFunction3D {
 public:
  static ceres::CostFunction* CreateAutoDiffCostFunction(
      const double scaling_factor,
      const Eigen::Vector3d& delta_velocity_imu_frame,
      const double first_delta_time_seconds,
      const double second_delta_time_seconds) {
    return new ceres::AutoDiffCostFunction<
        AccelerationCostFunction3D, 3 /* residuals */,
        4 /* rotation variables */, 3 /* position variables */,
        3 /* position variables */, 3 /* position variables */,
        1 /* gravity variables */, 4 /* rotation variables */>(
        new AccelerationCostFunction3D(scaling_factor, delta_velocity_imu_frame,
                                       first_delta_time_seconds,
                                       second_delta_time_seconds));
  }

  template <typename T>
  bool operator()(const T* const middle_rotation, const T* const start_position,
                  const T* const middle_position, const T* const end_position,
                  const T* const gravity_constant,
                  const T* const imu_calibration, T* residual) const {
    const Eigen::Quaternion<T> eigen_imu_calibration(
        imu_calibration[0], imu_calibration[1], imu_calibration[2],
        imu_calibration[3]);
    // 利用IMU计算去掉重力的世界坐标系下的速度增量
    const Eigen::Matrix<T, 3, 1> imu_delta_velocity =
        // 第2个node坐标系下的旋转乘以Imu外参转换到世界坐标系下
        ToEigen(middle_rotation) * eigen_imu_calibration *
            // 速度增量转换到世界坐标系下
            delta_velocity_imu_frame_.cast<T>() -
        // 减掉重力加速度的影响：gt
        *gravity_constant *
            (0.5 * (first_delta_time_seconds_ + second_delta_time_seconds_) *
             Eigen::Vector3d::UnitZ())
                .cast<T>();
    // 通过node计算速度变化量
    // 第1-2个node之间的平均速度
    const Eigen::Matrix<T, 3, 1> start_velocity =
        (Eigen::Map<const Eigen::Matrix<T, 3, 1>>(middle_position) -
         Eigen::Map<const Eigen::Matrix<T, 3, 1>>(start_position)) /
        T(first_delta_time_seconds_);
    // 第2-3个node之间的平均速度
    const Eigen::Matrix<T, 3, 1> end_velocity =
        (Eigen::Map<const Eigen::Matrix<T, 3, 1>>(end_position) -
         Eigen::Map<const Eigen::Matrix<T, 3, 1>>(middle_position)) /
        T(second_delta_time_seconds_);
    // 通过node得到的速度增量（世界坐标系下）
    const Eigen::Matrix<T, 3, 1> delta_velocity = end_velocity - start_velocity;
    // 两个增量相减得到残差
    (Eigen::Map<Eigen::Matrix<T, 3, 1>>(residual) =
         T(scaling_factor_) * (imu_delta_velocity - delta_velocity));
    return true;
  }

 private:
  AccelerationCostFunction3D(const double scaling_factor,
                             // 第1.5-2.5个node的速度变化量
                             const Eigen::Vector3d& delta_velocity_imu_frame,
                             // 第1-2个node之间的时间差
                             const double first_delta_time_seconds,
                             // 第2-3个node之间的时间差
                             const double second_delta_time_seconds)
      : scaling_factor_(scaling_factor),
        delta_velocity_imu_frame_(delta_velocity_imu_frame),
        first_delta_time_seconds_(first_delta_time_seconds),
        second_delta_time_seconds_(second_delta_time_seconds) {}

  AccelerationCostFunction3D(const AccelerationCostFunction3D&) = delete;
  AccelerationCostFunction3D& operator=(const AccelerationCostFunction3D&) =
      delete;

  template <typename T>
  static Eigen::Quaternion<T> ToEigen(const T* const quaternion) {
    return Eigen::Quaternion<T>(quaternion[0], quaternion[1], quaternion[2],
                                quaternion[3]);
  }

  const double scaling_factor_;
  const Eigen::Vector3d delta_velocity_imu_frame_;
  const double first_delta_time_seconds_;
  const double second_delta_time_seconds_;
};

角速度残差：

class RotationCostFunction3D {
 public:
  static ceres::CostFunction* CreateAutoDiffCostFunction(
      const double scaling_factor,
      const Eigen::Quaterniond& delta_rotation_imu_frame) {
    return new ceres::AutoDiffCostFunction<
        RotationCostFunction3D, 3 /* residuals */, 4 /* rotation variables */,
        4 /* rotation variables */, 4 /* rotation variables */
        >(new RotationCostFunction3D(scaling_factor, delta_rotation_imu_frame));
  }

  template <typename T>
  bool operator()(const T* const start_rotation, const T* const end_rotation,
                  const T* const imu_calibration, T* residual) const {
    // 第1个node的rotation
    const Eigen::Quaternion<T> start(start_rotation[0], start_rotation[1],
                                     start_rotation[2], start_rotation[3]);
    // 第2个node的rotation
    const Eigen::Quaternion<T> end(end_rotation[0], end_rotation[1],
                                   end_rotation[2], end_rotation[3]);
    // 第1-2个node之间的rotation增量
    const Eigen::Quaternion<T> eigen_imu_calibration(
        imu_calibration[0], imu_calibration[1], imu_calibration[2],
        imu_calibration[3]);
    // R_2*T_ic = R_1 * T_ic * delta_R
    // 残差为：R_2.inverse() * R_1 * T_ic * delta_R *T_ic.inverse()
    const Eigen::Quaternion<T> error =
        end.conjugate() * start * eigen_imu_calibration *
        delta_rotation_imu_frame_.cast<T>() * eigen_imu_calibration.conjugate();
    residual[0] = scaling_factor_ * error.x();
    residual[1] = scaling_factor_ * error.y();
    residual[2] = scaling_factor_ * error.z();
    return true;
  }

 private:
  RotationCostFunction3D(const double scaling_factor,
                         // 第1-2个node之间的旋转增量
                         const Eigen::Quaterniond& delta_rotation_imu_frame)
      : scaling_factor_(scaling_factor),
        delta_rotation_imu_frame_(delta_rotation_imu_frame) {}

  RotationCostFunction3D(const RotationCostFunction3D&) = delete;
  RotationCostFunction3D& operator=(const RotationCostFunction3D&) = delete;

  const double scaling_factor_;
  // 第1-2个node之间的旋转增量
  const Eigen::Quaterniond delta_rotation_imu_frame_;
};

cartographer源码分析到此为止，主要的内容大概都有注释，更详细的分析暂时不会放在CSDN上，等以后再决定是否要放出，或更新到Git上。