Ceres学习

最新推荐文章于 2023-07-10 13:52:07 发布

VoladorL

最新推荐文章于 2023-07-10 13:52:07 发布

阅读量855

点赞数 1

分类专栏： slam基础文章标签：学习人工智能机器学习

本文链接：https://blog.csdn.net/weixin_44368449/article/details/127278870

版权

slam基础专栏收录该内容

4 篇文章 0 订阅

订阅专栏

Ceres简介

Ceres solver 是谷歌开发的一款用于非线性优化的库，在谷歌的开源激光雷达slam项目cartographer中被大量使用。Ceres官网上的文档非常详细地介绍了其具体使用方法，相比于另外一个在slam中被广泛使用的图优化库G2O，ceres的文档可谓相当丰富详细。
官网：http://ceres-solver.org/
Ceres可以解决边界约束的鲁棒化非线性最小二乘法问题，其形式为：

$\begin{split}\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\end{split}$
这种形式的问题出现在科学和工程的广泛领域中–从统计学中的曲线拟合，到计算机视觉中从t图片中构建3D模型。

表达式 $\rho_i\left(\left\|f_i\left(x_{i_1},...,x_{i_k}\right)\right\|^2\right)$ 被称为残差块（ResidualBlock）
其中 $f_i(\cdot)$ 是一个代价函数（CostFunction）依赖于参数块 $\left[x_{i_1},... , x_{i_k}\right]$ 。在大多数优化问题小组的标量会一起出现。例如，平移向量的三个分量和定义相机姿态的四元数的四个分量我们将这样一组小标量称为参数块(ParameterBlock)。当然参数块可以只是一个参数。
$l_j$ 和 $u_j$ 是参数块 $x_j$ 的下界和上界。
$ρ_i$ ：损失函数（LossFunction）或称为核函数，它属于标量函数，为了减小异常值对非线性优化的影响。

作为一种特殊情况，当 $ρ_i(x)=x$ ，即恒等函数，并且 $l_j=−∞$ 和 $u j = \infty$ 时，我们会更熟悉的非线性最小二乘问题。
$\frac{1}{2}\sum_{i} \left\|f_i\left(x_{i_1}, ... ,x_{i_k}\right)\right\|^2.$
ceres求解的一般步骤：

定义Cost Function模型，即代价函数。也就是我们要寻找的最优目标，这里我们用到了仿函数或称为拟函数（functor）。做法是写一个类，然后在仿函数中重载()运算符。
使用定义的代价函数构建待求解的优化问题。即调用AddResidualBlock将误差项，添加到目标函数中。由于优化需要梯度，我们有几种选择:1)使用ceres自动求导（Auto Diff）2）使用数值求导（Numeric Diff）3）自行推导解析形式，提供给ceres。
配置求解器参数并求解问题。配置项options比较丰富，可以查看options的定义。

Hello World!

首先，考虑寻找函数最小值的问题

$\frac{1}{2}(10 -x)^2.$
这是一个微不足道的问题，其最小值位于 $x = 10$ ，但它是一个很好的起点来说明用 Ceres 解决问题的基础知识。

第一步是编写一个代价函数

使用了仿函数，即对()进行了重载。其中输入和输出都是同一类型T。定义这样一个结构体之后，我们就可以通过ceres调用CostFunctor ::operator()来使用这个重载操作符了。当T=double时，输出double类型的残差，如果T=Jet，那么输出雅可比矩阵。
第一步是编写一个仿函数来评估函数 $f (x) = 10 - x$ ：

//第一步：构建代价函数，重载()，
struct CostFunctor {
   template <typename T>
   bool operator()(const T* const x, T* residual) const {
     residual[0] = 10.0 - x[0];
     return true;
   }
};

构建待求解的优化问题

一旦我们有了计算残差函数的方法，现在是时候使用它构造一个非线性最小二乘问题并让 Ceres 解决它了。

	//设置成本函数（残差），使用自动微分求导（雅可比）
	CostFunction* cost_functor = 
		new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
		//添加残差项
		// Problem默认掌握cost_function，loss_function和local_parameterization指针的所有权。这些对象在Problem的整个生命周期都保持活动状态。
		// 用少量参数添加残差的方便方法。这是常见的情况。不要将形参块实参指定为vector，而是将它们列示为指针。

	problem.AddResidualBlock(cost_functor, nullptr, &x);

