Ceres学习笔记005_使用Ceres解决简单光束平差问题

给定一组图像序列的二维观测图像特征点位置及对应关系，BA的目标是计算相机内外参数以及这些特征点的3D坐标，使得重投影误差最小。该优化问题通常可被描述成非线性最小二乘问题，误差为观测到的二维图像特征点与对应3D点投影到相机二维图像像素平面坐标之间的差。
编写Ceres的主要原因之一也是为了解决大规模光束平差问题（Bundle Adjustment）。

1 构建最小二乘问题

使用小孔成像模型，每个相机待估计的参数有旋转矩阵$R$，平移向量$t$，像素焦距$f$，两个径向畸变参数$k_1$和$k_2$,将世界坐标系中的3D空间点投影到图像像素空间的过程如下：

$P=R\ast X+t$ (1)
式中， $X$为 3D空间点在世界坐标系中的描述， $P$为 3D空间点在相机坐标系中的描述；
$p=P/P_z$ (2)
式中， $p$为 3D空间点投影到归一化图像物理平面上的 2D投影点， $P_z$为点 $P$在 Z轴方向的坐标分量；
$p^{\prime }=f\ast r(p)\ast p$ (3)
式中， $r_p$为关于点 $p$的径向畸变函数，如下：
$r(p)=1.0+k1\ast ||p|{|}^2+k2\ast ||p|{|}^4$ (4)
式中， $||p||=\sqrt{p_x^2+p_y^2}$。
最终构建成的非线性最小二乘问题如下：
$\text{min}\frac{1}{2}{\sum }_{i=1}^m{\sum }_{j=1}^{n_i}||q_{i,j}^x-p_{i,j}^{\prime x}|{|}^2$ (5)

$\text{min}\frac{1}{2}{\sum }_{i=1}^m{\sum }_{j=1}^{n_i}||q_{i,j}^y-p_{i,j}^{\prime y}|{|}^2$ (6)
式中， $m$为相机总数量， $n_i$为第 $i$个相机中能够观测到所有 3D空间点的总数量， $q$为相机观测到的二维图像特征点像素坐标。
注：

1. 上述相机成像模型中，并没有对主点坐标进行优化，即认为镜头光轴与相机成像平面的交点就是图像中心点；
2. 上述相机成像模型中，二维图像像素坐标，不管是投影点还是观测点，都描述在以图像中心点为原点的图像像素坐标系中。

2 数据

本文档使用的数据来源于BAL数据集（Bundle Adjustment in the Large）。
数据格式如下：

// <相机总数量><3D空间点总数量><2D观测点总数量>
// 一个3D空间点可能被多个相机观测到，即存在多个2D观测点
<num_cameras> <num_points> <num_observations>
// <相机1><3D空间点1><3D空间点1对应在相机1中的2D观测点像素坐标>
<camera_index_1> <point_index_1> <x_1> <y_1>
...
// <相机m><3D空间点n><3D空间点n对应在相机m中的2D观测点像素坐标>
<camera_index_num_observations> <point_index_num_observations> <x_num_observations> <y_num_observations>
// 相机1的参数，共9个参数，按照R(Rodrigues向量),t,f,k1,k2的次序
<camera_1>
...
// 相机m的参数，共9个参数，按照R(Rodrigues向量),t,f,k1,k2的次序
<camera_num_cameras>
// 3D空间点1的参数，共3个参数，按照x,y,z的次序   
<point_1>
...
// 3D空间点n的参数，共3个参数，按照x,y,z的次序  
<point_num_points>

另外需要说明的是，该数据集对相机坐标系的方向定义为，X轴朝右，Y轴朝上，Z轴朝后，即相机朝向为-Z轴方向，因此，对于该数据集，需要对式（2）进行如下修改：

$p=-P/P_z$ (7) 同样的，该数据集中，图像像素坐标系的方向也定义为， X轴朝右， Y轴朝上。

3 代码实践

3.1 用户自定义残差计算模型

/*
* 用户自定义残差计算模型
* 相机模型中使用9个参数:3个参数用于旋转，3个参数用于平移，1个参数用于焦距，2个参数用于径向畸变。
* 主点没有作为参数体现在模型中(即假设主点位于图像中心)。
*/
struct SnavelyReprojectionError
{
    // 实例化模型时，传入2D观测点像素坐标
    SnavelyReprojectionError(double observed_x, double observed_y)
        : observed_x(observed_x), observed_y(observed_y) {}

