【目标】:使用LM算法估计
y
=
e
x
p
(
a
t
2
+
b
t
+
c
)
y=exp(at^2+bt+c)
y=exp(at2+bt+c)参数a, b, c
。
已知量:
t
t
t及与之对应的含有噪声的量测值
y
^
\hat{y}
y^;同时,模型已知(仅未知参数)。
仿真数据生成
取参数为a=1.0, b=2.0, c=1.0
double a=1.0, b=2.0, c=1.0; // 真实参数值
int N = 100; // 数据点数目
double w_sigma= 1.; // 噪声Sigma值(方差)
std::default_random_engine generator;
std::normal_distribution<double> noise(0.,w_sigma);
// 构造 N 次观测
for (int i = 0; i < N; ++i) {
double t = i/100.; // t的取值在0.0~1.0之间
double n = noise(generator);
double y = std::exp( a*t*t + b*t + c ) + n; // // 含噪声的观测 y
}
确定待优化变量、残差函数、及损失函数
待优化变量(Vertex): 参数a, b, c
,此时放在一个
3
×
1
3\times 1
3×1的向量
x
x
x中;
残差函数(Edge): 以第k
步迭代参数的估计值
x
k
x_k
xk(参数)和第i
个自变量代入已知模型得到的预测值
y
^
k
\hat{y}_k
y^k,和第k
步迭代的含有噪声的量测值
y
k
y_k
yk的差值作为残差。记为
f
i
(
x
k
)
f_i(x_k)
fi(xk)
损失函数(Problem):
F
(
x
k
)
=
Σ
i
=
1
N
∥
f
i
(
x
k
)
∥
2
F(x_k)=\Sigma_{i=1}^N\|f_i(x_k)\|_2
F(xk)=Σi=1N∥fi(xk)∥2
【代码1-待优化变量】:
shared_ptr< CurveFittingVertex > vertex(new CurveFittingVertex());
vertex->SetParameters(Eigen::Vector3d (0.,0.,0.));
其中,CurveFittingVertex
继承自Vertex
,且在构造函数中指明了一个3维向量:
// 曲线模型的顶点,模板参数:优化变量维度
class CurveFittingVertex: public Vertex{
public:
CurveFittingVertex(): Vertex(3) {} // abc: 三个参数, Vertex 是 3 维的
};
而Vertex
在构造函数中,其成员变量VecX Parameters_;
被resize
为一个3维向量:
explicit Vertex(int num_dimension, int local_dimension = -1);//类内的声明
Vertex::Vertex(int num_dimension, int local_dimension) { // 类外的定义
parameters_.resize(num_dimension, 1);
id_ = global_vertex_id++; // 初始值有:unsigned long global_vertex_id = 0;
}
注:VecX
实际上是一个别名typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VecX;
。
【代码2-残差项】
对于每一个观测量,构造其对应的残差(此时还并未真正去计算):
// 每个观测对应的残差函数
shared_ptr< CurveFittingEdge > edge(new CurveFittingEdge(t,y));
// CurveFittingEdge 构造函数为:
class CurveFittingEdge: public Edge{
public:
CurveFittingEdge( double x, double y ): Edge(1,1, std::vector<std::string>{"abc"}) {
x_ = x;
y_ = y;
}
...
}
父类Edge
的构造函数为:
Edge::Edge(int residual_dimension, int num_verticies,
const std::vector<std::string> &verticies_types) {
residual_.resize(residual_dimension, 1); // 残差向量
jacobians_.resize(num_verticies); // 雅可比矩阵
// 信息矩阵
Eigen::MatrixXd information(residual_dimension, residual_dimension);
information.setIdentity();
information_ = information;
}
...
补充其中用到的几个类型
VecX residual_; // 残差
std::vector<MatXX> jacobians_; // 雅可比,每个雅可比维度是 residual x vertex[i]
MatXX information_; // 信息矩阵
VecX observation_; // 观测信息
同时,对于该观测量及其对应的残差,指定它们与哪些待优化的变量(参数)有关!
std::vector<std::shared_ptr<Vertex>> edge_vertex;
edge_vertex.push_back(vertex); //本次曲线拟合例题中仅一个顶点(其他或许多个)
edge->SetVertex(edge_vertex); // 指定该edge对应的顶点
// 该成员函数的定义:
bool SetVertex(const std::vector<std::shared_ptr<Vertex>> &vertices) {
verticies_ = vertices;
return true;
}
// 其中,成员变量的定义:
std::vector<std::shared_ptr<Vertex>> verticies_; // 该边对应的顶点
【代码3-损失函数与优化】
问题的最终构建和求解
Problem problem(Problem::ProblemType::GENERIC_PROBLEM);
// 其构造函数
Problem::Problem(ProblemType problemType) :
problemType_(problemType) {
verticies_marg_.clear();
}
// 其成员变量类型
HashVertex verticies_marg_;
typedef std::map<unsigned long, std::shared_ptr<Vertex>> HashVertex;
添加Vertex
:
problem.AddVertex(vertex);
// 该成员函数的定义:
bool Problem::AddVertex(std::shared_ptr<Vertex> vertex) {
if (verticies_.find(vertex->Id()) != verticies_.end()) {
return false; // 重复的顶点不会再添加,根据ID来判定是否重复。
} else {
verticies_.insert(pair<unsigned long, shared_ptr<Vertex>>(vertex->Id(), vertex));
}
return true;
}
// 其成员变量:
HashVertex verticies_; /// all vertices
(注):每个vertex都有自己的ID。在本例题中,只有一个顶点,因此在使用和优化该顶点的参数时,经常看到Vec3 abc = verticies_[0]->Parameters();
中的0,本质是本例题中仅有的一个顶点的ID。
添加Edge
:
problem.AddEdge(edge);
// 该成员函数定义:
bool Problem::AddEdge(shared_ptr<Edge> edge) {
edges_.insert(pair<ulong, std::shared_ptr<Edge>>(edge->Id(), edge));
for (auto &vertex: edge->Verticies()) {
vertexToEdge_.insert(pair<ulong, shared_ptr<Edge>>(vertex->Id(), edge));
}
return true;
}
// 使用的成员变量
HashEdge edges_; /// all edges
HashVertexIdToEdge vertexToEdge_; /// 由vertex id查询edge (一对多)
typedef std::unordered_map<unsigned long, std::shared_ptr<Edge>> HashEdge;
typedef std::unordered_multimap<unsigned long, std::shared_ptr<Edge>> HashVertexIdToEdge;
求解
problem.Solve(30);
// 该成员函数的定义:
bool Problem::Solve(int iterations) {
SetOrdering(); // 统计待估计的所有变量的总维度,为构建 H 矩阵做准备
MakeHessian(); // 遍历edge, 构建 H = J^T * J 矩阵
ComputeLambdaInitLM(); // LM 初始化
// LM 算法迭代求解
bool stop = false;
int iter = 0;
while (!stop && (iter < iterations)) {
bool oneStepSuccess = false;
int false_cnt = 0;
while (!oneStepSuccess) { // 不断尝试 Lambda, 直到成功迭代一步
AddLambdatoHessianLM(); // setLambda
// 第四步,解线性方程 H X = B
SolveLinearSystem();
//
RemoveLambdaHessianLM();
// 优化退出条件1: delta_x_ 很小则退出
if (delta_x_.squaredNorm() <= 1e-6 || false_cnt > 10) {
stop = true;
break;
}
// 更新状态量 X = X+ delta_x
UpdateStates();
// 判断当前步是否可行以及 LM 的 lambda 怎么更新
oneStepSuccess = IsGoodStepInLM();
// 后续处理,
if (oneStepSuccess) {
// 在新线性化点 构建 hessian
MakeHessian();
false_cnt = 0;
} else {
false_cnt++;
RollbackStates(); // 误差没下降,回滚
}
}
iter++;
// 优化退出条件3: currentChi_ 跟第一次的chi2相比,下降了 1e6 倍则退出
if (sqrt(currentChi_) <= stopThresholdLM_)
stop = true;
}
return true;
}
依次简要看一下成员函数:
1.统计维数
成员函数Problem::SetOrdering()的定义:
// Note:: verticies_ 是 map 类型的, 顺序是按照 id 号排序的
// 统计待估计的所有变量的总维度
for (auto vertex: verticies_) {
ordering_generic_ += vertex.second->LocalDimension(); // 所有的优化变量总维数
}
(注):对于本例题是3维!!
2.求解Hessian
void Problem::MakeHessian() {
// 直接构造大的 H 矩阵
ulong size = ordering_generic_; // 维数
MatXX H(MatXX::Zero(size, size));
VecX b(VecX::Zero(size));
// 遍历每个残差,并计算他们的雅克比,得到最后的 H = J^T * J
for (auto &edge: edges_) {
// 其中的一个edge,计算残差和雅可比
edge.second->ComputeResidual(); // 这两个成员函数在子类中进行了override
edge.second->ComputeJacobians(); // 关于它俩的推导,见补充
std::vector<MatXX> jacobians = edge.second->Jacobians();
VecX verticies = edge.second->Verticies();
assert(jacobians.size() == verticies.size()); // 两个都是vector,本例题中为1
// 顶点中的每一个参数
for (size_t i = 0; i < verticies.size(); ++i) {
auto v_i = verticies[i];
if (v_i->IsFixed()) continue; // Hessian 里不需要添加它的信息,也就是它的雅克比为 0
auto jacobian_i = jacobians[i]; // 该参数对应的雅可比
ulong index_i = v_i->OrderingId();
ulong dim_i = v_i->LocalDimension();
// 此处PDF上直接为 H = J^T * J ?
MatXX JtW = jacobian_i.transpose() * edge.second->Information();
// 上三角
for (size_t j = i; j < verticies.size(); ++j) {
auto v_j = verticies[j];
if (v_j->IsFixed()) continue;
auto jacobian_j = jacobians[j];
ulong index_j = v_j->OrderingId();
ulong dim_j = v_j->LocalDimension();
assert(v_j->OrderingId() != -1);
MatXX hessian = JtW * jacobian_j;
// 所有的信息矩阵叠加起来(即填充了一个H中的一块区域)
H.block(index_i, index_j, dim_i, dim_j).noalias() += hessian;
if (j != i) { // 对称的下三角
H.block(index_j, index_i, dim_j, dim_i).noalias() += hessian.transpose();
}
}
b.segment(index_i, dim_i).noalias() -= JtW * edge.second->Residual();
}
}
Hessian_ = H;
b_ = b;
delta_x_ = VecX::Zero(size); // initial delta_x = 0_n;
}
代码中出现了Eigen中的一个noalias()语句,其解释见博客链接:https://www.cnblogs.com/houkai/p/6349990.html
总而言之,该部分实现了正则方程(normal equation)中系数的确定,即
H
Δ
x
=
b
H\Delta x=b
HΔx=b中的
H
H
H和
b
b
b,下一步求解其中的增量
Δ
x
\Delta x
Δx。
【补充:】子类中override
的两个成员函数的推导和代码:(仅适用于该例题)
y
=
e
a
t
2
+
b
t
+
c
,
x
=
[
a
,
b
,
c
]
T
y=e^{at^2+bt+c},\quad x=[a,b,c]^T
y=eat2+bt+c,x=[a,b,c]T
∂
y
∂
x
=
[
∂
y
∂
a
∂
y
∂
b
∂
y
∂
c
]
=
[
∂
y
∂
a
∂
y
∂
b
∂
y
∂
c
]
=
[
t
2
⋅
y
t
⋅
y
y
]
\frac{\partial y}{\partial x}=\begin{bmatrix} \frac{\partial y}{\partial a}\\ \frac{\partial y}{\partial b}\\ \frac{\partial y}{\partial c} \end{bmatrix}=\begin{bmatrix} \frac{\partial y}{\partial a}\\ \frac{\partial y}{\partial b}\\ \frac{\partial y}{\partial c} \end{bmatrix}=\begin{bmatrix} t^2\cdot y\\ t\cdot y \\ y \end{bmatrix}
∂x∂y=⎣⎡∂a∂y∂b∂y∂c∂y⎦⎤=⎣⎡∂a∂y∂b∂y∂c∂y⎦⎤=⎣⎡t2⋅yt⋅yy⎦⎤
y
(
x
+
Δ
x
)
=
y
(
x
)
+
∂
y
∂
x
Δ
x
y(x+\Delta x)=y(x)+\frac{\partial y}{\partial x}\Delta x
y(x+Δx)=y(x)+∂x∂yΔx
// 计算曲线模型误差
virtual void ComputeResidual() override{
Vec3 abc = verticies_[0]->Parameters(); // 估计的参数 // 顶点有自己的ID
residual_(0) = std::exp( abc(0)*x_*x_ + abc(1)*x_ + abc(2) ) - y_; // 构建残差
}
// 计算残差对变量的雅克比
virtual void ComputeJacobians() override{
Vec3 abc = verticies_[0]->Parameters();
double exp_y = std::exp( abc(0)*x_*x_ + abc(1)*x_ + abc(2) );
Eigen::Matrix<double, 1, 3> jaco_abc; // 误差为1维,状态量 3 个,所以是 1x3 的雅克比矩阵
jaco_abc << x_ * x_ * exp_y, x_ * exp_y , 1 * exp_y;
jacobians_[0] = jaco_abc;
}
2.2 LM的H矩阵
LM算法求解的是方程:
(
J
T
J
+
λ
I
)
Δ
x
=
−
J
T
f
(J^TJ+\lambda I)\Delta x = -J^Tf
(JTJ+λI)Δx=−JTf,目前求得了其中的
H
=
J
T
J
H=J^TJ
H=JTJ,
b
=
−
J
T
f
b=-J^Tf
b=−JTf。目前还差一个
λ
\lambda
λ(它在每一步优化中动态调整)。
一个简单的
λ
\lambda
λ的初始化策略:
λ
0
=
τ
⋅
m
a
x
{
(
J
T
J
)
i
i
}
\lambda_0=\tau\cdot max\{(J^TJ)_{ii}\}
λ0=τ⋅max{(JTJ)ii}
其中,
τ
\tau
τ一般取
[
1
0
−
8
,
1
]
[10^{-8},1]
[10−8,1]。
void Problem::ComputeLambdaInitLM() {
double maxDiagonal = 0;
ulong size = Hessian_.cols();
assert(Hessian_.rows() == Hessian_.cols() && "Hessian is not square");
for (ulong i = 0; i < size; ++i) {
maxDiagonal = std::max(fabs(Hessian_(i, i)), maxDiagonal);
}
double tau = 1e-5;
currentLambda_ = tau * maxDiagonal;
}
一种
λ
\lambda
λ的更新策略:
ρ
=
F
(
x
)
−
F
(
x
+
Δ
x
)
L
(
0
)
−
L
(
Δ
x
)
\rho=\frac{F(x)-F(x+\Delta x)}{L(0)-L(\Delta x)}
ρ=L(0)−L(Δx)F(x)−F(x+Δx)
λ
:
=
λ
m
a
x
{
1
3
,
1
−
(
2
ρ
−
1
)
3
}
;
ν
:
=
2
;
(
ρ
>
0
)
λ
:
=
λ
∗
ν
;
ν
:
=
2
∗
ν
;
(
ρ
≤
0
)
\begin{aligned} \lambda := \lambda max\{\frac{1}{3},1-(2\rho-1)^3\}; \quad \nu:=2; \quad & (\rho>0) \\ \lambda := \lambda * \nu;\quad \nu:=2*\nu; \quad & (\rho \leq 0) \end{aligned}
λ:=λmax{31,1−(2ρ−1)3};ν:=2;λ:=λ∗ν;ν:=2∗ν;(ρ>0)(ρ≤0)
其中,
L
(
Δ
x
)
L(\Delta x)
L(Δx)为损失函数二阶泰勒展开的近似,即
F
(
x
+
Δ
x
)
≈
L
(
Δ
x
)
=
F
(
x
)
+
J
Δ
x
+
1
2
Δ
x
T
H
Δ
x
F(x+\Delta x)\approx L(\Delta x)=F(x)+J\Delta x+\frac{1}{2}\Delta x^TH\Delta x
F(x+Δx)≈L(Δx)=F(x)+JΔx+21ΔxTHΔx。且有:
L
(
0
)
−
L
(
Δ
x
)
=
1
2
Δ
x
T
(
λ
Δ
x
+
b
)
L(0)-L(\Delta x)=\frac{1}{2}\Delta x^T(\lambda\Delta x+b)
L(0)−L(Δx)=21ΔxT(λΔx+b)
下面的代码对应的非常完美:
F
(
x
)
F(x)
F(x)对应currentChi
,
F
(
x
+
Δ
x
)
F(x+\Delta x)
F(x+Δx)对应tempChi
。
bool Problem::IsGoodStepInLM() {
double scale = 0;
scale = delta_x_.transpose() * (currentLambda_ * delta_x_ + b_); // 似乎少了1/2
scale += 1e-3; // make sure it's non-zero :)
// recompute residuals after update state
// 统计所有的残差
double tempChi = 0.0;
for (auto edge: edges_) {
edge.second->ComputeResidual();
tempChi += edge.second->Chi2();
}
double rho = (currentChi_ - tempChi) / scale;
if (rho > 0 && isfinite(tempChi)) // last step was good, 误差在下降
{
double alpha = 1. - pow((2 * rho - 1), 3);
alpha = std::min(alpha, 2. / 3.); // 此处比公式中还多了一步
double scaleFactor = (std::max)(1. / 3., alpha);
currentLambda_ *= scaleFactor;
ni_ = 2;
currentChi_ = tempChi;
return true;
} else {
currentLambda_ *= ni_;
ni_ *= 2;
return false; // 用新的lambda值再去【重新计算】当前步
}
}
3.求解 Δ x \Delta x Δx
// Solve Hx = b, we can use PCG iterative method or use sparse Cholesky
void Problem::SolveLinearSystem() {
delta_x_ = Hessian_.inverse() * b_; // 很直接的方法
}
求得 Δ x \Delta x Δx之后,首先要根据 ρ \rho ρ判断它是否是一次有效的更新(确实使得损失函数下降);在是有效更新的前提下,考虑是否满足终止条件。
4.终止条件
优化退出条件1: delta_x_ 很小则退出
if (delta_x_.squaredNorm() <= 1e-6) {
stop = true;
break;
}