在ceres-solver官方教程中提供了三个SLAM相关的优化问题，分别为：bundle_adjuster、pose_graph_2d、pose_graph_3d。本文就这三个问题进行拆解和分析。

在阅读本文之前，你需要确保已经知晓了Ceres的基础用法，这部分可以参考文章：【SLAM】Ceres优化库超详细解析。

bundle_adjuster

问题描述与分析

Bundle Adjustment（BA、光束平差）问题是视觉SLAM中最基础最常用的优化方法和思路。

给定一组测量的图像特征位置和对应关系，光束平差的目标是优化3D空间点和相机参数来最小化重投影误差。这个优化问题通常是被定义为一个非线性最小二乘问题，其误差是观测特征点像素位置与相对应3D空间点在相机图像平面投影像素点的$L_2$范式的平方（一般都在归一化平面点上）。其成像过程如下所示。

如果不太了解其细节，可以参考文章：SLAM后端：BA优化（Bundle Adjustment）。

给定一组3D空间点$X_1$、$X_2$、……、$X_n$，它们对应的相机参数为$P_1$、$P_2$、……、$P_n$。如果空间点$X_i$可以在图像$j$上可见，那么就会有一个对应的2D图像坐标点$u_{ij}$与之对应，并且它可以由图像投影原理$P_i{X}_i$得到。那么对应的重投影误差就是：

$$r=\sum (u_{ij}-P_i{X}_i)$$

其中，$0\le i\le n$、$0\le j\le n$。

3D空间点很好表示，那么相机参数如何表示呢？本例用九参数定义法：三个表示旋转（外参）为罗德里格斯的角轴向量，三个表示平移（外参），一个焦距和两个径向畸变（内参）。

代码详解

完整代码位置：bundle_adjuster代码位置。

使用Ceres库的关键是构造costfunction，具体如下所示：

struct SnavelyReprojectionError {
  SnavelyReprojectionError(double observed_x, double observed_y)
      : observed_x(observed_x), observed_y(observed_y) {}

  template <typename T>
  bool operator()(const T* const camera, const T* const point,
                  T* residuals) const {
    // 将3D空间点通过相机外参进行旋转平移
    T p[3];
    AngleAxisRotatePoint(camera, point, p);
    p[0] += camera[3];
    p[1] += camera[4];
    p[2] += camera[5];
    // 采用Noah Snavely's Bundler assumes相机模型，并进行归一化
    // （本来相平面在光心后面，这里将其放到光心前面，因此z值实际上是带负号的）
    const T xp = -p[0] / p[2];
    const T yp = -p[1] / p[2];
    // 去畸变
    const T& l1 = camera[7];
    const T& l2 = camera[8];
    const T r2 = xp * xp + yp * yp;
    const T distortion = 1.0 + r2 * (l1 + l2 * r2);
    // 将3D空间点通过相机内参进行图像投影
    const T& focal = camera[6];
    const T predicted_x = focal * distortion * xp;
    const T predicted_y = focal * distortion * yp;
    // 计算残差
    residuals[0] = predicted_x - observed_x;
    residuals[1] = predicted_y - observed_y;
    return true;
  }

  // 静态方法用于创建代价函数实例
  static ceres::CostFunction* Create(const double observed_x,
                                     const double observed_y) {
    // 自动求导方式，残差维度为2，后续为相机参数维度、3D空间点维度
    return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
        new SnavelyReprojectionError(observed_x, observed_y)));
  }

  // 图像坐标点的x坐标
  double observed_x;
  // 图像坐标点的y坐标
  double observed_y;
};

残差的计算方式比较简单，主要就是将3D空间点进行图像投影，再计算其与图像坐标点之间的差值。

// 设置优化问题的配置选项（略）
void SetSolverOptionsFromFlags(BALProblem* bal_problem,
                               Solver::Options* options) {
  SetMinimizerOptions(options);
  SetLinearSolver(options);
  SetOrdering(bal_problem, options);
}