在这里插入图片描述

配置求解器参数并求解问题

	Solver::Options options;//求解参数选项
	options.linear_solver_type = ceres::DENSE_QR;//稠密QR分解
	//使用std::out输出
	options.minimizer_progress_to_stdout = true;
	Solver::Summary summary;
	Solve(options, &problem, &summary);//求解

完整代码：

#include<iostream>
#include<ceres/ceres.h>

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solve;
using ceres::Solver;
//代价函数
struct CostFunctor
{
	template<typename T>
	bool operator()(const T* const x, T* residual) const{
		residual[0] = (10.0 - x[0]) ;
		return true;
	}
	/* data */
};



int main(int argc, char** argv){
	google::InitGoogleLogging(argv[0]);
	//设置初始数值
	double initial_x = 5;
	//求解变量
	double x = initial_x;

	ceres::Problem problem;

	/*
	* AutoDiffCostFunction
	* 第1个模板参数是仿函数CostFunctor
	* 第2个模板参数是残差块中残差的数量
	* 第3个模板参数是第一个参数块中参数的数量
	* 如果有多个参数块，则第4/5...个模板参数: 依次写出各个参数块中参数的数量
	*/
	//自动求导
	
	CostFunction* cost_functor = 
		new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
		// 用少量参数添加残差的方便方法。这是常见的情况。不要将形参块实参指定为vector，而是将它们列示为指针。
	problem.AddResidualBlock(cost_functor, nullptr, &x);

	Solver::Options options;//求解选项
	options.linear_solver_type = ceres::DENSE_QR;
	//使用std::out输出
	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;
}

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  4.900500e+01    0.00e+00    9.90e+00   0.00e+00   0.00e+00  1.00e+04        0    9.08e-06    2.81e-05
   1  4.899520e-07    4.90e+01    9.90e-04   9.90e+00   1.00e+00  3.00e+04        1    2.05e-05    7.22e-05
   2  5.443548e-16    4.90e-07    3.30e-08   9.90e-04   1.00e+00  9.00e+04        1    3.36e-06    8.12e-05
Ceres Solver Report: Iterations: 3, Initial cost: 4.900500e+01, Final cost: 5.443548e-16, Termination: CONVERGENCE
x: 5.0->10

数值导数

在某些情况下，不可能定义模板化成本函子，例如，当残差评估涉及对您无法控制的库函数的调用时。在这种情况下，可以使用数值微分。用户定义一个计算残差值并使用它构造 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, nullptr, &x);

该构造看起来几乎与用于自动微分的构造相同，除了一个额外的模板参数，该参数指示用于计算数值导数的有限差分方案的种类。
一般来说，建议使用自动微分而不是数值微分。使用 C++ 模板可以提高自动微分效率，而数值微分成本高，容易出现数值错误，并导致收敛速度较慢

解析导数

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

在这种情况下，可以提供您自己的残差和雅可比计算代码。为此，如果您在编译时知道参数和残差的大小，请定义 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 提供了一个参数输入数组、一个用于残差的输出数组残差和一个用于雅可比矩阵的输出数组 jacobians。 jacobians 数组是可选的，Evaluate 应该检查它何时为非空，如果是，则用残差函数的导数的值填充它。在这种情况下，由于残差函数是线性的，雅可比是常数。

//构造代价函数
CostFunction* cost_function = new QuadraticCostFunction;

按照官方说明，以下情况可以使用解析求导:

函数式简单，便于求出导数解析式
能使用Matlab Maple Mathmatic SymPy等数学软件求出了导数的解析式
性能极致要求
没有其他好的方法去求导
喜欢手算导数

综上所述，建议优先使用自动求导和数值求导的方式，对雅克比计算擅长者和极致性能追求者可考虑使用解析求导的方式。

Problem类简述

