SLAM实践：Ceres BA

准备工作

Ubuntu 及衍生版本用户安装 MeshLab 1.3.3

大体介绍：

使用BAL数据集进行BA演示实验。BAL数据集提供若干场景，每个场景的相机和路标点信息由一个文本文件给定，本实践以problem-16-22106-pre.txt文件为例：
本次实践用common.h中定义的BALProblem类读入该文件的内容，然后用Ceres求解。

• ceres同样包含common（提供了命令解析commandargs、参数配置bundleparams、数据存储读写BALProblem、重投影函数projection及常用数学工具tools）和data（存放txt格式数据集）。除此之外包含SnavelyReprojectionError.h(计算重投影误差代价函数)以及ceresBundle.cpp(ceres优化)

• Ceres对BA的介绍如下：
  给定一系列测得的图像，包含特征点位置和对应关系。BA的目标就是，通过最小化重投影误差，确定三维空间点的位置和相机参数。这个优化问题通常被描述为非线性最小二乘法问题，要最小化的目标函数即为测得图像中特征点位置和相机成像平面上的对应的投影之间的差的平方。

• 定义误差类,承接观测数据,计算误差,生成误差雅克比,使用了common中的projection.h及tools/rotation.h
  第一步:定义误差类,通过构造函数读取观测值坐标
  第二步:通过重载的括号函数用于计算重投影误差,与g2o_bal_class中类似,参数为camera,point和用于承接结果的predictions
  第三步:通过静态成员函数create(观测值),直接生成误差对应雅克比矩阵代价函数对象的指针,作为向优化器中添加问题函数的第一个参数.

• 使用ceres的自动求导模板类创建,模板参数为误差类,输出维度,输入维度(camera,point),构造函数参数为具体模板参数设定的对象.使用create函数传入的参数观测值实例化一个误差对象,将该误差对象用于实例化误差雅克比AutoDiffCostFunction对象.

1. SnavelyReprojectionError.h 定义出投影误差模型

第一步依然使定义一个模板functor来计算残差，在这个问题中也就是的重投影误差。每个残差值依赖于空间点的位置（3个参数）和相机参数（9个参数）。九个相机参数包含用Rodriques’ axis-angle表示的旋转向量（3个参数），平移参数（3个参数），以及焦距和两个径向畸变参数。相机模型的详细描述也可以在BAL的主页找到。

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 {
                  // 重载操作符（）
    // camera[0,1,2] are the angle-axis rotation.
    T p[3];
    ceres::AngleAxisRotatePoint(camera, point, p);
    // camera[3,4,5] are the translation.
    p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];

    // Compute the center of distortion. The sign change comes from
    // the camera model that Noah Snavely's Bundler assumes, whereby
    // the camera coordinate system has a negative z axis.
    T xp = - p[0] / p[2];
    T yp = - p[1] / p[2];

    // Apply second and fourth order radial distortion.
    const T& l1 = camera[7];
    const T& l2 = camera[8];
    T r2 = xp*xp + yp*yp;
    T distortion = T(1.0) + r2  * (l1 + l2  * r2);

    // Compute final projected point position.
    const T& focal = camera[6];
    T predicted_x = focal * distortion * xp;
    T predicted_y = focal * distortion * yp;

    // The error is the difference between the predicted and observed position.
    residuals[0] = predicted_x - T(observed_x);
    residuals[1] = predicted_y - T(observed_y);
    return true;
  }

   // Factory to hide the construction of the CostFunction object from
   // the client code.
   static ceres::CostFunction* Create(const double observed_x,
                                      const double observed_y) {
     return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
                 new SnavelyReprojectionError(observed_x, observed_y)));
   }

  double observed_x;
  double observed_y;
};

该类的括号运算符实现了Ceres计算误差的接口，实际的计算在CamProjectionWithDistortion函数中。该类的静态函数Create作为外部调用接口，直接返回一个可自动求导的Ceres代价函数。只需调用Create函数，把代价函数放入ceres::Problem.

2. bundle_adjustment_ceres.cpp BA的搭建和求解

#include <iostream>
#include <ceres/ceres.h>
#include "common.h"
#include "SnavelyReprojectionError.h"

using namespace std;

void SolveBA(BALProblem &bal_problem);

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "usage: bundle_adjustment_ceres bal_data.txt" << endl;
        return 1;
    }

    BALProblem bal_problem(argv[1]); //使用BALProblem类读取数据
    bal_problem.Normalize(); // 对原始数据归一化
    bal_problem.Perturb(0.1, 0.5, 0.5); //给数据增加噪声
    bal_problem.WriteToPLYFile("initial.ply"); // 将结果写入ply文件（一种点云文件格式）
    SolveBA(bal_problem); // 调用，解出BA
    bal_problem.WriteToPLYFile("final.ply");

    return 0;
}

void SolveBA(BALProblem &bal_problem) {
	// 读出路标和相机的维度，
    const int point_block_size = bal_problem.point_block_size();
    const int camera_block_size = bal_problem.camera_block_size();
    // 还有路标和相机的数据首位置
    double *points = bal_problem.mutable_points();
    double *cameras = bal_problem.mutable_cameras();

    // Observations is 2 * num_observations long array observations
    // [u_1, u_2, ... u_n], where each u_i is two dimensional, the x
    // and y position of the observation.
    const double *observations = bal_problem.observations();
    ceres::Problem problem;

    for (int i = 0; i < bal_problem.num_observations(); ++i) {
        ceres::CostFunction *cost_function;

        // Each Residual block takes a point and a camera as input
        // and outputs a 2 dimensional Residual
        // 代价函数：既不是误差，他是误差构造出来的。用法上有点像导数的意味，类似于g2o的雅克比矩阵？
        cost_function = SnavelyReprojectionError::Create(observations[2 * i + 0], observations[2 * i + 1]);

        // If enabled use Huber's loss function.
        // 定义problem->AddResidualBlock()函数中需要的Huber核函数
        ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);

        // Each observation corresponds to a pair of a camera and a point
        // which are identified by camera_index()[i] and point_index()[i]
        // respectively.
        // 定义problem->AddResidualBlock()函数中需要的待估计参数
        double *camera = cameras + camera_block_size * bal_problem.camera_index()[i];
        double *point = points + point_block_size * bal_problem.point_index()[i];
        // 紧接着的数组就是待优化参数了，由于各参数维度不同，所以类型为double*
        problem.AddResidualBlock(cost_function, loss_function, camera, point); // 为每个观测值增加一个残差块
    }

    // show some information here ...
    std::cout << "bal problem file loaded..." << std::endl;
    std::cout << "bal problem have " << bal_problem.num_cameras() << " cameras and "
              << bal_problem.num_points() << " points. " << std::endl;
    std::cout << "Forming " << bal_problem.num_observations() << " observations. " << std::endl;

    std::cout << "Solving ceres BA ... " << endl;
    // 设定求解办法，SPARSE_SCHUR会让ceres实时求解过程为：先对路标部分进行Schur边缘化，以加速的方式求解此问题。
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::LinearSolverType::SPARSE_SCHUR;
    options.minimizer_progress_to_stdout = true;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.FullReport() << "\n";
}