slam 基本算法 --- 使用【Ceres】进行slam中BA问题求解（理论+实践）

最新推荐文章于 2025-02-18 22:27:25 发布

GuNight

最新推荐文章于 2025-02-18 22:27:25 发布

阅读量2.7k

点赞数 2

CC 4.0 BY-SA版权

分类专栏： slam 文章标签： ceres BA slam

本文链接：https://blog.csdn.net/CGJustDoIT/article/details/108537503

slam 基本算法 --- 使用【Ceres】进行slam中BA问题求解（理论+实践）

注
1. 简要
2. ceres求解关键步骤
3.3 代码实践

注

ceres 相比g2o 能自动对误差项关于优化变量的导数求导，简易方便很多；
后续再具体的原理，这里先暂时应用。

1. 简要

ceres 解BA 官方链接： http://www.ceres-solver.org/nnls_tutorial.html#bundle-adjustment

BAL 数据集说明：http://grail.cs.washington.edu/projects/bal/
重投影误差函数构建详见 Camera Model

This gives a projection in pixels, where the origin of the image is the center of the image, the positive x-axis points right, and the positive y-axis points up (in addition, in the camera coordinate system, the positive z-axis points backwards, so the camera is looking down the negative z-axis, as in OpenGL).

2. ceres求解关键步骤

2.1 定义误差项结构

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