// 来自于ceres-solver-1.14.0/include/ceres/problem.h
class CERES_EXPORT Problem {
 public:

// Problem默认掌握cost_function，loss_function和local_parameterization指针的所有权。这些对象在Problem的整个生命周期都保持活动状态。
// 如果用户希望控制这些对象的销毁行为，那么他们可以通过在Problem :: Options中设置相应的选项来实现。
  struct CERES_EXPORT Options {
    Options()
        : cost_function_ownership(TAKE_OWNERSHIP),
          loss_function_ownership(TAKE_OWNERSHIP),
          local_parameterization_ownership(TAKE_OWNERSHIP),
          enable_fast_removal(false),
          disable_all_safety_checks(false),
          context(NULL) {}

/*
enum Ownership {
  DO_NOT_TAKE_OWNERSHIP,
  TAKE_OWNERSHIP
};
*/

/*
默认值: TAKE_OWNERSHIP
作用:  该选项控制Problem对象是否拥有代价函数。
如果设置为TAKE_OWNERSHIP，那么Problem对象将在销毁时删除代价函数。析构函数只小心地删除指针一次，由于允许共享代价函数。
*/
    Ownership cost_function_ownership;

/*
默认值: TAKE_OWNERSHIP
作用:  该选项控制Problem对象是否拥有损失函数。
如果设置为TAKE_OWNERSHIP，那么Problem对象将在销毁时删除损失函数。析构函数只小心地删除指针一次，由于允许共享损失函数。
*/
    Ownership loss_function_ownership;

/*
默认值: TAKE_OWNERSHIP
作用:  该选项控制Problem对象是否拥有局部参数化。
如果设置为TAKE_OWNERSHIP，那么Problem对象将在销毁时删除局部参数化。析构函数只小心地删除指针一次，由于允许共享局部参数化。
*/
    Ownership local_parameterization_ownership;

/*
默认值: false
作用: 如果为true，则用内存交换更快的操作Problem::RemoveResidualBlock()和Problem::RemoveParameterBlock()???
默认情况下，Problem::RemoveParameterBlock()和Problem::RemoveResidualBlock()所花的时间与整个问题的大小成比例。如果您只是偶尔从问题中删除参数或残差，这可能是可以接受的。
但是，如果您有空闲的内存，则启用此选项使Problem::RemoveParameterBlock()占用的时间与依赖它的残差块的数量成比例，而Problem::RemoveResidualBlock()占用(平均)常量时间。
*/
    bool enable_fast_removal;

/*
默认值: false
作用: 默认情况下，Ceres在构造problem时执行各种安全检查。这些检查有一个很小但可测量的性能损失，通常约为构建时间的5%。
如果您确定problem构造是正确的，并且5%的problem构造时间确实是您想要避免的开销，那么您可以将disable_all_safety_checks设置为true。
WARNING 不要设置为true，除非你绝对确定你在做什么。
*/
    bool disable_all_safety_checks;
/*
默认值: nullptr
作用: 用于解决这个Problem的Ceres全局上下文。Ceres没有获得指针的所有权。
*/
    Context* context;
  };

// 默认构造函数等价于调用Problem(Problem::Options())。
  Problem();
  explicit Problem(const Options& options);

  ~Problem();

// 添加一个剩余块到总的成本函数。
  ResidualBlockId AddResidualBlock(
      CostFunction* cost_function,
      LossFunction* loss_function,
      const std::vector<double*>& parameter_blocks);

// 用少量参数添加残差的方便方法。这是常见的情况。不要将形参块实参指定为vector，而是将它们列示为指针。
  ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   double* x0);
  ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   double* x0, double* x1);
  ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   double* x0, double* x1, double* x2);
  ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   double* x0, double* x1, double* x2,
                                   double* x3);
  ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   double* x0, double* x1, double* x2,
                                   double* x3, double* x4);
  ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   double* x0, double* x1, double* x2,
                                   double* x3, double* x4, double* x5);
  ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   double* x0, double* x1, double* x2,
                                   double* x3, double* x4, double* x5,
                                   double* x6);
  ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   double* x0, double* x1, double* x2,
                                   double* x3, double* x4, double* x5,
                                   double* x6, double* x7);
  ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   double* x0, double* x1, double* x2,
                                   double* x3, double* x4, double* x5,
                                   double* x6, double* x7, double* x8);
  ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   double* x0, double* x1, double* x2,
                                   double* x3, double* x4, double* x5,
                                   double* x6, double* x7, double* x8,
                                   double* x9);

// 为问题添加一个大小适当的参数块。具有相同参数的重复调用将被忽略。使用相同的双指针但大小不同的重复调用将导致未定义行为。
  void AddParameterBlock(double* values, int size);