// 从BA构造非线性最小二乘优化问题
void BuildProblem(BALProblem* bal_problem, Problem* problem) {
  const int point_block_size = bal_problem->point_block_size();
  const int camera_block_size = bal_problem->camera_block_size();
  // BAL数据集的3D空间点
  double* points = bal_problem->mutable_points();
  // BAL数据集的相机参数
  double* cameras = bal_problem->mutable_cameras();
  // BAL数据集的图像坐标点
  const double* observations = bal_problem->observations();

  for (int i = 0; i < bal_problem->num_observations(); ++i) {
    CostFunction* cost_function;
    cost_function = SnavelyReprojectionError::Create(
        observations[2 * i + 0], observations[2 * i + 1]);
    LossFunction* loss_function = new HuberLoss(1.0);
    double* camera =
        cameras + camera_block_size * bal_problem->camera_index()[i];
    double* point = points + point_block_size * bal_problem->point_index()[i];
    problem->AddResidualBlock(cost_function, loss_function, camera, point);
  }
}

// 解决最小二乘的优化问题
void SolveProblem(const char* filename) {
  // 从BAL数据集中进行初始化，并存储一系列配置
  BALProblem bal_problem(filename);

  // 构建Ceres最小二乘问题并进行优化
  ceres::Problem problem;
  BuildProblem(&bal_problem, &problem);
  ceres::Solver::Options options;
  SetSolverOptionsFromFlags(&bal_problem, &options);
  options.gradient_tolerance = 1e-16;
  options.function_tolerance = 1e-16;
  options.parameter_tolerance = 1e-16;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  std::cout << summary.FullReport() << "\n";
}

int main(int argc, char** argv) {
  ceres::examples::SolveProblem(CERES_GET_FLAG(FLAGS_input).c_str());
  return 0;
}

代码大多是比较正常的Ceres的问题处理流程，唯一特殊的地方可能就是在linear_solver_type的配置上了。

由于这是一个很大的稀疏问题（无论如何对于DENSE_QR来说都是很大），但BA问题特有的稀疏结构使得使用另外的方法求解它更加有效。Ceres为这个BA提供了三个专门的求解器（统称为基于Schur的求解器）：SPARSE_SCHUR、DENSE_SCHUR、ITERATIVE_SCHUR。可以使用这种求解器快速高效求解。

pose_graph_2d

问题描述与分析

同时定位和建图（SLAM）问题包括构建未知环境的地图，同时根据该地图进行定位。这个问题的主要困难在于没有任何额外的外部辅助信息，例如GPS。SLAM被认为是机器人技术的基本挑战之一。关于SLAM有很多资源。位姿图优化问题是SLAM问题的一个示例。下面解释如何在具有相对位姿约束的二维中制定基于位姿图的SLAM问题。

考虑一个在二维平面上移动的机器人。机器人可以使用一组传感器，例如车轮里程计或激光测距扫描仪。根据这些原始测量结果，我们想要估计机器人的轨迹并构建环境地图。为了降低问题的计算复杂性，位姿图方法抽象了原始测量值。具体来说，它创建一个由表示机器人姿态的节点和表示两个节点之间的相对变换（增量位置和方向）的边组成的图。边缘是从原始传感器测量值导出的虚拟测量值，例如通过集成原始车轮里程计或对齐从机器人获取的激光距离扫描。生成的图表的可视化如下所示。

该图将机器人的姿态描述为三角形，测量值由连接线指示，环路闭合测量值显示为虚线。环路闭合是非顺序机器人状态之间的测量，它们可以减少随着时间的推移而累积的误差。下面将描述位姿图问题的数学公式。

机器人在时间戳为$t$的时刻，状态为$x_t=[p^T,\psi {]}^T$，其中$p$为一个二维向量，表示在平面中的位置，$\psi$是以弧度为单位的方向。$a$和$b$这两个时间戳下，机器人状态之间的相对变换的测量表示为：$z_{ab}=[{\hat{p}}_{ab}^T,{\hat{\psi }}_{ab}]$。Ceres代价函数中实现的残差计算，可以表示为测量值与预测值之间的误差：

$$\begin{array}{r}{r}_{ab}=\left[\begin{array}{c}{R}_a^T\left(p_b-p_a\right)-{\hat{p}}_{ab}\\ \mathrm{Normalize}\left({\psi }_b-{\psi }_a-{\hat{\psi }}_{ab}\right)\end{array}\right]\end{array}$$

其中，$Normalize$函数用来将角度$\psi$标准化到$[-\pi ,\pi )$，旋转矩阵$R$是由下式给出：

$$\begin{array}{r}{R}_a=\left[\begin{array}{cc}\cos {\psi }_a& -\sin {\psi }_a\\ \sin {\psi }_a& \cos {\psi }_a\end{array}\right]\end{array}$$

