Ceres学习（四）_angleaxisrotatepoint-CSDN博客

Bundle Adjustment

写Ceres的一个很重要的原因就是，需要解决大尺度BA问题。
给定一组测量的图像特征点位置和对应关系，BA的目标是找到使得重投影误差最小的3D点位置和相机参数。该优化问题通常表述为非线性最小二乘问题，其中误差是所观察到的特征点位置和对应的3D点在相机平面上的重投影位置差的平方 $L2$ 范数。Ceres用于很多解决BA问题的支持。
现在来解决BAL数据集问题。
第一步，定义一个模板类函数，用于计算重投影误差。函数的构造类似于ExponentialResidual，这里有一个对每一帧图像观测点的例子。
BAL问题中的每个残差依赖于一个三维点和含有九个参数的相机参数。定义相机的九个参数是：三个用于旋转的轴角矢量，三个用于平移，一个用于焦距，两个用于径向失真。相机模型的细节可以参考Bundler主页和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;
};

注意，和之前的例子不一样，这是一个不一样的函数，计算它的雅克比矩阵有些痛苦。自动求导使其变得简单。函数AngleAxisRotatePoint()和其他用于操作旋转的函数可以在include/ceres/rotation.h找到。
根据这个函数，BA问题可以构建为如下：

ceres::Problem problem;
for (int i = 0; i < bal_problem.num_observations(); ++i) {
  ceres::CostFunction* cost_function =
      SnavelyReprojectionError::Create(
           bal_problem.observations()[2 * i + 0],
           bal_problem.observations()[2 * i + 1]);
  problem.AddResidualBlock(cost_function,
                           NULL /* squared loss */,
                           bal_problem.mutable_camera_for_observation(i),
                           bal_problem.mutable_point_for_observation(i));
}

注意，BA问题的构建和曲线拟合例子很相似——每一个观测值，都有一项添加到目标函数上。
由于这是一个很大的稀疏问题，解决此问题的一个方法是将Solver::Options::linear_solver_type 设置为SPARSE_NORMAL_CHOLESKY，并且调用Solve。并且尽管这是一个合理的事情，BA问题有一个特殊的稀疏结构，可以利用从而有效解决问题。Ceres为这个任务提供了三个专门的求解器，统称为基于Schur的求解器。样例代码使用了最简单的一种：DENSE_SCHUR。

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";