// 向问题添加一个具有适当大小和参数化的参数块。具有相同参数的重复调用将被忽略。使用相同的双指针但大小不同的重复调用将导致未定义行为。
  void AddParameterBlock(double* values,
                         int size,
                         LocalParameterization* local_parameterization);

// 从问题中移除一个参数块。
  void RemoveParameterBlock(double* values);

// 从问题中移除一个残差块。
  void RemoveResidualBlock(ResidualBlockId residual_block);

// 在优化过程中保持所指示的参数块为常数。
  void SetParameterBlockConstant(double* values);

// 允许指定的参数块在优化期间变化。
  void SetParameterBlockVariable(double* values);

// 如果参数块被设置为常量，则返回true，否则返回false。
  bool IsParameterBlockConstant(double* values) const;

// 为一个参数块设置局部参数化。
  void SetParameterization(double* values,
                           LocalParameterization* local_parameterization);

// 获取与此参数块关联的局部参数化对象。如果没有关联的参数化对象，则返回NULL。
  const LocalParameterization* GetParameterization(double* values) const;

// 设置位置为"index"的参数的上下边界。
  void SetParameterLowerBound(double* values, int index, double lower_bound);
  void SetParameterUpperBound(double* values, int index, double upper_bound);

 // 问题中的参数块数。始终等于parameter_blocks().size()和parameter_block_sizes().size()。
  int NumParameterBlocks() const;

// 通过对所有参数块的大小求和得到的参数向量的大小。
  int NumParameters() const;

// 返回Problem中残差块的数量，永远等于residual_blocks().size()。
  int NumResidualBlocks() const;

// 返回残差向量的大小，包含所有残差块内残差个数的总和。
  int NumResiduals() const;

// 参数块的大小。
  int ParameterBlockSize(const double* values) const;

// 参数块的local parameterization的大小。如果没有与此参数块关联的local parameterization，则ParameterBlockLocalSize = ParameterBlockSize。
  int ParameterBlockLocalSize(const double* values) const;

// 给定的参数块在这个问题中是否存在?
  bool HasParameterBlock(const double* values) const;

// 用指向问题中当前参数块的指针填充传递的parameter_blocks vector。在这个调用之后，parameter_block.size() == NumParameterBlocks。
  void GetParameterBlocks(std::vector<double*>* parameter_blocks) const;

// 用指向问题中当前残差块的指针填充所传递的residual_blocks向量。在这个调用之后，residual_blocks.size() == NumResidualBlocks。
  void GetResidualBlocks(std::vector<ResidualBlockId>* residual_blocks) const;

// 获取所有依赖于给定残差块的参数块。
  void GetParameterBlocksForResidualBlock(
      const ResidualBlockId residual_block,
      std::vector<double*>* parameter_blocks) const;

// 获取给定残差块的CostFunction。
  const CostFunction* GetCostFunctionForResidualBlock(
      const ResidualBlockId residual_block) const;

// 获取给定残差块的LossFunction。如果没有loss函数与这个残差块相关联，则返回NULL。
  const LossFunction* GetLossFunctionForResidualBlock(
      const ResidualBlockId residual_block) const;

// 获取所有依赖于给定参数块的残差块。
// 如果Problem::Options::enable_fast_removal为true，那么获取残差块的速度很快，并且只依赖于残差块的数量。否则，获取参数块的残差块将导致对整个Problem对象的扫描。
  void GetResidualBlocksForParameterBlock(
      const double* values,
      std::vector<ResidualBlockId>* residual_blocks) const;

// 控制Problem::Evaluate的选项结构体
  struct EvaluateOptions {
    EvaluateOptions()
        : apply_loss_function(true),
          num_threads(1) {
    }

/*
需要执行计算的参数块的集合。这个向量决定了参数块出现在梯度向量和雅可比矩阵列中的顺序。如果parameter_blocks为空，则假定它等于一个包含所有参数块的向量。
一般来说，在这种情况下，参数块的顺序取决于它们添加到问题中的顺序，以及用户是否删除了任何参数块。
注意：这个向量应该包含与用于向问题添加参数块的指针相同的指针。这些参数块不应该指向新的内存位置。如果你这么做，就会有坏事发生。
*/
    std::vector<double*> parameter_blocks;

/*
需要执行计算的残差块的集合。
这个向量决定了残差发生的顺序，以及雅可比矩阵的行是如何排列的。如果residual_blocks为空，则假定它等于包含所有残差块的向量。
*/
    std::vector<ResidualBlockId> residual_blocks;

// 即使问题中的残差块可能包含loss函数，将apply_loss_function设置为false将关闭loss函数在cost函数输出中的应用。
    bool apply_loss_function;
// 要使用的线程数。(需要OpenMP)。
    int num_threads;
  };

