Ceres学习-2.Problem
目录
1.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);
};
PS:图片来自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
2.Problem类重要函数
2.1 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:
代价函数，包含了参数模块的维度信息。参考《Ceres学习-1.CostFunction》（ https://www.cnblogs.com/vivian187/p/15393995.html ）
- loss_function:
损失函数，用于处理参数中含有野值的情况，避免错误量测对估计的影响，常用参数包括HuberLoss、CauchyLoss等（参考Ceres官方文档 http://ceres-solver.org/nnls_modeling.html#instances ）；该参数可以取NULL或nullptr，此时损失函数为单位函数。
- parameter_blocks:
待优化的参数，可一次性传入所有参数的指针容器vector<double*>或依次传入所有参数的指针double*。
一些应用实例
// 例子1: 来自ceres-solver-1.14.0/examples/helloworld.cc
struct CostFunctor {
template <typename T> bool operator()(const T* const x, T* residual) const {
residual[0] = 10.0 - x[0];
return true;
}
};
double x = 0.5;
// Build the problem.
Problem problem;
// 使用自动求导构建代价函数
CostFunction* cost_function =
new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
// 添加残差
problem.AddResidualBlock(cost_function, NULL, &x);
// 例子2: 来自ceres-solver-1.14.0/examples/powell.cc
struct F1 {
template <typename T> bool operator()(const T* const x1,
const T* const x2,
T* residual) const {
// f1 = x1 + 10 * x2;
residual[0] = x1[0] + 10.0 * x2[0];
return true;
}
};
double x1 = 3.0;
double x2 = -1.0;
Problem problem;
problem.AddResidualBlock(new AutoDiffCostFunction<F1, 1, 1, 1>(new F1),
NULL,
&x1, &x2);
// 例子3: 来自ceres-solver-1.14.0/examples/bundle_adjuster.cc
CostFunction* cost_function;
cost_function =
(FLAGS_use_quaternions)
? SnavelyReprojectionErrorWithQuaternions::Create(
observations[2 * i + 0],
observations[2 * i + 1])
: SnavelyReprojectionError::Create(
observations[2 * i + 0],
observations[2 * i + 1]);
LossFunction* loss_function = FLAGS_robustify ? new HuberLoss(1.0) : NULL;
double* camera =
cameras + camera_block_size * bal_problem->camera_index()[i];
double* point = points + point_block_size * bal_problem->point_index()[i];
problem->AddResidualBlock(cost_function, loss_function, camera, point);
2.2 Problem::AddParameterBlock
用户在调用AddResidualBlock时其实已经隐式地向Problem传递了参数模块，但在一些情况下，需要用户显示地向Problem传入参数模块（通常出现在需要对优化参数进行重新参数化的情况）。Ceres提供了Problem::AddParameterBlock函数用于用户显式传递参数模块。函数原型
void AddParameterBlock(double* values, int size);
void AddParameterBlock(double* values,
int size,
LocalParameterization* local_parameterization);
其中，第一种函数原型除了会增加一些额外的参数检查之外，功能上和隐式传递参数并没有太大区别。第二种函数原型则会额外传入LocalParameterization参数，用于重构优化参数的维数。
查看第3节介绍LocalParameterization
3.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;
};
除了LocalParameterization，还有它的一些子类：
- IdentityParameterization
- SubsetParameterization
- QuaternionParameterization
- EigenQuaternionParameterization
- HomogeneousVectorParameterization
- ProductParameterization
举例介绍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;
}
TODO
Plus和ComputeJacobian参考
https://blog.csdn.net/jdy_lyy/article/details/119360492?utm_medium=distribute.pc_relevant.none-task-blog-2~default~baidujs_title~default-0.no_search_link&spm=1001.2101.3001.4242
https://blog.csdn.net/hjwang1/article/details/107869743
https://blog.csdn.net/weixin_43991178/article/details/100532618
3.2 自定义LocalParameterization
用户自定义一个类继承LocalParameterization类，主要实现的有
Plus(const double* x,const double* delta,double* x_plus_delta)：定义变量的加法
ComputeJacobian()：x对delta的雅克比矩阵
GlobalSize()：x 的自由度（可能有冗余），比如四元数的自由度是4
LocalSize()：Δx 所在的正切空间（tangent space）的自由度，那么这个自由度是3
可以参考上面的QuaternionParameterization类
参考
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
https://blog.csdn.net/weixin_43991178/article/details/100532618
https://blog.csdn.net/hjwang1/article/details/107869743
