ceres学习3

每个残差取决于一个三维点和一个九参数相机。定义相机的九个参数是：三个用于作为 Rodrigues 轴角矢量的旋转，三个用于平移，一个用于焦距，两个用于径向失真。

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 {
   
    T p[3];
    ceres::AngleAxisRotatePoint(camera, point, p);  //通过camera前三个参数对point进行旋转，并存到p中
    
    p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5]; //通过camera中间三个参数对point进行平移，并存到p中

    
    T xp = - p[0] / p[2];
    T yp = - p[1] / p[2];  //投影到归一化平面

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

    
    const T& focal = camera[6];
    T predicted_x = focal * distortion * xp;
    T predicted_y = focal * distortion * yp;  //利用正畸系数和内参，计算投影的像素坐标

   
    residuals[0] = predicted_x - T(observed_x);
    residuals[1] = predicted_y - T(observed_y);  //重投影误差
    return true;
  }

   //设置代价函数
   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;
};

AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
                 new SnavelyReprojectionError(observed_x, observed_y)

其中，2表示输出的残差维度，9表示第一个输入的维度，3表示第二个输入的维度。

构建problem，添加误差项

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,
                           nullptr /* 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";