需要注意的是：机器人的位置$p$是世界坐标系下的坐标，而${\hat{p}}_{ab}$是两个时间戳下的相对位置变换，因此在计算残差的时候需要利用旋转矩阵$R$进行转换。同时，$R$表示的是节点坐标系到世界坐标系的转换，那么$R^T$就是世界坐标系到节点坐标系的转换了，因此公式中写的是$R_a^T$。

为了完成代价函数，我们需要根据测量值的不确定性对残差进行加权。因此，我们将残差预先乘以协方差矩阵的倒数平方根以进行测量，即${\Sigma }_{ab}^{-\frac{1}{2}}{r}_{ab}$，其中${\Sigma }_{ab}$为协方差。

最后，我们使用manifold来将方向标准化到$[-\pi ,\pi )$的范围内。特别地，我们将重写AngleManifold::Plus()函数来实现$\mathrm{Normalize}(\psi +\Delta )$，重写AngleManifold::Minus()函数来实现$\mathrm{Normalize}(y)-\mathrm{Normalize}(x)$。

代码详解

完整代码位置：pose_graph_2d代码位置。

使用Ceres库的关键是构造costfunction，具体如下所示：

// 标准化角度到[-pi,pi)
template <typename T>
inline T NormalizeAngle(const T& angle_radians) {
  T two_pi(2.0 * M_PI);
  return angle_radians -
         two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi);
}

// 构造旋转矩阵R
template <typename T>
Eigen::Matrix<T, 2, 2> RotationMatrix2D(T yaw_radians) {
  const T cos_yaw = ceres::cos(yaw_radians);
  const T sin_yaw = ceres::sin(yaw_radians);
  Eigen::Matrix<T, 2, 2> rotation;
  rotation << cos_yaw, -sin_yaw, sin_yaw, cos_yaw;
  return rotation;
}

class PoseGraph2dErrorTerm {
 public:
  PoseGraph2dErrorTerm(double x_ab, double y_ab, double yaw_ab_radians,
                       const Eigen::Matrix3d& sqrt_information)
      : p_ab_(x_ab, y_ab),
        yaw_ab_radians_(yaw_ab_radians),
        sqrt_information_(sqrt_information) {}

  template <typename T>
  bool operator()(const T* const x_a, const T* const y_a, const T* const yaw_a,
                  const T* const x_b, const T* const y_b, const T* const yaw_b,
                  T* residuals_ptr) const {
    const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
    const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);
    Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals_map(residuals_ptr);
    // 位置残差
    residuals_map.template head<2>() =
        RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - p_ab_.cast<T>();
    // 方向残差
    residuals_map(2) = NormalizeAngle(
        (*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));
    // 通过平方根信息矩阵缩放残差以表示测量值的不确定性
    residuals_map = sqrt_information_.template cast<T>() * residuals_map;
    return true;
  }

  // 静态方法用于创建代价函数实例
  static ceres::CostFunction* Create(double x_ab, double y_ab,
                                     double yaw_ab_radians,
                                     const Eigen::Matrix3d& sqrt_information) {
    // 自动求导方式，残差维度为3，后续为a时刻x位置，a时刻y位置，a时刻角度，b时刻x位置，b时刻y位置，b时刻角度
    return (new ceres::AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1, 1, 1>(
      new PoseGraph2dErrorTerm(x_ab, y_ab, yaw_ab_radians, sqrt_information)));
  }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 private:
  // a和b时刻之间的相对位置变换
  const Eigen::Vector2d p_ab_;
  // a和b时刻之间的相对角度变换
  const double yaw_ab_radians_;
  // 测量协方差矩阵的倒数平方根
  const Eigen::Matrix3d sqrt_information_;
};

残差的计算方式和上文的公式一一对应，还是比较简单清晰的。

主要的疑惑点可能是关于Eigen的用法：

