Ceres solver

最新推荐文章于 2023-07-10 19:59:00 发布

zerolover

最新推荐文章于 2023-07-10 19:59:00 发布

阅读量1.1k

点赞数

分类专栏： math

math 专栏收录该内容

5 篇文章 0 订阅

订阅专栏

Ceres可以解决鲁棒约束的非线性最小二乘问题，具有一下形式：

$\min_{\mathbf{x}} \quad \frac{1}{2}\sum_{i} \rho_i\left(\left|f_i\left(x_{i_1}, … ,x_{i_k}\right)\right|^2\right) \\ \text{s.t.} \quad l_j \le x_j \le u_j$

表达式 $\rho_i\left(\left\|f_i\left(x_{i_1},...,x_{i_k}\right)\right\|^2\right)$ 为Residual Block
$f_i(\cdot)$ 为CostFunction
$\left[x_{i_1},... , x_{i_k}\right]$ 为parameter blocks
$l_j$ 和 $u_j$ 为parameter block $x_j$ 的取值范围。
$\rho_i$ 为LossFunction，可以降低由于outliers带来的影响。

当 $\rho_i(x) = x$ ， $l_j = -\infty$ 并且 $u_j = \infty$ ，即为一个最小二乘问题。

示例1：求极值

首先我们以Ceres库官网中的Hello World例子来进行说明。这里例子的目的是为了计算方程 $f(x)=\frac{1}{2}(10 -x)^2$ 取得最小值时x的值。从这个方程很容易看出来当 $x=10$ 时， $f(x)$ 取得最小值0。这个方程虽然没有什么实际意义，但是为了演示Ceres库还是很不错的例子。

1、编写一个 $f(x)=10-x$ 的残差方程。代码如下：

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

这里值得注意的是，必须要编写一个重载()运算，而且必须使用模板类型，所有的输入参数和输出参数都要使用T类型。

2、当我们写完了上面的计算残差的方程，接下来就可以使用Ceres库来构造一个求解非线性最小二乘法的Problem来进行求解未知数了。代码如下：

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

  // 指定求解的未知数的初值，这里设置为5.0
  double initial_x = 5.0;
  double x = initial_x;

  // 建立Problem
  Problem problem;

   // 建立CostFunction（残差方程）
   CostFunction* cost_function =
       new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
   problem.AddResidualBlock(cost_function, NULL, &x);

   // 求解方程!
   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作为输入。上面的例子编译执行后，会输出下面的信息：

       0: f: 1.250000e+01 d: 0.00e+00 g: 5.00e+00h: 0.00e+00 rho: 0.00e+00 mu: 1.00e+04 li: 0 it: 6.91e-06 tt: 1.91e-03  
       1: f: 1.249750e-07 d: 1.25e+01 g: 5.00e-04h: 5.00e+00 rho: 1.00e+00 mu: 3.00e+04 li: 1 it: 2.81e-05 tt: 1.99e-03  
       2: f: 1.388518e-16 d: 1.25e-07 g: 1.67e-08h: 5.00e-04 rho: 1.00e+00 mu: 9.00e+04 li: 1 it: 1.00e-05 tt: 2.01e-03  
    CeresSolver Report: Iterations: 2, Initial cost: 1.250000e+01, Final cost:1.388518e-16, Termination: PARAMETER_TOLERANCE.  
    x : 5-> 10

3、从上面的输出信息中可以看出，经过三次迭代计算，求得的x值为10时可以取得最小值。接下来我们分析下main函数中的代码。

第2行代码为Google的log库，详细内容请参考Google log库的相关说明。
第5、6行为定义了求解未知数的初值，初值设置为5。
第9行，声明一个Problem对象，用于求解。
第12行，声明一个残差方程，CostFunction通过模板类AutoDiffCostFunction来进行构造，第一个模板参数为残差对象，也就是最开始写的那个那个带有重载()运算符的结构体，第二个模板参数为残差个数，第三个模板参数为未知数个数，最后参数是结构体对象。
第14行，将观测值和残差方程加入Problem对象中，如果有多个观测值，都需要加进去，这点可以看后面的示例。
第16~19行，定义一个求解选项，里面主要包括对方程线性化的方式，迭代次数等，具体参考官网帮助文档。
第20行，定义一个求解结果报告。
第21行，调用Solve函数进行求解，第一个参数就是求解选项，第二个参数为Problem指针，第三个参数为求解报告指针。
第23行，输出求解报告信息。
第24行，输出求解前的初值和求解后的值。

Ref:
Non-linear Least Squares