LM 算法参数优化实例——残差函数、雅可⽐矩阵
【⽬标】：使⽤LM算法估计参数a, b, c 。
已知量：及与之对应的含有噪声的量测值；同时，模型已知（仅未知参数）。仿真数据⽣成取参数为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 ，此时放在⼀个的向量中；残差函数（Edge）： 以第k 步迭代参数的估计值（参数）和第i 个⾃变量代⼊已知模型得到的预测值，和第k 步迭代的含有噪声的量测值的差值作为残差。记为损失函数（Problem）： 【代码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-残差项】
对于每⼀个观测量，构造其对应的残差（此时还并未真正去计算）：
y =exp (at +2bt +c )t y
^3×1x x k y
^k y k f (x )
i k F (x )=k Σ∥f (x )∥i =1N
i k 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 初始化
/
rows函数的使用方法及实例/ 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 = anspose() * 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() += anspose();
}
}
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;
}
版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。
发表评论