/*
评估问题。任何输出指针都可以是NULL。使用哪些残差块和参数块由上面的EvaluateOptions结构体控制。
注意1: 计算将使用在构造问题时使用的参数块指针所指向的内存位置中存储的值。也就是说,
    Problem problem;
    double x = 1;
    problem.AddResidualBlock(new MyCostFunction, NULL, &x);
    double cost = 0.0;
    problem.Evaluate(Problem::EvaluateOptions(), &cost, NULL, NULL, NULL);
代价在x = 1处计算。如果你想求x = 2处的值，那么
    x = 2;
    problem.Evaluate(Problem::EvaluateOptions(), &cost, NULL, NULL, NULL);
就是这样做的方法。

注意2: 如果不使用局部参数化，则梯度向量的大小(以及雅可比矩阵中的列数)是所有参数块的大小之和。
       如果一个参数块具有局部参数化，那么它将为梯度向量(以及雅可比矩阵中的列数)提供“LocalSize”项。

注意3: 当问题正在被解决时，这个函数不能被调用，例如，不能在解决问题期间的迭代结束时从IterationCallback调用它。
*/
  bool Evaluate(const EvaluateOptions& options,
                double* cost,
                std::vector<double>* residuals,
                std::vector<double>* gradient,
                CRSMatrix* jacobian);

 private:
  friend class Solver;
  friend class Covariance;
// 整个Problem函数内部的核心操作实际上是由类对象内部的 problem_impl_ 操作的，
  internal::scoped_ptr<internal::ProblemImpl> problem_impl_;
  CERES_DISALLOW_COPY_AND_ASSIGN(Problem);
};

在这里插入图片描述

Problem类重要函数

Problem::AddResidualBlock

向Problem类传递残差模块的信息。函数原型

  ResidualBlockId AddResidualBlock(
      CostFunction* cost_function,
      LossFunction* loss_function,
      const std::vector<double*>& parameter_blocks);

  ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   double* x0,...);