    // 计算残差的过程主要就是将世界坐标系中的3D点投影到相机图像像素坐标系的过程
    template <typename T>
    bool operator()(const T* const camera, const T* const point, T* residuals) const 
    {
        // 首先将世界坐标系中的3D点转换到相机坐标系中
        // camera[0,1,2] 是世界坐标系相对于相机坐标系姿态的Rodrigues向量
        T p[3];
        ceres::AngleAxisRotatePoint(camera, point, p);
        // camera[3,4,5] 是世界坐标系相对于相机坐标系的位置
        p[0] += camera[3];
        p[1] += camera[4];
        p[2] += camera[5];

        // 将相机坐标系中的点投影到图像归一化平面物理坐标系
        // 加负号是因为Noah Snavely将相机的朝向定义为-Z轴方向
        // 因此3D点的Z坐标分量通常是负值
        T xp = -p[0] / p[2];
        T yp = -p[1] / p[2];

        // 畸变矫正
        // camera[7,8] 是径向畸变系数
        const T& l1 = camera[7];
        const T& l2 = camera[8];
        T r2 = xp * xp + yp * yp;
        T distortion = 1.0 + r2 * (l1 + l2 * r2);

        // 将归一化平面物理坐标系中的点投影到图像像素坐标系（原点在图像中心）
        // camera[6] 是相机像素焦距
        const T& focal = camera[6];
        T predicted_x = focal * distortion * xp;
        T predicted_y = focal * distortion * yp;

        // 定义目标函数：预测的投影点与实际观测到的点之间的坐标偏差
        residuals[0] = predicted_x - observed_x;
        residuals[1] = predicted_y - observed_y;

        return true;
    }

    // 隐藏通过Ceres构建代价函数的细节
    static ceres::CostFunction* Create(const double observed_x,
        const double observed_y) 
    {
        // 构建Ceres代价函数CostFuntion，用来计算残差，残差计算方法为用户自定义残差计算模型SnavelyReprojectionError
        // 本例中使用自动微分方法AutoDiffCostFunction来计算导数
        // 本例中只存在一个代价函数
        // 本例中输出残差维度为2，输出参数块有2个，维度分别为9和3
        return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>
            (new SnavelyReprojectionError(observed_x, observed_y)));
    }

    double observed_x;  // 观测点2D像素坐标
    double observed_y;
};

对于光束法平差问题，模型比较复杂，若使用解析解来计算导数，会很复杂，而自动微分法使得建模过程变得简单，因此本示例采用自动微分法。

3.2 构建Ceres问题Problem

    // 构建非线性最小二乘问题
    ceres::Problem problem;
    for (int i = 0; i < bal_problem.num_observations(); ++i) {
        // 构建代价函数，将每个观测点的像素坐标作为参数输入到用户自定义残差模型中
        ceres::CostFunction* cost_function = SnavelyReprojectionError::Create(
            observations[2 * i + 0], observations[2 * i + 1]);
        // 添加残差块，需要依次指定代价函数，损失函数，参数块
        // 本例中损失函数为单位函数
        // 每个残差块将上述2D观测点对应的3D空间点和相机作为输入参数块，最终输出一个2维的残差
        problem.AddResidualBlock(cost_function,
            NULL,
            bal_problem.mutable_camera_for_observation(i),
            bal_problem.mutable_point_for_observation(i));
    }

每个观测点都能构建一个残差块，从而添加到Problem中。

3.3 配置求解器参数Options

    // 配置求解器参数
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.minimizer_progress_to_stdout = true;

由于光束法平差问题是一个大规模稀疏问题（至少对于DENSE_QR求解方法而言很大），解决该问题的方法是将ceres::Solver::Options::linear_solver_type设置为SPARSE_NORMAL_CHOLESKY，尽管这是合理的，但光束法平差问题是一种特殊的稀疏问题，因此存在更有效的方法。Ceres为此提供了3个专用的求解器（统称为基于SCHUR的求解器）。上述示例代码用了最简单的DENSE_SCHUR，另外两个求解器分别是SPARSE_SCHUR和ITERATIVE_SCHUR。

3.4 完整过程及结果

#include 
#include 
#include 

#include "ceres/ceres.h"
#include "ceres/rotation.h"

// BALProblem用于加载数据
class BALProblem 
{
public:
    ~BALProblem() 
    {
        delete[] point_index_;
        delete[] camera_index_;
        delete[] observations_;
        delete[] parameters_;
    }

