【Ceres】Ceres学习笔记2 BA相关

7. Bundle Adjustment

​ 给定一组测量的图像特征位置和对应关系，Bundle Adjustment 的目标是找到最小化重投影误差的 3D 点位置和相机参数。 该优化问题通常被表述为非线性最小二乘问题，其中误差是观察到的特征位置与相机图像平面上相应 3D 点的投影之间的差异的平方范数。 Ceres 为解决 Bundle Adjustment 问题提供了广泛的支持。

​ BAL 问题中的每个残差都取决于一个三维点和一个九参数相机。 定义相机的九个参数是：三个用于作为罗德里格斯轴角矢量的旋转，三个用于平移，一个用于焦距，两个用于径向畸变。 此相机型号的详细信息可以在 Bundler 主页和 BAL 主页中找到。

1）读取数据

​ 拿到一个数据集，首先要了解其含义，即每个数据所代表的意义。然后根据其不同的意义分类，思考如何通过代码的方式如何读入数据。

​ 第二次记录：

​ 已经了解到，数据集分为三部分，1 header 相关的个数数据 2 某个相机观察到的3D点以及对应的2D观察点的数据 3 相机参数（9个）以及3D点的坐标

（1）读入第一部分的个数

​ 1 相机个数：int num_cameras_;

​ 2 所观测的现实世界的位置，即3D点数量：int num_points_;

​ 3 观测的2D点的数量：int num_observations_;

​ 以上可以通过通过数据集的第一行得到。

int num_observations() const { return num_observations_; }

FscanfOrDie(fptr, "%d", &num_cameras_);
FscanfOrDie(fptr, "%d", &num_points_);
FscanfOrDie(fptr, "%d", &num_observations_);

（2）每个相机观察到的3D点的索引以及其对应的2D点的两个坐标（第二部分）

​ 思考：只能按行读取数据，一次读取一行中的一个数据，其中一个隐含的条件是第二部分的行数与观测到的2D坐标点的个数相同，所以比较有用的便是 num_observations_ ，其描述了循环读取数据的次数。

​ 每一行中第一个数据表示第几个相机，第二个数据表示观测到的第几个3D点，那么由于 “第几个”，所以定义索引：

    int* point_index_;
    int* camera_index_;

​ 第几个观察到的2D点：double* observations_;

    point_index_ = new int[num_observations_];
    camera_index_ = new int[num_observations_];
    observations_ = new double[2 * num_observations_];

point_index_ = new int[num_observations_];
camera_index_ = new int[num_observations_];
observations_ = new double[2 * num_observations_];
num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
parameters_ = new double[num_parameters_];
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);
    }
}

（3）每个相机的参数以及3D点的坐标（第三部分）

​ 如何读入这部分的数据？
​ 1 首先确定这一部分总共有多少数据：int num_parameters_;

num_parameters_ = 9 * num_cameras_ + 3 * num_points_;

​ 2 通过 “第几个“确定位置

double* parameters_;

​ 3 首先将读取到的参数看作整体，然后再分配：

for (int i = 0; i < num_parameters_; ++i) {
	FscanfOrDie(fptr, "%lf", parameters_ + i);
	}

double* mutable_cameras() { return parameters_; }                   //相机参数首地址
double* mutable_points() { return parameters_ + 9 * num_cameras_; } //3D点的首地址
double* mutable_camera_for_observation(int i) {                     //返回第i个相机
	return mutable_cameras() + camera_index_[i] * 9;
}
double* mutable_point_for_observation(int i) {                      //返回第i个3D点
	return mutable_points() + point_index_[i] * 3;
}

​ 读取数据部分总结：1 了解数据文件格式 。2 明确目的，目的只有一个，即将数据读取入内存，那么首先写个读入函数，其次通过对数据集分类（3部分）用不同的方式读入。3 在读取的过程中需要用到的参数便自然而然的定义出来了。

2）Ceres构建部分

我们从数据集中得到的数据分为两部分，一部分是相机与3D点，3D点的坐标是从世界坐标系测量得到的，由相机参数与3D点可以计算投影到像素平面上的像素值；另一部分是观测图像的本身的像素值，两个像素值构成最小二乘问题。

​ 由公式：
$$P_c=R{P}_w+t$$
​ 然后归一化，去畸变（径向）：$x_{dis}=x(1+k_1{r}^2+k_2{r}^4)$，然后投影到像素平面
$$\begin{gathered} u=f_x{x}_{dis}+c_x \\ v=f_y{y}_{dis}+c_y \end{gathered}$$

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 = 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 - observed_x;
    residuals[1] = predicted_y - observed_y;
    return true;
  }

​

3）编写函数求解 $R{P}_w$

本例中是调用ceres中的函数来求解 $R{P}_w$，在学习视觉Slam十四讲时作业代码中涉及自己编写求解代码，所以在此回顾。

（1）求旋转矩阵R：应用罗德里格斯公式
$$R=cos\theta I+(1-cos\theta )n{n}^T+sin\theta n^{\Lambda }$$
（2）$R{P}_w$
$$R{P}_w=P_{w}cos\theta +(1-cos\theta )n(n{P}_w)+sin\theta n\times P_w$$
于是将一个大问题分解为求 $n$ 与 $P_w$ 的点乘以及 $n$ 与 $P_w$ 的叉乘。

出现了新问题：n 与 $\theta$ 如何求解

（3）n 与 $\theta$

​ 任意旋转都可以用一个旋转轴和一个旋转角来刻画，所以可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角，这种向量称为旋转向量（角轴），用数学语言表示为 $\theta n$，其中n为旋转轴且是一个单位长度的向量， $\theta$ 为角度。

​ 所以很显然 ：
$$\theta n\cdot \theta n={\theta }^2$$
​ 角轴已知，我们假设为 angle 所以可以求得 $n=\frac{angle}{\theta }$

​ 因为角 $\theta$ 可能为 0，所以在编写代码时需要分类讨论。

突然发现十四讲的代码也是调用的ceres库的。。。

4）ceres求解部分

	BALProblem bal_problem;
    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]);
        problem.AddResidualBlock(cost_function,
                                nullptr /* squared loss */,
                                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() << "\n";