ceres教程（1）

2023-11-28 23:27:16

文章目录

一、介绍
二、简单的例子
- 2.1 定义CostFunction
- 2.2 构建最小二乘问题求解
三、3种求导形式
四、多个CostFunction
- 4.1 定义每一个CostFunction
- 4.2 添加ResidualBlock
五、曲线拟合
- 5.1 定义CostFunction
- 5.2 添加ResidualBlock
六、鲁棒的曲线拟合
七、Bundle Adjustment

官方教程的中文精简版，方便自己和已经有一定基础的同学查看

一、介绍

Ceres 可以解决以下形式的边界约束鲁棒化非线性最小二乘问题

f i ( . ) f_i(.) fi(.)是CostFunction。也就是误差函数，也叫代价函数。
ρ i \rho_i ρi是LossFunction。LossFunction 是一个标量函数，用于减少异常值对非线性最小二乘问题的解决方案的影响。

二、简单的例子

f ( x ) = 1 2 ( 10 − x ) 2 f(x)=\frac{1}{2}(10-x)^2 f(x)=21(10−x)2

2.1 定义CostFunction

struct CostFunctor {
   template <typename T>
   bool operator()(const T* const x, T* residual) const {
     residual[0] = 10.0 - x[0];
     return true;
   }
};

2.2 构建最小二乘问题求解

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

  // The variable to solve for with its initial value.
  double initial_x = 5.0;
  double x = initial_x;

  // Build the problem.
  Problem problem;

  // Set up the only cost function (also known as residual). This uses
  // auto-differentiation to obtain the derivative (jacobian).
  CostFunction* cost_function =
      new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
  problem.AddResidualBlock(cost_function, nullptr, &x);

  // Run the solver!
  Solver::Options options;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = true;
  Solver::Summary summary;
  Solve(options, &problem, &summary);

  std::cout << summary.BriefReport() << "\n";
  std::cout << "x : " << initial_x
            << " -> " << x << "\n";
  return 0;
}

三、3种求导形式

3.1 自动求导

应用于已知CostFunction形式的场合，使用方法如2.2小节中所示

struct CostFunctor {
   template <typename T>
   bool operator()(const T* const x, T* residual) const {
     residual[0] = 10.0 - x[0];
     return true;
   }
};

 CostFunction* cost_function =
      new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
     
 problem.AddResidualBlock(cost_function, nullptr, &x);

3.2 数值导数（即人工求导）

在某些情况下，无法定义模板化的CostFunction，例如当残差的评估涉及对无法控制的库函数的调用时。

struct NumericDiffCostFunctor {
  bool operator()(const double* const x, double* residual) const {
    residual[0] = 10.0 - x[0];
    return true;
  }
};

CostFunction* cost_function =
  new NumericDiffCostFunction<NumericDiffCostFunctor, ceres::CENTRAL, 1, 1>(
      new NumericDiffCostFunctor);
      
problem.AddResidualBlock(cost_function, nullptr, &x);

一般来说，推荐自动微分而不是数字微分。使用 C++ 模板使自动微分高效，而数值微分代价高昂，容易出现数值错误，并导致收敛速度较慢。

3.3 分析导数

在某些情况下，无法使用自动微分。例如，可能的情况是，以封闭形式计算导数比依赖自动微分代码使用的链式规则更有效。

class QuadraticCostFunction : public ceres::SizedCostFunction<1, 1> {
 public:
  virtual ~QuadraticCostFunction() {}
  virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) const {
    const double x = parameters[0][0];
    residuals[0] = 10 - x;

    // Compute the Jacobian if asked for.
    if (jacobians != nullptr && jacobians[0] != nullptr) {
      jacobians[0][0] = -1;
    }
    return true;
  }
};

CostFunction* cost_function = new QuadraticCostFunction;
 
problem.AddResidualBlock(cost_function, NULL, &x);

