使用Ceres进行slam必须要弄清楚的几个类和函数
- ceres简介
- ceres的使用流程
- ceres必须要知道的类和函数
- class LossFunction
- LocalParameterization
- class problem
- class CostFunction
- class AutoDiffCostFunction
Ceres solver 是谷歌开发的一款用于非线性优化的库，在谷歌的开源激光雷达slam项目cartographer中被大量使用。
在之前的博客说了,图优化的本质就是一个非线性优化问题.所以ceres刚好适用图优化问题的解决.
在进行特征点匹配后进行迭代的优化最优变换位姿时也可以使用ceres.
ceres简介
Ceres可以解决边界约束鲁棒非线性最小二乘法优化问题。这个概念可以用以下表达式表示：
这一表达式在工程和科学领域有非常广泛的应用。比如统计学中的曲线拟合，或者在计算机视觉中依据图像进行三维模型的构建等等。
注意这个公式里面各模块有几个特殊的概念要熟知,涉及到具体的使用:
残差块（ResidualBlock）:
这一部分被称为残差块（ResidualBlock）.代价函数（CostFunction）:
这一部分被称为代价函数（CostFunction）.参数块（ParameterBlock）:
代价函数依赖于一系列参数,这一系列参数（均为标量）称为参数块（ParameterBlock）.当然参数块中也可以只含有一个变量上下边界:
lj和uj是xj的上下边界.损失函数（LossFunction）:
pi是损失函数（LossFunction）.按照损失函数的是一个标量函数，其作用是减少异常值（Outliers）对优化结果的影响。其效果类似于对函数的过滤。
ceres的使用流程
1.构建代价函数(cost function)
2.通过代价函数构建待求解的优化问题
3.配置求解器参数并求解问题
ceres必须要知道的类和函数
class LossFunction
class LossFunction 损失函数
最小二乘问题的输入数据可能包含异常值(错误测量得到的)，使用损失函数减少这部分数据的影响。
比如说当一个移动相机的场景中,街道上有消防栓和汽车,当图像的处理算法把消防栓的尖和汽车的前灯匹配在了一起,那么如果不做任何处理,则会导致ceres为使这个错误的大的误差减小,而是优化结果偏离正确位置.
LossFunction可以让大的残差的权重降低,从而对 最终的优化结果没有太大的影响.
class LossFunction {
public:
virtual void Evaluate(double s, double out[3]) const = 0;
};LossFunction的类,关键的函数就是 LossFunction::Evaluate()
一个非负的参数s,计算输出
Ceres包含了几种定义好的损失函数,都是没有缩放的.具体效果如下图所示:
图中红色的就是没有经过损失函数的,y=x*x.蓝色的是HuberLoss,值低于正常值,并且x越大,效果越明显.
正常的是:
HuberLoss是:
SoftLOneLoss是:
CauchyLoss是:
ArctanLoss是:
TolerantLoss是:
用定义好的损失函数使用也很简单.例如:
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);定义ceres 的 损失函数 0.1代表 残差大于0.1的点 ,则权重降低,具体效果看上面的公式. 小于0.1 则认为正常,不做特殊的处理
定义好后,在添加残差
LocalParameterization
LocalParameterization 本地参数
在许多优化问题中，尤其是传感器融合问题中，必须对存在于称为流形的空间中的量进行建模，例如由四元数表示的传感器的旋转/方向。
Ceres定义了一些特殊的参数,对于slam,用的更多的就是旋转的四元数
QuaternionParameterization
EigenQuaternionParameterization
定义两个主要的原因就是Eigen的存储四元数的方式和一般的不同,Eigen是x,y,z,w,实数部分的w放在最后,一般的则是:w,x,y,z.
使用
double para_q[4] = {0, 0, 0, 1};
ceres::LocalParameterization *q_parameterization =
new ceres::EigenQuaternionParameterization();
problem.AddParameterBlock(para_q, 4, q_parameterization);class problem
problem类就是代表者具有双边约束的最小二乘问题
为了创建一个最小二乘问题,需要使用
Problem::AddResidalBlock() 添加残差模块
Problem::AddParameterBlock() 添加参数模块
这两个方法
举个例子,一个问题包含三个参数模块,尺寸分别是3,4,5,两个残差模块尺寸分别是2和6
double x1[] = { 1.0, 2.0, 3.0 };
double x2[] = { 1.0, 2.0, 3.0, 5.0 };
double x3[] = { 1.0, 2.0, 3.0, 6.0, 7.0 };
Problem problem;
problem.AddResidualBlock(new MyUnaryCostFunction(...), x1);
problem.AddResidualBlock(new MyBinaryCostFunction(...), x2, x3);方法Problem::AddResidualBlock(),和名字一样,功能就是添加参数模块到problem中,这个方法必须有参数CostFunction和可选参数LossFunction,这个方法就连接了CostFunction和参数模块.
CostFunction 具有它希望的参数块尺寸的信息.
这个函数检查这些是否与 parameter_blocks 中列出的参数块的大小相匹配 .如果检测到错误的匹配,程序会终止.
LossFunction 可以有,也可以没有
可以使用Problem::AddParameterBlock() 这个方法来添加参数模块,这个会添加一次参数尺寸的检测.将参数块显式添加到问题中。 还允许将 Manifold 对象与参数块相关联。
这个函数可以用带LocalParameterization的参数,也可以不带.
problem.AddParameterBlock(para_q, 4, q_parameterization);// 添加四元数的参数块
problem.AddParameterBlock(para_t, 3);//添加平移的参数块声明的时候一般这样:
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);先声明一个ceres::Problem::Options,然后再用Options初始化problem
class CostFunction
代价函数CostFunction 负责计算残差向量和雅克比矩阵.
代价函数依赖参数块
其内部定义是这样的
class CostFunction {
public:
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) = 0;
const vector<int32>& parameter_block_sizes();
int num_residuals() const;
protected:
vector<int32>* mutable_parameter_block_sizes();
void set_num_residuals(int num_residuals);
};这部分不用太管,因为使用的时候用其它类定义的这个类的内部
定义 CostFunction 或 SizedCostFunction 可能容易出错，尤其是在计算导数时。 为此，Ceres 提供了自动微分。
class AutoDiffCostFunction
定义 CostFunction 或 SizedCostFunction 可能容易出错，尤其是在计算导数时。 为此，Ceres 提供了自动微分。
template <typename CostFunctor,
int kNumResiduals, // Number of residuals, or ceres::DYNAMIC.
int... Ns> // Size of each parameter block
class AutoDiffCostFunction : public
SizedCostFunction<kNumResiduals, Ns> {
public:
AutoDiffCostFunction(CostFunctor* functor, ownership = TAKE_OWNERSHIP);
// Ignore the template parameter kNumResiduals and use
// num_residuals instead.
AutoDiffCostFunction(CostFunctor* functor,
int num_residuals,
ownership = TAKE_OWNERSHIP);
};得到一个可以自动求导的代价函数,必须定义一个类或者结构体在里面重载运算符,在里面实现用参数模板计算代价函数,重载的运算符必须在最后一个参数里存入计算结果,并且返回true.
举个例子,要计算 一个 代价函数是 e= k - xTy.
x和y是二维的向量,k是一个不变的参数.
那么可以定义一个这样的类
class MyScalarCostFunctor {
MyScalarCostFunctor(double k): k_(k) {}
template <typename T>
bool operator()(const T* const x , const T* const y, T* e) const {
e[0] = k_ - x[0] * y[0] - x[1] * y[1];
return true;
}
private:
double k_;
};在重载运算符的定义里面.参数x和y在前面,如果有更多的输入参数,则继续可以排在y后面,输出也就是残差放在最后一个参数,
给定这个类的定义之后，它的自动微分代价函数可以定义如下:
CostFunction* cost_function
= new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>(
new MyScalarCostFunctor(1.0)); ^ ^ ^
| | |
Dimension of residual ------+ | |
Dimension of x ----------------+ |
Dimension of y -------------------+上面的1,2,2 就是注释的那样,计算1维的残差,2个2维的优化量