    // 获取2D观测点总数量
    int num_observations() const { return num_observations_; }
    // 获取存储2D观测点像素坐标数组的地址
    const double* observations() const { return observations_; }
    // 获取存储所有待优化的相机参数数组的地址
    double* mutable_cameras() { return parameters_; }
    // 获取存储所有待优化的3D空间点数组的地址
    double* mutable_points() { return parameters_ + 9 * num_cameras_; }
    // 获取第i个观测点所属相机的地址
    double* mutable_camera_for_observation(int i) { return mutable_cameras() + camera_index_[i] * 9; }
    // 获取第i个观测点所属3D空间点的地址
    double* mutable_point_for_observation(int i) { return mutable_points() + point_index_[i] * 3; }

    // 从数据集文件中加载数据
    bool LoadFile(const char* filename) 
    {
        FILE* fptr = fopen(filename, "r");
        if (fptr == NULL) {
            return false;
        };

        // 读取相机总数量
        FscanfOrDie(fptr, "%d", &num_cameras_);
        // 读取3D空间点总数量
        FscanfOrDie(fptr, "%d", &num_points_);
        // 读取2D观测点总数量
        FscanfOrDie(fptr, "%d", &num_observations_);

        // 用于存储每个观测点对应3D空间点索引的数组
        point_index_ = new int[num_observations_];
        // 用于存储每个观测点对应相机索引的数组
        camera_index_ = new int[num_observations_];
        // 用于存储所有观测点的2D像素坐标
        observations_ = new double[2 * num_observations_];

        // 所有待优化参数的数量
        num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
        // 用于存储所有待优化的参数，次序为所有待优化的相机参数 + 所有待优化的3D空间点参数
        parameters_ = new double[num_parameters_];

        // 依次读取每个2D观测点对应的相机索引、3D点索引、观测点像素坐标
        for (int i = 0; i < num_observations_; ++i) {
            FscanfOrDie(fptr, "%d", camera_index_ + i);
            FscanfOrDie(fptr, "%d", point_index_ + i);
            for (int j = 0; j < 2; ++j) {
                FscanfOrDie(fptr, "%lf", observations_ + 2 * i + j);
            }
        }

        // 读取所有待优化参数的初值
        for (int i = 0; i < num_parameters_; ++i) {
            FscanfOrDie(fptr, "%lf", parameters_ + i);
        }
        return true;
    }

private:
    template <typename T>
    void FscanfOrDie(FILE* fptr, const char* format, T* value) 
    {
        int num_scanned = fscanf(fptr, format, value);
        if (num_scanned != 1) {
            LOG(FATAL) << "Invalid UW data file.";
        }
    }

    int num_cameras_;   // 相机总数量
    int num_points_;    // 3D空间点总数量
    int num_observations_;  // 2D观测点总数量
    int num_parameters_;    // 所有待优化参数的数量

    int* point_index_;  // 用于存储每个观测点对应3D空间点索引的数组
    int* camera_index_; // 用于存储每个观测点对应相机索引的数组
    double* observations_;  // 用于存储所有观测点的2D像素坐标
    double* parameters_;    // 用于存储所有待优化的参数，次序为所有待优化的相机参数 + 所有待优化的3D空间点参数
};

/*
* 用户自定义残差计算模型
* 相机模型中使用9个参数:3个参数用于旋转，3个参数用于平移，1个参数用于焦距，2个参数用于径向畸变。
* 主点没有作为参数体现在模型中(即假设主点位于图像中心)。
*/
struct SnavelyReprojectionError
{
    // 实例化模型时，传入2D观测点像素坐标
    SnavelyReprojectionError(double observed_x, double observed_y)
        : observed_x(observed_x), observed_y(observed_y) {}

    // 计算残差的过程主要就是将世界坐标系中的3D点投影到相机图像像素坐标系的过程
    template <typename T>
    bool operator()(const T* const camera, const T* const point, T* residuals) const 
    {
        // 首先将世界坐标系中的3D点转换到相机坐标系中
        // camera[0,1,2] 是世界坐标系相对于相机坐标系姿态的Rodrigues向量
        T p[3];
        ceres::AngleAxisRotatePoint(camera, point, p);
        // camera[3,4,5] 是世界坐标系相对于相机坐标系的位置
        p[0] += camera[3];
        p[1] += camera[4];
        p[2] += camera[5];

        // 将相机坐标系中的点投影到图像归一化平面物理坐标系
        // 加负号是因为Noah Snavely将相机的朝向定义为-Z轴方向
        // 因此3D点的Z坐标分量通常是负值
        T xp = -p[0] / p[2];
        T yp = -p[1] / p[2];

        // 畸变矫正
        // camera[7,8] 是径向畸变系数
        const T& l1 = camera[7];
        const T& l2 = camera[8];
        T r2 = xp * xp + yp * yp;
        T distortion = 1.0 + r2 * (l1 + l2 * r2);

        // 将归一化平面物理坐标系中的点投影到图像像素坐标系（原点在图像中心）
        // camera[6] 是相机像素焦距
        const T& focal = camera[6];
        T predicted_x = focal * distortion * xp;
        T predicted_y = focal * distortion * yp;

        // 定义目标函数：预测的投影点与实际观测到的点之间的坐标偏差
        residuals[0] = predicted_x - observed_x;
        residuals[1] = predicted_y - observed_y;

        return true;
    }

