没有合适的资源?快使用搜索试试~ 我知道了~
g2o源码阅读笔记 g2o源码阅读笔记g2o源码阅读笔记g2o源码阅读笔记g2o源码阅读笔记g2o源码阅读笔记g2o源码阅读笔记g2o源码阅读笔记g2o源码阅读笔记g2o源码阅读笔记 非线性优化框架 ORB-SLAM LM优化 SLAM后端
资源推荐
资源详情
资源评论
g2o 源码阅读
1 图的构建
顶点的添加
optimizer.addVertex(vSE3);
实际调用
bool OptimizableGraph::addVertex(HyperGraph::Vertex* v, Data* userData)
主要工作:
1 通过 ID 判断 顶点是否已经被插入过
2 return HyperGraph::addVertex(v);
bool HyperGraph::addVertex(Vertex* v)
中 _vertices.
insert
(
std
::
make_pair
(v->id(),v) );
_vertices 类型为
std
::
tr1
::
unordered_map
<int, Vertex*>
optimizer.addEdge(e);
bool OptimizableGraph::addEdge(HyperGraph::Edge* e_)
1 调用 HyperGraph::addEdge(e);
2 e->_internalId = _nextEdgeId++;
3 e->resolveParameters()
4 e->resolveCaches()
5 _jacobianWorkspace.updateSize(e);
第 3,4 步 是做一些检查, 还没细看
第一步:
typedef
std
::
set
<Edge*> EdgeSet;
插入一条边到集合中, 然后
遍历边的每一个顶点,顶点的边集合中插入 这条边, 这个后面 buildStructure 中有用到!
2 初始化
bool SparseOptimizer::initializeOptimization(int level)
1 为 Jacobian 分配空间
_jacobianWorkspace.allocate()
typedef
std
::
vector
<Eigen::VectorXd> WorkspaceVector;
2 clearIndexMapping()
清空_ivMap 中的数据,_ivMap 是顶点集合,从后门看,应该是非固定顶点的集合
3
_activeVertices.
clear
();
_activeVertices.
reserve
(vset.
size
());
_activeEdges.
clear
();
4 遍历所有顶点,检查 顶点和边的关系是否完整,
基本上可以把 上面几个 _active 开头的变量理解为本次参与优化的顶点和边的集合。
5 sortVectorContainers();
根据顶点和边的 id 对顶点和 边进行排序
_activeVertices 、 _activeEdges
6
buildIndexMapping(_activeVertices)
大概意思:
固定的顶点, HessianIndex 为-1, 不 Marg 的顶点,也就是位姿对应的顶点的 HessianIndex 从 0
开始累加, 然后是 需要 Marg 的顶点 HessianIndex 继续累加
_ivMap 是 vector,按照 HessianIndex 的顺序保存了 不固定的顶点,
3 优化
int SparseOptimizer::optimize(int iterations, bool online)
1 _algorithm->init(online);
遍历所有顶点,如果至少有一个要 marg,那么 useSchur 设置为 true
_solver->init(_optimizer, online)
bool BlockSolver<Traits>::init(SparseOptimizer* optimizer, bool online)
_Hpp, _Hpl, _Hll 三个矩阵清空
_init 置为 true
2 进行若干次迭代!
1 preIteration(i) ,可以理解为没做啥事
2 _algorithm->solve(i, online)
OptimizationAlgorithmLevenberg::
solve(int iteration, bool online)
1 _solver->buildStructure() ---> 构建 H 矩阵及相关数据的内存结构
2 _optimizer->computeActiveErrors() ---> 计算每条边的误差
3 double currentChi = _optimizer->activeRobustChi2(); --->计算每条边总和
4 _solver->buildSystem(); --->计算 Jacobian 和 H 矩阵、b 向量
5 _currentLambda = computeLambdaInit();
6 进入一个循环, _solver->setLambda(_currentLambda, true);
然后 bool ok2 = _solver->solve(); --->求解一次待优化变量的增量
_optimizer->update(_solver->x()); --->增量叠加对待优化变量进行更新
_solver->restoreDiagonal();
_optimizer->computeActiveErrors(); --->使用更新后的解计算误差
tempChi = _optimizer->activeRobustChi2();
根据情况 更新 _currentLambda
bool BlockSolver<Traits>::
buildStructure(bool zeroBlocks)
1 遍历所有顶点,根据是否 Marg, 建立顶点的_colInHessian 信息,看名字是 在 Hessian 矩阵中的
列 数 , 并 且 建 立 了 blockPoseIndices 和 blockLandmarkIndices 两 个 数 组 , 元 素 值 是
_colInHessian。 索引为 pose 或者 landmark 的顺序号,各自从 0 开始计数
剩余15页未读,继续阅读
资源评论
pangdawa
- 粉丝: 124
- 资源: 9
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功