参数说明:
cost_function:代价函数，包含了参数模块的维度信息。
loss_function:损失函数，用于处理参数中含有野值的情况，避免错误量测对估计的影响，常用参数包括HuberLoss、CauchyLoss等,该参数可以取NULL或nullptr，此时损失函数为单位函数。(http://ceres-solver.org/nnls_modeling.html#instances)
parameter_blocks:待优化的参数，可一次性传入所有参数的指针容器vector<double*>或依次传入所有参数的指针double*。

Problem::AddParameterBlock

用户在调用AddResidualBlock时其实已经隐式地向Problem传递了参数模块，但在一些情况下，需要用户显示地向Problem传入参数模块（通常出现在需要对优化参数进行重新参数化的情况）。Ceres提供了Problem::AddParameterBlock函数用于用户显式传递参数模块。函数原型

  void AddParameterBlock(double* values, int size);

  void AddParameterBlock(double* values,
                         int size,
                         LocalParameterization* local_parameterization);

其中，第一种函数原型除了会增加一些额外的参数检查之外，功能上和隐式传递参数并没有太大区别。第二种函数原型则会额外传入LocalParameterization参数，用于重构优化参数的维数。

LocalParameterization类

LocalParameterization类的作用是解决非线性优化中的过参数化问题。所谓过参数化，即待优化参数的实际自由度小于参数本身的自由度。例如在SLAM中，当采用四元数表示位姿时，由于四元数本身的约束（模长为1），实际的自由度为3而非4。此时，若直接传递四元数进行优化，冗余的维数会带来计算资源的浪费，需要使用Ceres预先定义的QuaternionParameterization对优化参数进行重构：

problem.AddParameterBlock(quaternion, 4);// 直接传递4维参数

ceres::LocalParameterization* local_param = new ceres::QuaternionParameterization();
problem.AddParameterBlock(quaternion, 4, local_param)//重构参数，优化时实际使用的是3维的等效旋转矢量

3.1 Ceres预定义的LocalParameterization

LocalParameterization本身是一个虚基类，详细定义如下。用户可以自行定义自己需要使用的子类，或使用Ceres预先定义好的子类。

/* 目的: 有时参数块x可能会过参数化问题

    min f(x)
     x
例如，三维中的球体是嵌入在三维空间中的二维流形。
在球面上的每一点上，它的平面切线定义了一个二维切线空间。
对于这个球上定义的代价函数，给定一个点x，在这个点上向球的法线方向移动是没有用的。
因此，做局部优化的一个更好的方法是在这个点的切空间上优化二维向量delta，然后“移动”到点x + delta，
在这里移动操作涉及到投影回球面。这样做可以从优化中删除一个冗余维度，使其在数值上更加健壮和高效。

更一般地，我们可以定义一个函数

    x_plus_delta = Plus(x, delta),

其中x_plus_delta的大小与x相同，而delta的大小小于或等于x。函数Plus推广了向量加法的定义。因此它满足恒等式

    Plus(x, 0) = x, for all x.

一个普通的加号是当delta和x大小相同时

    Plus(x, delta) = x + delta

更有趣的情况是，如果x是二维向量，用户希望保持第一个坐标常量。那么，delta是一个标量，加号定义为

    Plus(x, delta) = x + [0] * delta
                         [1]

一个常见的来自运动的结构问题的例子是，当相机旋转使用四元数参数化。
只做与定义四元数的4向量正交的更新是有用的。一种方法是让delta是一个三维向量并定义+为
    Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x

RHS上两个4-向量之间的乘法是标准的四元数乘积。

给定g和点x，优化f现在可以重新表述为

     min  f(Plus(x, delta))
    delta

给出这个问题的解delta，则最优值为

    x* = Plus(x, delta)

LocalParameterization类定义了函数Plus及其雅可比矩阵，用于计算f关于delta的雅可比矩阵。
*/
class CERES_EXPORT LocalParameterization {
 public:
  virtual ~LocalParameterization();

  /* 加法操作的实现

      x_plus_delta = Plus(x, delta)

      条件: Plus(x, 0) = x.
  */
  virtual bool Plus(const double* x,
                    const double* delta,
                    double* x_plus_delta) const = 0;

  /* The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
  
    jacobian is a row-major GlobalSize() x LocalSize() matrix.
  */
  virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0;

  /* local_matrix = global_matrix * jacobian

     global_matrix is a num_rows x GlobalSize  row major matrix.
     local_matrix is a num_rows x LocalSize row major matrix.
     jacobian(x) is the matrix returned by ComputeJacobian at x.

     这只在GradientProblem中使用。对于大多数正常使用，使用默认实现是可以的。
  */
  virtual bool MultiplyByJacobian(const double* x,
                                  const int num_rows,
                                  const double* global_matrix,
                                  double* local_matrix) const;

  // Size of x. 参数的实际维数。
  // GlobalSize()返回参数块大小，eg：四元数返回4
  virtual int GlobalSize() const = 0;

  // Size of delta. 正切空间上的参数维数
  // LocalSize()返回参数块在对应空间的实际大小，eg，四元数返回3
  virtual int LocalSize() const = 0;
};

QuaternionParameterization:
注意:
在 ceres 源码中没有明确说明之处都认为矩阵 raw memory 存储方式是 Row Major 的，这与 Eigen 默认的 Col Major 是相反的。
ceres 默认的 Quaternion raw memory 存储方式是 w, x, y, z，而 Eigen Quaternion 的存储方式是 x, y, z, w，这就导致在 ceres 代码中除ceres::QuaternionParameterization 之外还有ceres::EigenQuaternionParameterization。

Eigen Quaternion指的是eigen库中的函数Eigen::Quaternion(w,x,y,z)函数中，实数w在首；但是实际上它的内部存储顺序是[x y z w]，对其访问的时候最后一个元素才是w

对三个函数内部存储顺序总结
ceres::QuaternionParameterization：内部存储顺序为(w,x,y,z)
ceres::EigenQuaternionParameterization：内部存储顺序为(x,y,z,w)
Eigen::Quaternion(w,x,y,z)：内部存储顺序为(x,y,z,w)（与构造函数没有保持一致）

ceres 中 Quaternion 是 Hamilton Quaternion，遵循 Hamilton 乘法法则。

class CERES_EXPORT QuaternionParameterization : public LocalParameterization {
 public:
  virtual ~QuaternionParameterization() {}
  //重载的Plus函数给出了四元数的更新方法，接受参数分别为优化前的四元数【x】，用旋转矢量表示的增量【delta】，以及更新后的四元数【x_plus_delta】。
  //函数首先将增量【delta】由旋转矢量转换为四元数，随后采用标准四元数乘法对四元数进行更新。
  virtual bool Plus(const double* x,
                    const double* delta,
                    double* x_plus_delta) const;
  virtual bool ComputeJacobian(const double* x,
                               double* jacobian) const;
  //GlobalSize 返回值为4，即四元数本身的实际维数。由于在内部优化时，ceres采用的是旋转矢量，维数为3，因此LocalSize()的返回值为3。
  //GlobalSize 就是表示他真正的维数是一个4维的
  virtual int GlobalSize() const { return 4; }
  //LocalSize是告诉Ceres他表示的东西是一个三维的
  virtual int LocalSize() const { return 3; }
};
//=============================================================================
//重载的Plus函数给出了四元数的更新方法，接受参数分别为优化前的四元数【x】，用旋转矢量表示的增量【delta】，以及更新后的四元数【x_plus_delta】。
//函数首先将增量【delta】由旋转矢量转换为四元数，随后采用标准四元数乘法对四元数进行更新。
bool QuaternionParameterization::Plus(const double* x,
                                      const double* delta,
                                      double* x_plus_delta) const {
  // 将旋转矢量转换为四元数形式
  const double norm_delta =
      sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
  if (norm_delta > 0.0) {
    const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);
    double q_delta[4];
    q_delta[0] = cos(norm_delta);
    q_delta[1] = sin_delta_by_delta * delta[0];
    q_delta[2] = sin_delta_by_delta * delta[1];
    q_delta[3] = sin_delta_by_delta * delta[2];
    // 采用四元数乘法更新
    QuaternionProduct(q_delta, x, x_plus_delta);
  } else {
    for (int i = 0; i < 4; ++i) {
      x_plus_delta[i] = x[i];
    }
  }
  return true;
}

