当前位置: 首页 > 工具软件 > Ceres Solver > 使用案例 >

非线性求解器Ceres Solver

燕超
2023-12-01

非线性求解器Ceres Solver

1 非线性最小二乘法

1.1 摘要

这个库主要用于求解具有边界约束的鲁棒非线性二乘问题,形式如下:
min ⁡ x = 1 2 ∑ i ρ i ( ∣ ∣ f i ( x i 1 , . . . , x i k ) ∣ ∣ 2 ) s.t. l j ≤ x j ≤ u j (1) \min\limits_{\bf{x}} = \frac{1}{2} \sum_i \rho_i(||f_i(x_{i1},...,x_{ik})||^2) \\ \text{s.t.} \quad l_j \leq x_j \leq u_j \tag{1} xmin=21iρi(fi(xi1,...,xik)2)s.t.ljxjuj(1)
其中, ρ i ( ∣ ∣ f i ( x i 1 , . . . , x i k ) ∣ ∣ 2 ) \rho_i(||f_i(x_{i1},...,x_{ik})||^2) ρi(fi(xi1,...,xik)2)被称为残差块; f ( ∗ ) f(*) f()被称为代价函数; x i 1 , . . . , x i k ) x_{i1},...,x_{ik}) xi1,...,xik)被称为参数块; ρ i \rho_i ρi为损失函数,目的是降低outlier对优化问题的影响。

如果定义; ρ i ( x ) = x \rho_i(x) = x ρi(x)=x为损失函数,同时不给出优化变量的边界,我们就得到了一个非线性最小二乘问题。

1.2 一个简单的例子

给出一个函数,求取其最小值

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

STEP1:写出代价函数 f ( x ) = 10 − x f(x) = 10 - x f(x)=10x

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

operator()是一个模版方法,这里假设输入输出均为类型T

我们有了方法计算残差函数,直接给出如何使用Ceres求解函数。

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

1.3 导数

跟很多非线性优化器一样,优化器取决于目标函数的取值与函数来确定优化是否完成,但是在之前的例子中,我们使用的是自动求导,显然如果给出导数的显式表达,计算会更快。
考虑另外两种形式:1.解析求导;2.数值求导

1.3.1 数值求导

在一些情况下,很难去定义模版化的代价函数,比如这个残差块涉及到别的库,在这种情况下,只能使用数值求导,即便将会付出很大的计算代价。
这个需要用到NumericDiffCostFunction,如

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

这个时候,加到Problem是:

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

与自动求导的相比:

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

1.3.2 解析求导

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

在解析求导的情形下,需要提供你自己的残差与雅可比计算的代码。可以定义一个CostFunction或者SizedCostFunction

给一个解析求导的简例SimpleCostFunction

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

SimpleCostFunction::Evaluate通过输入参数确定,残差与雅可比依靠定义的residualsjacobians

1.4 鲍威尔函数

考虑一种更复杂的例子,最小化鲍威尔函数:
给定 x = [ x 1 , x 2 , x 3 , x 4 ] x = [x_1, x_2, x_3, x_4] x=[x1,x2,x3,x4]

f 1 ( x ) = x 1 + 10 x 2 f 2 ( x ) = 5 ( x 3 − x 4 ) f 3 ( x ) = ( x 2 − 2 x 3 ) 2 f 4 ( x ) = 10 ( x 1 − x 4 ) 2 F ( x ) = [ f ( x 1 ) , f ( x 2 ) , f ( x 3 ) , f ( x 4 ) ] f_1(x) = x_1 + 10x_2\\ f_2(x) = \sqrt{5}(x_3-x_4)\\ f_3(x) = (x_2 - 2x_3)^2 \\ f_4(x) = \sqrt{10}(x_1 - x_4)^2 \\ F(x) = [f(x_1),f(x_2),f(x_3),f(x_4)] f1(x)=x1+10x2f2(x)=5 x3x4f3(x)=(x22x3)2f4(x)=10 (x1x4)2F(x)=[f(x1),f(x2),f(x3),f(x4)]

很显然,F(x)是一个四个参数的函数,含有四个残差,我们需要找到一个参数使得 ∣ ∣ F ( x ) ∣ ∣ ||F(x)|| F(x)最小。

首先,先写出 f 4 ( x 1 , x 4 ) f_4(x_1,x_4) f4(x1,x4):

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

同样的,我们也可以定义F1,F2F3。此时,问题可以构建如下:

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

1.5 曲线拟合

这个例子出现在高翔的slam 14讲中。高翔给出了手写非线性优化。

1.6 鲁棒曲线拟合

如果在观测中,出现一些外点,这个时候非线性最小二乘就会出现拟合问题,就需要自己去拟合曲线。
在鲁棒曲线拟合中,通常的做法,是引入损失函数减少外点的影响。
为了把损失函数加入到残差块中,我们可以改变上面提到的加入残差块的方法

原先方式:

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

加入损失函数的方式:

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

1.7 Bundle Adjustment

解决大规模BA问题是写ceres的动机之一。

BA是为了解决最小化重投影误差。
首先还是构建残差,类似于曲线拟合,每一张图像都构建重投影残差。
如下BA问题需要优化 相机的R、T以及焦距以及两个畸变系数。

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 - 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最简单的例子,Ceres给出了很多示例:https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/

 类似资料: