1. 最大团匹配方法
最大团匹配方法区别于其它其它匹配方法的有点就是可以得到整体最优。缺点是随着语义增多,算法复杂度增长很快。可以利用最大团匹配方法进行初始化。主要参考下面的论文。
CLIPPER: A Graph-Theoretic Framework for Robust Data Association
链接:clipper 论文
作者:Parker C. Lusk*, Kaveh Fathian*, Jonathan P. How
2.clipper匹配代码
链接:clipper 代码库
3. 数值法求雅克比
优化最核心的步骤就是求出目标函数的雅克比矩阵,然后利用高斯牛顿法或者梯度下降法都是可以的。而求雅克比矩阵在工程上主要有两种方法:一种是数值法,此种方法简单,但是对处理器的运算量大。一种链式法则解析法,此种方法求出解析雅克比矩阵,但是求出过程推导复杂,对处理器运算量要求较小。下面介绍数值法。
y
=
f
(
x
)
y=f(x)
y=f(x)
在X0附近,对每一维加一个小量(delta_x(i) = 1e-6),求出delta_y。可以得到雅克比矩阵的第i列。
J(:,i)= delta_y/delta_x(i).
注:这种方法对于f形式复杂时候可以避免计算解析形式的雅克比。但是每次迭代都要重新计算,增加了算法量。
4.链式法则求解析雅克比
本节介绍解析法求雅克比矩阵。最大好处就是3中所说的形式复杂的函数进行分阶段求导完成。
如:向量y和向量x表达如下。
y
=
f
(
x
)
y=f(x)
y=f(x)
很难直接求雅克比时,可以引进一些中间向量出来,复杂的雅克比转换为多个简单的雅克比。
y
=
f
1
(
a
)
,
a
=
f
2
(
b
)
,
b
=
f
3
(
x
)
J
y
x
=
J
y
a
∗
J
a
b
∗
J
b
x
y=f_1(a),a=f_2(b),b=f_3(x)\\ J_{yx}=J_{ya}*J_{ab}*J_{bx}
y=f1(a),a=f2(b),b=f3(x)Jyx=Jya∗Jab∗Jbx
链式法则的详细描述见第六到七篇。关于各种问题的位姿求解详见第六到十篇。
5.ceres优化代码
在工程上经常采用ceres库进行优化,ceres库的重要优化函数包含如下。
链接:步骤
5.1 建立问题
ceres::Problem problem;
ceres::LossFunction *loss_function;
//loss_function = new ceres::HuberLoss(1.0);
loss_function = new ceres::CauchyLoss(1.0);
5.2 添加参数块
void AddParameterBlock(double* values, int size);
void AddParameterBlock(double* values,
int size,
LocalParameterization* local_parameterization);
5.3 添加残差块
template <typename... Ts>
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0,
Ts*... xs)
ResidualBlockId AddResidualBlock(
CostFunction* cost_function,
LossFunction* loss_function,
const std::vector<double*>& parameter_blocks);
也就是需要提供三种参数 —— cost_function对象、鲁棒核函数对象、 该残差的关联参数.
两种方法提供优化选项:手动求导和自动求导。