    // 隐藏通过Ceres构建代价函数的细节
    static ceres::CostFunction* Create(const double observed_x,
        const double observed_y) 
    {
        // 构建Ceres代价函数CostFuntion，用来计算残差，残差计算方法为用户自定义残差计算模型SnavelyReprojectionError
        // 本例中使用自动微分方法AutoDiffCostFunction来计算导数
        // 本例中只存在一个代价函数
        // 本例中输出残差维度为2，输出参数块有2个，维度分别为9和3
        return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>
            (new SnavelyReprojectionError(observed_x, observed_y)));
    }

    double observed_x;  // 观测点2D像素坐标
    double observed_y;
};

int main(int argc, char** argv) 
{
    google::InitGoogleLogging(argv[0]);

    std::string str_file_path = "data/BundleAdjuster/problem-49-7776-pre.txt";
    BALProblem bal_problem;
    if (!bal_problem.LoadFile(str_file_path.c_str())) {
        std::cerr << "ERROR: unable to open file " << argv[1] << "\n";
        return 1;
    }

    const double* observations = bal_problem.observations();

    // 构建非线性最小二乘问题
    ceres::Problem problem;
    for (int i = 0; i < bal_problem.num_observations(); ++i) {
        // 构建代价函数，将每个观测点的像素坐标作为参数输入到用户自定义残差模型中
        ceres::CostFunction* cost_function = SnavelyReprojectionError::Create(
            observations[2 * i + 0], observations[2 * i + 1]);
        // 添加残差块，需要依次指定代价函数，损失函数，参数块
        // 本例中损失函数为单位函数
        // 每个残差块将上述2D观测点对应的3D空间点和相机作为输入参数块，最终输出一个2维的残差
        problem.AddResidualBlock(cost_function,
            NULL,
            bal_problem.mutable_camera_for_observation(i),
            bal_problem.mutable_point_for_observation(i));
    }

    // 配置求解器参数
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.minimizer_progress_to_stdout = true;

    // 输出日志内容
    ceres::Solver::Summary summary;

    // 开始优化求解
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.FullReport() << std::endl;
    std::system("pause");
    return 0;
}