1. 程序中有一句EIGEN_MAKE_ALIGNED_OPERATOR_NEW来声明内存对齐，这在自定义的类中使用Eigen的数据类型时是必要的，还有的时候能看到在std::vector中插入Eigen的变量后面要加一个Eigen::Aligned…，也是为了声明Eigen的内存对齐，这样才能使用STL容器。所以这里要清楚，Eigen为了实现矩阵运算，其内存和原生的C++内存分配出现了差别，很多时候都需要声明一下
2. 在()运算中定义残差的时候，使用到了Eigen中的map类。实际上该map类是为了兼容Eigen的矩阵操作与C++的数组操作。也就是说，该类就是为了实现和C++中数组一样的内存分配方式，比如使用指针可以快速访问；但是同时它又可以支持Eigen的矩阵运算，所以在需要对矩阵元素频繁操作或者访问的时候，声明为map类是一个很好的选择。更多可参考文章：Eigen库中的Map类到底是做什么的
3. template head和template cast的使用，这两个都是template在模板的使用，表明后面都是templated method。更多可参考文章：.template cast＜＞中template的作用、.template head＜＞中template的作用

// 两个时刻点之间的相对位姿的变换
struct Constraint2d {
  int id_begin;
  int id_end;
  double x;
  double y;
  double yaw_radians;
  Eigen::Matrix3d information;
};

// 每个时刻点位姿的世界坐标系下的表示
struct Pose2d {
  double x;
  double y;
  double yaw_radians;
};

// 将角度归一化到[-pi,pi)的局部参数化类
class AngleManifold {
 public:
  template <typename T>
  bool Plus(const T* x_radians,
            const T* delta_radians,
            T* x_plus_delta_radians) const {
    *x_plus_delta_radians = NormalizeAngle(*x_radians + *delta_radians);
    return true;
  }
  template <typename T>
  bool Minus(const T* y_radians,
             const T* x_radians,
             T* y_minus_x_radians) const {
    *y_minus_x_radians =
        NormalizeAngle(*y_radians) - NormalizeAngle(*x_radians);
    return true;
  }

  // 静态方法用于创建Manifold实例
  static ceres::Manifold* Create() {
    return new ceres::AutoDiffManifold<AngleManifold, 1, 1>;
  }
};

// 从位姿图约束构造非线性最小二乘优化问题
void BuildOptimizationProblem(const std::vector<Constraint2d>& constraints,
                              std::map<int, Pose2d>* poses,
                              ceres::Problem* problem) {
  ceres::LossFunction* loss_function = nullptr;
  ceres::Manifold* angle_manifold = AngleManifold::Create();
  for (const auto& constraint : constraints) {
    auto pose_begin_iter = poses->find(constraint.id_begin);
    auto pose_end_iter = poses->find(constraint.id_end);
    // llt = LL^T，cholesky分解，求解信息矩阵的平方根
    const Eigen::Matrix3d sqrt_information =
        constraint.information.llt().matrixL();
    ceres::CostFunction* cost_function = PoseGraph2dErrorTerm::Create(
        constraint.x, constraint.y, constraint.yaw_radians, sqrt_information);
    problem->AddResidualBlock(cost_function,
                              loss_function,
                              &pose_begin_iter->second.x,
                              &pose_begin_iter->second.y,
                              &pose_begin_iter->second.yaw_radians,
                              &pose_end_iter->second.x,
                              &pose_end_iter->second.y,
                              &pose_end_iter->second.yaw_radians);
    problem->SetManifold(&pose_begin_iter->second.yaw_radians, angle_manifold);
    problem->SetManifold(&pose_end_iter->second.yaw_radians, angle_manifold);
  }
  // 该位姿图优化问题具有三个未完全约束的自由度。这通常称为规范自由度。你可以对所有节点应用刚体变换，优化问题仍然具有完全相同的成本
  // Levenberg-Marquardt算法的内部阻尼可以缓解这个问题，但最好适当地约束规范自由度
  // 这可以通过固定其中一个位姿来完成作为常量，因此优化器无法更改它
  auto pose_start_iter = poses->begin();
  problem->SetParameterBlockConstant(&pose_start_iter->second.x);
  problem->SetParameterBlockConstant(&pose_start_iter->second.y);
  problem->SetParameterBlockConstant(&pose_start_iter->second.yaw_radians);
}

// 解决最小二乘的优化问题
bool SolveOptimizationProblem(ceres::Problem* problem) {
  ceres::Solver::Options options;
  options.max_num_iterations = 100;
  options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
  ceres::Solver::Summary summary;
  ceres::Solve(options, problem, &summary);
  std::cout << summary.FullReport() << '\n';
  return summary.IsSolutionUsable();
}

int main(int argc, char** argv) {
  std::map<int, ceres::examples::Pose2d> poses;
  std::vector<ceres::examples::Constraint2d> constraints;
  // 读取数据（略）
  ReadG2oFile(&poses, &constraints);

  ceres::Problem problem;
  ceres::examples::BuildOptimizationProblem(constraints, &poses, &problem);
  ceres::examples::SolveOptimizationProblem(&problem);
  return 0;
}