自定义LocalParameterization

用户自定义一个类继承LocalParameterization类，主要实现的有

Plus(const double* x,const double* delta,double* x_plus_delta)：定义变量的加法
ComputeJacobian()：x对delta的雅克比矩阵
GlobalSize()：x 的自由度（可能有冗余），比如四元数的自由度是4
LocalSize()：Δx 所在的正切空间（tangent space）的自由度，那么这个自由度是3

Solver

置求解器参数并求解问题，这个步骤就是设置方程怎么求解、求解过程是否输出等，然后调用一下Solve方法。

// 来自于ceres-solver-1.14.0/examples/helloworld.cc

// 第三部分： 配置并运行求解器

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

std::cout << summary.BriefReport() << "\n"; // 输出优化的简要信息

求解最小二乘问题

ceres::Solve函数是Ceres求解最小二乘问题的核心函数，函数原型如下：

// 来自于ceres-solver-1.14.0/include/ceres/solver.h

void Solve(const Solver::Options& options, Problem* problem, Solver::Summary* summary);

Solver::Options: 求解选项。是Ceres求解的核心，包括消元顺序、分解方法、收敛精度等在内的求解器所有行为均由Solver::Options控制。
Problem : 求解问题。
Solver::Summary: 求解报告。用于存储求解过程中的相关信息，并不影响求解器性能

Solver::Options

Solver::Options含有的参数很多，API文档中对于每个参数的作用和意义都给出了详细的说明。由于在大多数情况下，大多数参数我们都会使用Ceres的默认设置。
列举了一些可能会改变的参数:
linear_solver_type：信赖域方法中求解线性方程组所使用的求解器类型，默认为DENSE_QR，其他可选项如下：

