ceres solver Non-linear Least Squares

1. 简介

ceres solver

1.1 基本概念

ceres 可以解决形式受限的鲁棒非线性最小二乘问题：
$$\underset{x}{\min }\frac{1}{2}\sum _i{\rho }_i({\Vert f_i(x_{i_1},...,x_{i_k})\Vert }^2)$$$$s.t.l_j\le x_j\le u_j$$ 在表达式中：${\rho }_i({\Vert f_i(x_{i_1},...,x_{i_k})\Vert }^2$是作为已知量的残差块。

• $f_i(\cdot )$是一个依赖参数块$[x_{i_1},...,x_{i_k}]$的代价函数。
• ${\rho }_i$是损失函数。损失函数：是一个标量函数，用于减少离群值对非线性最小二乘问题解的影响。

作为一种特殊情况，当${\rho }_i(x)=x$时，即恒等函数，以及$l_j=-\infty ,u_j=\infty$，我们得到了更为熟悉的非线性最小二乘问题。

1.2 案例

求最小值： $\frac{1}{2}(x-10{)}^2$

1. 首先写一个函数，评估上述的函数：$f(x)=(x-10)$
2. 一旦我们有了一种计算残差函数的方法，现在就可以使用它来构造非线性最小二乘问题，并让Ceres解决它。

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

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, NULL, &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;
}

• AutoDiffCostFunction将CostFunctor作为输入，对其进行自动区分并为其提供CostFunction接口。

1.3 衍生

像大多数优化程序包一样，Ceres Solver依赖于能够在任意参数值下评估目标函数中每个项的值和导数。 正确有效地执行此操作对于获得良好的结果至关重要。 Ceres Solver提供了许多方法。
现在我们考虑另外两种可能性。解析和数值导数。

1.3.1 数值导数

在某些情况下，无法定义模板化的成本函子，例如，当残差的评估涉及对您无法控制的库函数的调用时。 在这种情况下，可以使用数值微分。 用户定义一个函子，该函子可计算残差值并使用其构造一个NumericDiffCostFunction。 例如，对于f（x）= 10-x，相应的函子将为：

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, NULL, &x);

注意我们使用自动微分时的相似之处：

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

该构造看上去几乎与用于自动微分的构造相同，除了一个额外的模板参数，该参数指示了用于计算数值导数的有限微分方案的类型[3]。 有关更多详细信息，请参见NumericDiffCostFunction的文档。
一般来说，我们建议使用自动微分而不是数值微分。 C ++模板的使用使自动微分有效，而数字微分昂贵，容易出现数字错误并导致收敛速度变慢。

1.3.2 解析导数

在某些情况下，无法使用自动区分。 例如，可能存在这样的情况，即以封闭形式计算导数而不是依靠自动微分代码使用的链式规则更为有效。
在这种情况下，可以提供自己的残差和雅可比计算代码。 为此，如果您知道编译时参数和残差的大小，请定义CostFunction或SizedCostFunction的子类。 例如，这里是实现f（x）= 10-x的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 != NULL && jacobians[0] != NULL) {
      jacobians[0][0] = -1;
    }
    return true;
  }
};

SimpleCostFunction :: Evaluate提供了一个输入参数数组，一个用于残差的输出数组残差以及一个用于Jacobian的输出数组jacobians。 jacobians数组是可选的，Evaluate应该检查它是否为非空值，如果是这种情况，则用残差函数的导数值填充它。 在这种情况下，由于残差函数是线性的，因此雅可比常数为常数[4]。
从上面的代码片段可以看出，实现CostFunction对象有点繁琐。 我们建议除非您有充分的理由自己管理jacobian计算，否则请使用AutoDiffCostFunction或NumericDiffCostFunction构造残差块。

1.4 有关衍生工具的更多信息

到目前为止，计算导数是使用Ceres的最复杂的部分，并且根据情况，用户可能需要更复杂的计算导数的方法。 本节仅介绍如何将衍生物提供给Ceres。 一旦您对使用NumericDiffCostFunction和AutoDiffCostFunction感到满意，我们建议您看一下DynamicAutoDiffCostFunction，CostFunctionToFunctor，NumericDiffFunctor和ConditionedCostFunction，以了解构建和计算成本函数的更高级方法。

2. Powell’s Function

现在考虑一个稍微复杂一点的示例-powell’s函数的最小化。让:
$x=[x_1,x_2,x_3,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_1(x),f_2(x),f_3(x),f_4(x)]$
$F(x)$是四个参数的函数，有四个残差，我们找$x$使得${\Vert \frac{1}{2}F(x)\Vert }^2$最小。
同样，第一步是定义对目标函子中的项进行求值的函子。这是评估代码$f_4(x_1,x_4)$:

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

同理，我们定义$F1,F2,F3$，使用这些，问题可以构造如下：

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), NULL, &x1, &x2);
problem.AddResidualBlock(
  new AutoDiffCostFunction<F2, 1, 1, 1>(new F2), NULL, &x3, &x4);
problem.AddResidualBlock(
  new AutoDiffCostFunction<F3, 1, 1, 1>(new F3), NULL, &x2, &x3)
problem.AddResidualBlock(
  new AutoDiffCostFunction<F4, 1, 1, 1>(new F4), NULL, &x1, &x4);

请注意，每个ResidualBlock仅取决于相应残差对象所依赖的两个参数，而不取决于所有四个参数。