代码大多是比较正常的Ceres的问题处理流程，比较难懂的就是SetParameterBlockConstant()了。为什么要固定poses->begin()的位姿呢？

其实也比较容易理解，我们拿到的数据中，constraints是两个时刻位姿之间的相对变换，它是有噪声的测量值；poses是每个时刻的位姿，它是我们需要优化的值。那么我们会根据其中的约束关系进行优化，使得其中的残差最小。那么我们就会对poses进行微调，以寻求最小的残差。若如果所有的poses都不是固定的，虽然也可能优化出一个值，但是这样的话就很麻烦。所以我们就需要固定住其中的一个（代码中选择的开始的那个）。

效果图

运行程序，利用官方提供的plot_results.py可以画出原始和优化后的位姿地图，类似下图：

pose_graph_3d

问题描述与分析

下面解释如何在具有相对位姿约束的三维空间中制定基于位姿图的SLAM问题。该示例还说明了如何将Eigen的几何模块与Ceres的自动微分功能结合使用。

机器人在时间戳为$t$的时刻，状态为$x_t=[p^T,q^T{]}^T$，其中$p$为一个三维向量，表示在三维空间的位置，$q$是四元数表示的方向。$a$和$b$这两个时间戳下，机器人状态之间的相对变换的测量表示为：$z_{ab}=[{\hat{p}}_{ab}^T,{\hat{q}}_{ab}^T{]}^T$。Ceres代价函数中实现的残差计算，可以表示为测量值与预测值之间的误差：

$$\begin{array}{r}{r}_{ab}=\left[\begin{array}{c}{q}_a^{-1}(p_b-p_a)-{\hat{p}}_{ab}\\ 2.0\mathrm{vec}\left((q_a^{-1}{q}_b){\hat{q}}_{ab}^{-1}\right)\end{array}\right]\end{array}$$

其中，$vec$函数返回四元数的向量部分。例如，$[q_x,q_y,q_z]$。$R_q$是四元数对应的旋转矩阵。

需要注意的是：对于向量$v$，经过四元数$q$的变换之后，向量转换为$qv{q}^{-1}$。但是上面的公式，并没有采用这样的方式进行描述，采用的是Eigen::Quaternion的方法，乘法（*）表示的是旋转作用，逆（-1、conjugate()函数）表示反旋转作用。这是因为Eigen对其进行了重写。

为了完成代价函数，我们需要根据测量值的不确定性对残差进行加权。因此，我们将残差预先乘以协方差矩阵的倒数平方根以进行测量，即${\Sigma }_{ab}^{-\frac{1}{2}}{r}_{ab}$，其中${\Sigma }_{ab}$为协方差。

鉴于我们使用四元数来表示方向，我们需要使用流形（EigenQuaternionManifold）来应用于四元数四维向量的更新操作。Eigen中定义的四元数，其元素位置与常用的采取不同的内部存储布局。具体来说，Eigen将内存中的元素存储为$[x,y,z,w]$，其中实部位于最后，而常用的存储方式是存储在开头。由于Ceres操作的参数块是裸指针，因此这种差异很重要并且需要不同的参数化。

代码详解

完整代码位置：pose_graph_3d代码位置。

使用Ceres库的关键是构造costfunction，具体如下所示：

// 每个时刻点位姿的世界坐标系下的表示
struct Pose3d {
  Eigen::Vector3d p;
  Eigen::Quaterniond q;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

class PoseGraph3dErrorTerm {
 public:
  PoseGraph3dErrorTerm(Pose3d t_ab_measured,
                       Eigen::Matrix<double, 6, 6> sqrt_information)
      : t_ab_measured_(std::move(t_ab_measured)),
        sqrt_information_(std::move(sqrt_information)) {}

