logo资料库

g2o源码阅读.docx

发布时间：2022-06-15 发布人：admin 分类：说明书 资料大小：0.14M 资料格式：docx 举报 版权申诉
第1页 / 共11页
第2页 / 共11页
第3页 / 共11页
第4页 / 共11页
第5页 / 共11页
第6页 / 共11页
第7页 / 共11页
第8页 / 共11页
资料共11页，剩余部分请下载后查看
    1 图的构建
    2 初始化
    3 优化
    solve(int iteration, bool online)
    buildStructure(bool zeroBlocks)
    稀疏矩阵两种表示
    computeActiveErrors()
    _optimizer->activeRobustChi2();
    buildSystem()
    computeLambdaInit()
    BlockSolver::
    setLambda
    solve()
    内存释放
    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 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 插入一条边到集合中， 然后 EdgeSet; 遍历边的每一个顶点，顶点的边集合中插入 这条边， 这个后面 buildStructure 中有用到！ 2 初始化 bool SparseOptimizer::initializeOptimization(int level) 1 为 Jacobian 分配空间 _jacobianWorkspace.allocate() typedef std::vector 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::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() 2 _optimizer->computeActiveErrors() 3 double currentChi = _optimizer->activeRobustChi2(); 4 _solver->buildSystem(); 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:: buildStructure(bool zeroBlocks) 1 遍历所有顶点，根据是否 Marg, 建立顶点的_colInHessian 信息，看名字是 在 Hessian 矩阵中的 列 数 ， 并 且 建 立 了 blockPoseIndices 和 blockLandmarkIndices 两 个 数 组 ， 元 素 值 是 _colInHessian。 索引为 pose 或者 landmark 的顺序号，各自从 0 开始计数 2 BlockSolver::resize 创建了 一些空间， _Hpp， _Hschur 等等， 涉及到一些泛型数据类型，
    struct BlockSolverTraits { static const int PoseDim = _PoseDim; static const int LandmarkDim = _LandmarkDim; typedef Matrix PoseMatrixType; typedef Matrix LandmarkMatrixType; typedef Matrix PoseLandmarkMatrixType; typedef Matrix PoseVectorType; typedef Matrix LandmarkVectorType; typedef SparseBlockMatrix PoseHessianType; typedef SparseBlockMatrix LandmarkHessianType; typedef SparseBlockMatrix PoseLandmarkHessianType; typedef LinearSolver LinearSolverType; }; _Hpp=new PoseHessianType _Hpl=new PoseLandmarkHessianType _Hll=new LandmarkHessianType _Hschur=new PoseHessianType _DInvSchur = new SparseBlockMatrixDiagonal _HplCCS = new SparseBlockMatrixCCS _HschurTransposedCCS = new SparseBlockMatrixCCS Hschur 对应 第一个方程左边的系数矩阵 DinvSchur 对应 Hll 的逆矩阵， SparseBlockMatrixDiagonal 类型 HplCCS 对应 Hpl HschurTransposedCCS 应该是 Hschur 的逆矩阵 3 分配 Hpp 和 Hll 中 对角线上的块， 以及 进行 内存映射，
    _Hpp->block(), blcok 函数负责 new 出一个 矩阵， mapHessianMemory 负责内存映射， 具体实现如下： new (&_hessian) HessianBlockType(d, D, D); 这种语法的意思是： 在 &_hessian 这个地址指向的内存构建 一个 HessianBlockType 对象 所谓 placement new 调用构造函数 比如 v->mapHessianMemory(m->data()); 就是把刚 new 出的矩阵 m 映射到顶点 v 内部的某个变量。 HessianBlockType 是 Eigen 库中的 Map， 也是专门为了内存复用设计的数据结构 按头文件所说： SparseBlockMatrix 的数据存储机制， 通常情况下，SparseBlockMatrix 利用矩阵块来实现稀疏矩 阵。静态 SparseBlockMatrix ，每个矩阵块维度是一致的。 模板参数是固定的矩阵块的维度。 实际上是 对某个块 首次调用 block 函数时 分配了内存，并且记录了内存地址。 SparseBlockMatrix::SparseMatrixBlock* SparseBlockMatrix::block 中， _blockCols[c].insert(std::make_pair(r, _block)) 记录了 r 行 c 列处的 内存地址 _block void BaseVertex::mapHessianMemory(double* d) { } new (&_hessian) HessianBlockType(d, D, D); mapHessianMemory 这 个 函 数 实 际 上 是 为 了 复 用 前 面 分 配 的 内 存 块 ， _hessian 是 HessianBlockType 类型，也就是 eigen 的 Map 类型， 参考 https://blog.csdn.net/caomin1hao/article/details/81359322 也就是 复用 d 指向的内存， d 就是 前面 SparseMatrixBlock::block 分配的内存 使用 block 分配了保存矩阵块的内存后， 用 mapHessianMemory 做内存映射工作 4 建立 schurMatrixLookup，暂时没看这个结构 TODO 5 假设 landmark 的索引在 pose 之后， 利用所有的边 建立 Hpp, Hll and in Hpl 的结构！ 是一个三重循环， 最外层 遍历 所有边，第二层 遍历边的所有顶点， 第三层是第二层的顶点之后 的顶点。 LocalBA 这里的情况， 应该只有两个顶点。那么第二层和第三层循环都只进行一次！ 简单地 说： 依然是 利用上面的 block 函数 分配 Hpp, Hll and in Hpl 中矩阵块的内存，注意， 对角线上的块 在 第 3 步中已经分配好！ 而且， 只 分配 整个大矩阵 上三角 区域的内存，因为整个大矩阵 是对称的 注意： 这里面有 schurMatrixLookup->addBlock 的动作，参考第 8 步。
    使用 block 分配了保存矩阵块的内存后， 用 mapHessianMemory 做内存映射工作。 6 _DInvSchur->diagonal().resize(landmarkIdx); _DInvSchur->diagonal()返回的是 _diagonal 的引用， _diagonal 是 Eigen 矩阵块的 vector 7 _Hpl->fillSparseBlockMatrixCCS(*_HplCCS); 看字面意思， 填充_HplCCS 大概过程就是 把 _Hpl 的块结构转为 HplCCS 的块结构，并且仍 指向 之前 的 block 地址！也就是 内存复用！ _Hpl 和 _HplCCS 的矩阵块用的同样的内存区域。 8 接下来一个大循环，遍历所有顶点 v， 如果是对应 Landmark 的顶点，那么 取出连接这个顶点的 所有边 vedges，遍历所有边 it1， 遍历这条边的顶点 v1， 如果 v1 不是 v，且 v1 不是固定顶点， 遍历 vedges 的所有边 it2, 遍历 it2 的所有顶点 v2, 如果 v2 不是固定顶点且不等于 v, 取出 v1 和 v2 的 hessianIndex， i1 和 i2， 调用 schurMatrixLookup->addBlock(i1, i2); 注意： schurMatrixLookup->addBlock(i1, i2) 里面会分配矩阵块对应的内存！ 结合前面第 5 步。 这里实际上是已经知道 Schur 矩阵的结构了，也就是知道 Schur 矩阵里面哪些块 为 0，哪些不为 0.这里在为不为 0 的块分配矩阵。前面的 v1 和 v2 必然是指向位姿的， 实际上 是 一种共视关系的表现。参考高翔书 254 页。 9 _Hschur->takePatternFromHash(*schurMatrixLookup); 把 schurMatrixLookup 里的结构 输出到 Hschur 中， 里面有个排序， TODO: 这个排序后面再看下！ 释放 schurMatrixLookup 这个结构的内存， schurMatrixLookup->addBlock 分配的矩阵块应该没有 释放！ 里面有这样的代码： HashSparseColumn aux; swap(aux, column); 说是提前释放一些内存， HashSparseColumn 是一种 vector 类型，即，std::vector， RowBlock 定义如下： struct RowBlock { }; int row; ///< row of the block MatrixType* block; ///< matrix pointer for the block RowBlock() : row(-1), block(0) {} RowBlock(int r, MatrixType* b) : row(r), block(b) {} bool operator<(const RowBlock& other) const { return row < other.row; } std::vector 被释放的时候，里面 block 指向的内存应该没有被释放！这些内存由 delete _Hschur; 的时候释放！ 10 _Hschur->fillSparseBlockMatrixCCSTransposed(*_HschurTransposedCCS);
    构建 HschurTransposedCCS 的结构，和 第 7 步相似，但是多了个转置！ 稀疏矩阵两种表示 SparseBlockMatrixCCS const std::vector& _rowBlockIndices; ///< vector of the indices of the blocks along the rows. const std::vector& _colBlockIndices; ///< vector of the indices of the blocks along the cols 上面出现的索引是矩阵中的绝对索引，也就是 完整视图下的索引，而不是 基于块的索引 std::vector _blockCols; ///< the matrices stored in CCS order struct RowBlock { }; int row; ///< row of the block MatrixType* block; ///< matrix pointer for the block RowBlock() : row(-1), block(0) {} RowBlock(int r, MatrixType* b) : row(r), block(b) {} bool operator<(const RowBlock& other) const { return row < other.row; } typedef std::vector SparseColumn; 上面出现的 row 是基于 块的索引！ //! how many rows does the block at block-row r has? inline int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; } //! how many cols does the block at block-col c has? inline int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; } 这两个函数很关键，表明了 矩阵块实际维度的存储方式。 r 和 c 是 基于块的索引， 即 块视 图 下 第 r 行和 第 c 列的块。 //! where does the row at block-row r starts? inline int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; } //! where does the col at block-col r starts? inline int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; } 矩阵块的起始行列获取方法 ------------------------------------------------------------------------------------ SparseBlockMatrix std::vector _rowBlockIndices; ///< vector of the indices of the blocks along the rows.
    std::vector _colBlockIndices; ///< vector of the indices of the blocks along the cols //! array of maps of blocks. The index of the array represent a block column of the matrix //! and the block column is stored as a map row_block -> matrix_block_ptr. std::vector _blockCols; typedef std::map IntBlockMap; 按照代码中 SparseBlockMatrix 的存储方式设计，在列视图下， 不会出现某一列全为 0 的情况! SparseOptimizer:: computeActiveErrors() 遍历 _activeEdges，调用 e->computeError(); 然后检查有没有 nan 情况出现 _optimizer->activeRobustChi2(); 获取每条边上的误差， 得到误差总和 bool BlockSolver:: buildSystem() 计算每条边的 Jacobian 矩阵，然后计算 H 矩阵， 计算出 b 矩阵 1 对每条边调用 e->linearizeOplus(jacobianWorkspace); 执行 void BaseBinaryEdge::linearizeOplus new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D, Di); new (&_jacobianOplusXj) JacobianXjOplusType(jacobianWorkspace.workspaceForVertex(1), D, Dj); linearizeOplus(); 内部实现如上， 分配了两个内存，分配到 jacobianOplusXi 和 jacobianOplusXj 随后的 linearizeOplus()会把计算出的 Jacobian 写入到这两个地方。 2 接着对每条边调用 e->constructQuadraticForm()，得到 Hessian 矩阵 这里的乘法计算很明显。JT*omega*J , 得到 Hessian 矩阵， 关键的地方在于， Hessian 矩阵 对角线上的块是保持在顶点中的_hessian 上面， 而这个正好是前 面分配的， Hessian 矩阵 上三角的块 是保留在 边中的 _hessian 上面，这个的分配 是在 buildStructure 中进行的，并且使用 mapHessianMemory 进行映射！顶点的_hessian 映射到 Hpp, Hll 中， 边中的 _hessian 映射到 Hpl 中。 并且，计算了 b 矩阵， 保存 在 顶点的 _b 中 3 所有边的上述两个函数调用完以后，把所有顶点中的 _b 拷贝到 BlockSolver 的 _b 中
    分享到：
    赞 收藏

    相关推荐

    资料库

    后端

    共收录2142份资料，累计10个分类，关注成员有19位，主要包括：Perl,Docker,Delphi,VB,PHP,Java,C,C#,Python,C++

    热门标签

    Perl Docker Delphi VB PHP Java C C# Python C++

    最新资料