DENSE_QR：QR分解，用于小规模最小二乘问题求解；
DENSE_NORMAL_CHOLESKY&SPARSE_NORMAL_CHOLESKY：Cholesky分解，用于具有稀疏性的大规模非线性最小二乘问题求解；
CGNR：使用共轭梯度法求解稀疏方程；
DENSE_SCHUR&SPARSE_SCHUR：SCHUR分解，用于BA问题求解；
ITERATIVE_SCHUR：使用共轭梯度SCHUR求解BA问题；
** min_linear_solver_iteration/max_linear_solver_iteration**:线性求解器的最小/最大迭代次数，默认为0/500，一般不需要更改
max_num_iterations:求解器的最大迭代次数；
num_threads:Ceres求解时使用的线程数
linear_solver_ordering:线性方程求解器的消元顺序，默认为NULL，即由Ceres自行决定消元顺序；在以BA为典型代表的，对消元顺序有特殊要求的应用中，可以通过成员函数reset设定消元顺序。

Ceres消元顺序的设置由linear_solver_ordering的reset函数完成，该函数接受参数为ParameterBlockOrdering对象。该对象将所有待优化参数存储为带标记（ID）的组（Group），ID小的Group在求解线性方程的过程中会被首先消去。因此，我们需要做的第一个工作是调用其成员函数AddElementToGroup将参数添加到对应ID的Group中，函数原型为：

bool ParameterBlockOrdering::AddElementToGroup(const double *element, const int group)

接收的元素为变量数组的指针；组ID为非负整数，最小为0，如果该Id对应的Group不存在，则Ceres会自动创建。下面我们来看一个BA中的例子：

ceres::ParameterBlockOrdering* ordering = new ceres::ParameterBlockOrdering();

    // set all points in ordering to 0
    for(int i = 0; i < num_points; i++){
        ordering->AddElementToGroup(points + i * point_block_size, 0);
    }
    // set all cameras in ordering to 1
    for(int i = 0; i < num_cameras; i++){
        ordering->AddElementToGroup(cameras + i * camera_block_size, 1);
    }

该例子中，所有路标点被分到了ID = 0组，而所有相机位姿被分到了ID = 1组，因此在线性方程组的求解中，所有路标点会变首先SCHUR消元。
接下来，我们就可以使用reset函数制定线性求解器的消元顺序了

// set ordering in options
options->linear_solver_ordering.reset(ordering);

在实际应用中，对最终求解性能最大的就是线性方程求解器类型linear_solver_type和线程数，如果发现最后的求解精度或求解效率不能满足要求，应首先尝试更换这两个参数。

Solver::Summary

包含了求解器本身和求解中各变量的信息，许多成员函数与Solver::Options一致，详细列表同样请参阅API文档，这里只给出另外两个常用的成员函数：

BriefReport()：输出单行的简单总结；
FullReport()：输出多行的完整总结。

参考：

[1] http://ceres-solver.org/nnls_tutorial.html
[2] https://www.cnblogs.com/vivian187/p/15393995.html
https://www.cnblogs.com/vivian187/p/15394000.html#2problem%E7%B1%BB%E9%87%8D%E8%A6%81%E5%87%BD%E6%95%B0
[3] https://www.cnblogs.com/vivian187/p/15394010.html#:~:text=Solver%3A%3A,%E5%B8%B8%E7%94%A8%E7%9A%84%E6%88%90%E5%91%98%E5%87%BD%E6%95%B0%EF%BC%9A
[4] https://blog.csdn.net/jdy_lyy/article/details/119360492?utm_medium=distribute.pc_relevant.none-task-blog-2defaultbaidujs_title~default-0.no_search_link&spm=1001.2101.3001.4242

VoladorL

关注

1
点赞
踩
7

收藏

觉得还不错? 一键收藏
打赏
0
评论
Ceres学习

LocalParameterization本身是一个虚基类，详细定义如下。用户可以自行定义自己需要使用的子类，或使用Ceres预先定义好的子类。/* 目的: 有时参数块x可能会过参数化问题min f(x)x例如，三维中的球体是嵌入在三维空间中的二维流形。在球面上的每一点上，它的平面切线定义了一个二维切线空间。对于这个球上定义的代价函数，给定一个点x，在这个点上向球的法线方向移动是没有用的。
复制链接

扫一扫