  template <typename T>
  bool operator()(const T* const p_a_ptr, const T* const q_a_ptr,
                  const T* const p_b_ptr, const T* const q_b_ptr,
                  T* residuals_ptr) const {
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_a(p_a_ptr);
    Eigen::Map<const Eigen::Quaternion<T>> q_a(q_a_ptr);
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_b(p_b_ptr);
    Eigen::Map<const Eigen::Quaternion<T>> q_b(q_b_ptr);
    Eigen::Map<Eigen::Matrix<T, 6, 1>> residuals(residuals_ptr);
    // 位置残差
    Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();
    Eigen::Matrix<T, 3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a);
    residuals.template block<3, 1>(0, 0) =
        p_ab_estimated - t_ab_measured_.p.template cast<T>();
    // 方向残差
    Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * q_b;
    Eigen::Quaternion<T> delta_q =
        t_ab_measured_.q.template cast<T>() * q_ab_estimated.conjugate();
    residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();
  
    // 通过平方根信息矩阵缩放残差以表示测量值的不确定性
    residuals.applyOnTheLeft(sqrt_information_.template cast<T>());
    return true;
  }

  // 静态方法用于创建代价函数实例
  static ceres::CostFunction* Create(
      const Pose3d& t_ab_measured,
      const Eigen::Matrix<double, 6, 6>& sqrt_information) {
    // 自动求导方式，残差维度为6，后续为a时刻位置，a时刻方向，b时刻位置，b时刻方向
    return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>(
        new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information));
  }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 private:
  // a和b时刻之间的相对位姿变换
  const Pose3d t_ab_measured_;
  // 测量协方差矩阵的倒数平方根
  const Eigen::Matrix<double, 6, 6> sqrt_information_;
};

残差的计算方式和上文的公式一一对应，还是比较简单清晰的。

using MapOfPoses =
  std::map<int, Pose3d, std::less<int>,
  Eigen::aligned_allocator<std::pair<const int, Pose3d>>>;

// 两个时刻点之间的相对位姿的变换
struct Constraint3d {
  int id_begin;
  int id_end;
  Pose3d t_be;
  Eigen::Matrix<double, 6, 6> information;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

// 从位姿图约束构造非线性最小二乘优化问题
void BuildOptimizationProblem(const VectorOfConstraints& constraints,
                              MapOfPoses* poses,
                              ceres::Problem* problem) {
  ceres::LossFunction* loss_function = nullptr;
  ceres::Manifold* quaternion_manifold = new EigenQuaternionManifold;
  for (const auto& constraint : constraints) {
    auto pose_begin_iter = poses->find(constraint.id_begin);
    auto pose_end_iter = poses->find(constraint.id_end);
    // llt = LL^T，cholesky分解，求解信息矩阵的平方根
    const Eigen::Matrix<double, 6, 6> sqrt_information =
        constraint.information.llt().matrixL();
    ceres::CostFunction* cost_function =
        PoseGraph3dErrorTerm::Create(constraint.t_be, sqrt_information);
    problem->AddResidualBlock(cost_function,
                              loss_function,
                              pose_begin_iter->second.p.data(),
                              pose_begin_iter->second.q.coeffs().data(),
                              pose_end_iter->second.p.data(),
                              pose_end_iter->second.q.coeffs().data());
    problem->SetManifold(pose_begin_iter->second.q.coeffs().data(),
                         quaternion_manifold);
    problem->SetManifold(pose_end_iter->second.q.coeffs().data(),
                         quaternion_manifold);
  }
  // 规范自由度
  auto pose_start_iter = poses->begin();
  problem->SetParameterBlockConstant(pose_start_iter->second.p.data());
  problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
}

// 解决最小二乘的优化问题
bool SolveOptimizationProblem(ceres::Problem* problem) {
  CHECK(problem != nullptr);
  ceres::Solver::Options options;
  options.max_num_iterations = 200;
  options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
  ceres::Solver::Summary summary;
  ceres::Solve(options, problem, &summary);
  std::cout << summary.FullReport() << '\n';
  return summary.IsSolutionUsable();
}

int main(int argc, char** argv) {
  ceres::examples::MapOfPoses poses;
  ceres::examples::VectorOfConstraints constraints;
  // 读取数据（略）
  ReadG2oFile(FLAGS_input, &poses, &constraints);

  ceres::Problem problem;
  ceres::examples::BuildOptimizationProblem(constraints, &poses, &problem);
  ceres::examples::SolveOptimizationProblem(&problem);
  return 0;
}

代码大多是比较正常的Ceres的问题处理流程。

效果图

运行程序，利用官方提供的plot_results.py可以画出原始和优化后的位姿地图，类似下图：

相关阅读

• Ceres-solver examples之pose_graph_2d学习笔记
• Ceres-solver examples之pose_graph_3d学习笔记