Evaluate提供了一个parameters输入数组，一个用于残差的输出数组residuals和一个用于Jacobians的输出数组jacobians。jacobians数组是可选的，Evaluate要检查它是否为非空，如果是的话，就用残差函数的导数值填充它。
这种方法最繁琐，一般不建议使用

四、多个CostFunction

最小化 1 2 ∣ ∣ F ( x ) ∣ ∣ 2 \frac{1}{2}||F(x)||^2 21∣∣F(x)∣∣2

4.1 定义每一个CostFunction

例如 f 4 ( x ) f_4(x) f4(x)

struct F4 {
  template <typename T>
  bool operator()(const T* const x1, const T* const x4, T* residual) const {
    residual[0] = sqrt(10.0) * (x1[0] - x4[0]) * (x1[0] - x4[0]);
    return true;
  }
};

4.2 添加ResidualBlock

double x1 =  3.0; double x2 = -1.0; double x3 =  0.0; double x4 = 1.0;

Problem problem;

// Add residual terms to the problem using the using the autodiff
// wrapper to get the derivatives automatically.
problem.AddResidualBlock(
  new AutoDiffCostFunction<F1, 1, 1, 1>(new F1), nullptr, &x1, &x2);
problem.AddResidualBlock(
  new AutoDiffCostFunction<F2, 1, 1, 1>(new F2), nullptr, &x3, &x4);
problem.AddResidualBlock(
  new AutoDiffCostFunction<F3, 1, 1, 1>(new F3), nullptr, &x2, &x3)
problem.AddResidualBlock(
  new AutoDiffCostFunction<F4, 1, 1, 1>(new F4), nullptr, &x1, &x4);

五、曲线拟合

之前的例子都是没有数据的，这里用数据拟合一条曲线
y = e m x + c y=e^{mx+c} y=emx+c

5.1 定义CostFunction

struct ExponentialResidual {
  ExponentialResidual(double x, double y)
      : x_(x), y_(y) {}

  template <typename T>
  bool operator()(const T* const m, const T* const c, T* residual) const {
    residual[0] = y_ - exp(m[0] * x_ + c[0]);
    return true;
  }

 private:
  // Observations for a sample.
  const double x_;
  const double y_;
};

5.2 添加ResidualBlock

假设观察值在一个名为data的2n大小的数组中，问题的构建是一个简单的问题，即为每个观察值创建一个CostFunction。

double m = 0.0;
double c = 0.0;

Problem problem;
for (int i = 0; i < kNumObservations; ++i) {
  CostFunction* cost_function =
       new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(
           new ExponentialResidual(data[2 * i], data[2 * i + 1]));
  problem.AddResidualBlock(cost_function, nullptr, &m, &c);
}

可以看到这里的CostFunction比没有数据时多了参数输入。

六、鲁棒的曲线拟合

现在假设我们得到的数据有一些离群值，也就是说，我们有一些不服从噪声模型的点。如果我们使用上面的代码来拟合这些数据，我们会得到一个如下的拟合结果。

为了处理异常值，标准技术是使用 LossFunction。LossFunction减少了残差高的残差块的影响，这些通常是那些对应于异常值的残差块。为了将损失函数与残差块相关联，我们改变

problem.AddResidualBlock(cost_function, nullptr , &m, &c);

problem.AddResidualBlock(cost_function, new CauchyLoss(0.5) , &m, &c);

CauchyLoss 是 Ceres Solver 附带的损失函数之一。参数 0.5 指定损失函数的规模。结果，我们得到了低于 7 的拟合。

七、Bundle Adjustment

以视觉SLAM的重投影误差作为CostFunction

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];
    // 将世界坐标系的3D点point，转到相机坐标系下的3D点P
    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 - 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;
};

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));
}

由于这是一个很大的稀疏问题（无论如何对于 DENSE_QR 来说都很大），解决这个问题的一种方法是将 Solver::Options::linear_solver_type 设置为 SPARSE_NORMAL_CHOLESKY 并调用 Solve()。虽然这是一个合理的做法，但束调整问题有一个特殊的稀疏结构，可以利用它来更有效地解决它们。 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";

码农公寓