结果如下：

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  8.509125e+05    0.00e+00    8.57e+06   0.00e+00   0.00e+00  1.00e+04        0    5.79e-02    1.27e-01
   1  4.648193e+04    8.04e+05    3.55e+06   2.10e+02   9.61e-01  3.00e+04        1    1.92e-01    3.20e-01
   2  1.481752e+04    3.17e+04    4.47e+05   3.30e+02   9.60e-01  9.00e+04        1    1.82e-01    5.03e-01
   3  1.346029e+04    1.36e+03    3.80e+04   5.21e+02   9.70e-01  2.70e+05        1    1.81e-01    6.84e-01
   4  1.343304e+04    2.73e+01    4.68e+04   1.02e+03   3.56e-01  2.64e+05        1    1.92e-01    8.77e-01
   5  1.338876e+04    4.43e+01    1.90e+04   1.23e+03   7.88e-01  3.26e+05        1    1.88e-01    1.06e+00
   6  1.337551e+04    1.33e+01    1.40e+04   1.64e+03   6.47e-01  3.34e+05        1    1.89e-01    1.26e+00
   7  1.336596e+04    9.54e+00    7.09e+03   1.78e+03   7.75e-01  4.00e+05        1    2.05e-01    1.46e+00
   8  1.336049e+04    5.48e+00    5.24e+03   2.21e+03   7.44e-01  4.54e+05        1    1.82e-01    1.66e+00
   9  1.335651e+04    3.98e+00    3.31e+03   2.59e+03   7.77e-01  5.46e+05        1    1.60e-01    1.82e+00
  10  1.335360e+04    2.90e+00    2.36e+03   3.21e+03   7.75e-01  6.55e+05        1    1.54e-01    1.97e+00
  11  1.335141e+04    2.19e+00    1.59e+03   3.94e+03   7.81e-01  7.97e+05        1    1.44e-01    2.12e+00
  12  1.334976e+04    1.65e+00    1.07e+03   4.89e+03   7.83e-01  9.74e+05        1    1.44e-01    2.26e+00
  13  1.334851e+04    1.25e+00    7.13e+02   6.08e+03   7.86e-01  1.20e+06        1    1.40e-01    2.40e+00
  14  1.334756e+04    9.51e-01    4.78e+02   7.57e+03   7.88e-01  1.48e+06        1    1.42e-01    2.54e+00
  15  1.334683e+04    7.28e-01    3.27e+02   9.46e+03   7.89e-01  1.83e+06        1    1.36e-01    2.68e+00
  16  1.334627e+04    5.61e-01    2.29e+02   1.18e+04   7.91e-01  2.28e+06        1    1.39e-01    2.82e+00
  17  1.334583e+04    4.35e-01    1.65e+02   1.49e+04   7.92e-01  2.85e+06        1    1.42e-01    2.96e+00
  18  1.334549e+04    3.38e-01    1.21e+02   1.87e+04   7.93e-01  3.57e+06        1    1.36e-01    3.10e+00
  19  1.334523e+04    2.65e-01    9.10e+01   2.35e+04   7.93e-01  4.48e+06        1    1.33e-01    3.23e+00
  20  1.334502e+04    2.08e-01    6.97e+01   2.95e+04   7.94e-01  5.62e+06        1    1.37e-01    3.37e+00
  21  1.334486e+04    1.64e-01    5.44e+01   3.72e+04   7.94e-01  7.05e+06        1    1.35e-01    3.51e+00
  22  1.334473e+04    1.29e-01    4.32e+01   4.68e+04   7.94e-01  8.86e+06        1    1.34e-01    3.64e+00
  23  1.334463e+04    1.01e-01    3.51e+01   5.26e+04   7.97e-01  1.12e+07        1    1.41e-01    3.78e+00
  24  1.334455e+04    7.79e-02    2.61e+01   5.92e+04   8.00e-01  1.43e+07        1    1.32e-01    3.91e+00
  25  1.334449e+04    6.09e-02    2.18e+01   7.05e+04   8.00e-01  1.82e+07        1    1.42e-01    4.06e+00
  26  1.334444e+04    4.82e-02    2.02e+01   8.68e+04   7.99e-01  2.32e+07        1    1.40e-01    4.20e+00
  27  1.334440e+04    3.81e-02    1.90e+01   9.83e+04   8.00e-01  2.96e+07        1    1.38e-01    4.34e+00
  28  1.334437e+04    2.97e-02    1.97e+01   9.60e+04   8.03e-01  3.81e+07        1    1.38e-01    4.47e+00
  29  1.334435e+04    2.32e-02    2.13e+01   1.04e+05   8.06e-01  4.93e+07        1    1.35e-01    4.61e+00
  30  1.334433e+04    1.86e-02    2.36e+01   1.20e+05   8.05e-01  6.37e+07        1    1.39e-01    4.75e+00
  31  1.334432e+04    1.42e-02    2.53e+01   1.33e+05   8.19e-01  8.62e+07        1    1.37e-01    4.89e+00

Solver Summary (v 2.0.0-eigen-(3.3.8)-no_lapack-eigensparse-no_openmp)

                                     Original                  Reduced
Parameter blocks                         7825                     7825
Parameters                              23769                    23769
Residual blocks                         31843                    31843
Residuals                               63686                    63686

Minimizer                        TRUST_REGION

Dense linear algebra library            EIGEN
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver                     DENSE_SCHUR              DENSE_SCHUR
Threads                                     1                        1
Linear solver ordering              AUTOMATIC                  7776,49
Schur structure                         2,3,9                    2,3,9

Cost:
Initial                          8.509125e+05
Final                            1.334432e+04
Change                           8.375681e+05

Minimizer iterations                       32
Successful steps                           32
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                         0.069075

  Residual only evaluation           0.260949 (32)
  Jacobian & residual evaluation     1.540338 (32)
  Linear solver                      2.758068 (32)
Minimizer                            4.913483

Postprocessor                        0.001599
Total                                4.984157

Termination:                      CONVERGENCE (Function tolerance reached. |cost_change|/cost: 8.233123e-07 <= 1.000000e-06)

注：本示例数据集中一共有31843个观测点，最终收敛后的总残差约为13344像素，平均每个观测点的误差为0.42像素。
本文仅是光束法平差比较简单的示例，关于更复杂的示例，将会涉及到Ceres中更多更高级的功能，包括各种线性求解器、鲁棒损失函数、流形等等。