写在前面
最近学习g2o的程序，跟着例程做了几个程序，其实其中大多数要注意的就是顶点和边的一些东西，本次博客旨在记录那些不被看到的过程，也就是g2o帮助我们做了哪些东西，主要参考的就是如下网站：
http://docs.ros.org/fuerte/api/re_vision/html/namespaceg2o.html
这个网站里面有较为全面的g2o的类以及函数的讲解，很方便。
那么这里也就是一个比较浅显的总结，并没有很深入，大致上就是记录g2o在解决问题的内部过程。
块求解器（BlockSolver_P_L）
这个地方我在之前的博客中提过一次，这里再提一下，首先祭出他的源码：
template<int p, int l>
using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;接着往上走，我们会看到比较详细的BlockSoverTraits类
template <int _PoseDim, int _LandmarkDim>
struct BlockSolverTraits
{
static const int PoseDim = _PoseDim;
static const int LandmarkDim = _LandmarkDim;
typedef Eigen::Matrix<number_t, PoseDim, PoseDim, Eigen::ColMajor> PoseMatrixType;
typedef Eigen::Matrix<number_t, LandmarkDim, LandmarkDim, Eigen::ColMajor> LandmarkMatrixType;
typedef Eigen::Matrix<number_t, PoseDim, LandmarkDim, Eigen::ColMajor> PoseLandmarkMatrixType;
typedef Eigen::Matrix<number_t, PoseDim, 1, Eigen::ColMajor> PoseVectorType;
typedef Eigen::Matrix<number_t, LandmarkDim, 1, Eigen::ColMajor> LandmarkVectorType;
typedef SparseBlockMatrix<PoseMatrixType> PoseHessianType;
typedef SparseBlockMatrix<LandmarkMatrixType> LandmarkHessianType;
typedef SparseBlockMatrix<PoseLandmarkMatrixType> PoseLandmarkHessianType;
typedef LinearSolver<PoseMatrixType> LinearSolverType;
};
/**
* \brief traits to summarize the properties of the dynamic size optimization problem
*/
template <>
struct BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic>
{
static const int PoseDim = Eigen::Dynamic;
static const int LandmarkDim = Eigen::Dynamic;
typedef MatrixX PoseMatrixType;
typedef MatrixX LandmarkMatrixType;
typedef MatrixX PoseLandmarkMatrixType;
typedef VectorX PoseVectorType;
typedef VectorX LandmarkVectorType;
typedef SparseBlockMatrix<PoseMatrixType> PoseHessianType;
typedef SparseBlockMatrix<LandmarkMatrixType> LandmarkHessianType;
typedef SparseBlockMatrix<PoseLandmarkMatrixType> PoseLandmarkHessianType;
typedef LinearSolver<PoseMatrixType> LinearSolverType;
};接着是比较详细的BlockSolver类
template <typename Traits>
class BlockSolver: public BlockSolverBase
{
public:
static const int PoseDim = Traits::PoseDim;
static const int LandmarkDim = Traits::LandmarkDim;
typedef typename Traits::PoseMatrixType PoseMatrixType;
typedef typename Traits::LandmarkMatrixType LandmarkMatrixType;
typedef typename Traits::PoseLandmarkMatrixType PoseLandmarkMatrixType;
typedef typename Traits::PoseVectorType PoseVectorType;
typedef typename Traits::LandmarkVectorType LandmarkVectorType;
typedef typename Traits::PoseHessianType PoseHessianType;
typedef typename Traits::LandmarkHessianType LandmarkHessianType;
typedef typename Traits::PoseLandmarkHessianType PoseLandmarkHessianType;
typedef typename Traits::LinearSolverType LinearSolverType;
public:
/**
* allocate a block solver ontop of the underlying linear solver.
* NOTE: The BlockSolver assumes exclusive access to the linear solver and will therefore free the pointer
* in its destructor.
*/
BlockSolver(std::unique_ptr<LinearSolverType> linearSolver);
~BlockSolver();
virtual bool init(SparseOptimizer* optmizer, bool online = false);
virtual bool buildStructure(bool zeroBlocks = false);
virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges);
virtual bool buildSystem();
virtual bool solve();
virtual bool computeMarginals(SparseBlockMatrix<MatrixX>& spinv, const std::vector<std::pair<int, int> >& blockIndices);
virtual bool setLambda(number_t lambda, bool backup = false);
virtual void restoreDiagonal();
virtual bool supportsSchur() {return true;}
virtual bool schur() { return _doSchur;}
virtual void setSchur(bool s) { _doSchur = s;}
LinearSolver<PoseMatrixType>& linearSolver() const { return *_linearSolver;}
virtual void setWriteDebug(bool writeDebug);
virtual bool writeDebug() const {return _linearSolver->writeDebug();}
virtual bool saveHessian(const std::string& fileName) const;
virtual void multiplyHessian(number_t* dest, const number_t* src) const { _Hpp->multiplySymmetricUpperTriangle(dest, src);}
protected:
void resize(int* blockPoseIndices, int numPoseBlocks,
int* blockLandmarkIndices, int numLandmarkBlocks, int totalDim);
void deallocate();
std::unique_ptr<SparseBlockMatrix<PoseMatrixType>> _Hpp;
std::unique_ptr<SparseBlockMatrix<LandmarkMatrixType>> _Hll;
std::unique_ptr<SparseBlockMatrix<PoseLandmarkMatrixType>> _Hpl;
std::unique_ptr<SparseBlockMatrix<PoseMatrixType>> _Hschur;
std::unique_ptr<SparseBlockMatrixDiagonal<LandmarkMatrixType>> _DInvSchur;
std::unique_ptr<SparseBlockMatrixCCS<PoseLandmarkMatrixType>> _HplCCS;
std::unique_ptr<SparseBlockMatrixCCS<PoseMatrixType>> _HschurTransposedCCS;
std::unique_ptr<LinearSolverType> _linearSolver;
std::vector<PoseVectorType, Eigen::aligned_allocator<PoseVectorType> > _diagonalBackupPose;
std::vector<LandmarkVectorType, Eigen::aligned_allocator<LandmarkVectorType> > _diagonalBackupLandmark;
# ifdef G2O_OPENMP
std::vector<OpenMPMutex> _coefficientsMutex;
# endif
bool _doSchur;
std::unique_ptr<number_t[], aligned_deleter<number_t>> _coefficients;
std::unique_ptr<number_t[], aligned_deleter<number_t>> _bschur;
int _numPoses, _numLandmarks;
int _sizePoses, _sizeLandmarks;
};那么有了以上的信息，整个BlockSolver_P_L的作用也就比较清楚了，还是那句老话：P表示的是Pose的维度（注意一定是流形manifold下的最小表示），L表示Landmark的维度（这里就不涉及流行什么事儿了）。这里思考一个问题，假如说在某个应用下，我们的P和L在程序开始并不能确定（例如程序中既有映射关系的边，同时还有位姿图之间的边，这时候的 Hpp H p p 矩阵将不是那么的稀疏），那么此时这个块状求解器如何定义呢？其实也比较简单，g2o已经帮我们定义了一个不定的BlockSolverTraits，所有的参数都在中间过程中被确定，那么为什么g2o还帮助我们定义了一些常用的类型呢？一种可能是出于节约初始化时间的角度出发的，但是个人估计不是很靠谱，毕竟C++申请出一块内存应该还是很快的，没有必要为了这点儿时间纠结；那么另一种也许就是作者想把常用的类型定义出来而已吧～
优化器（optimizer）最初做的事（initializeOptimization）
想必这句话大家应该也经常用到
optimizer.initializeOptimization();别看他若不经风的，其实在整个程序中的用处还真的很大（这不是废话嘛，哪个函数的用处都很大好吗），下面就仔细的说一下这个函数里面都干了什么：
1. 把所有的顶点（Vertex）插入到vset的集合中（妈妈在也不用担心我插入了相同的定点）
2. 遍历vset集合，取出每个顶点的边（这里每个边都有一个level的概念，默认情况下，g2o只处理level=0的边，在orbslam中，如果确定某个边的重投影误差过大，则把level设置为1，也就是舍弃这个边对于整个优化的影响），并判断边所连接的顶点是否都是有效的（在vset中），如果是，则认为这是一个有效的边和顶点，并分别加入到_activeEdges和_activeVertices中（妈妈在也不用担心边少顶点或者图中没有边的顶点了）
3. 对上述的_activeEdges和_activeVertices按照ID号进行排序，其中Vertex的ID号是自己设置的，而Edge的ID号是g2o内部有个变量进行赋值的
4. 对于上述的_activeVertices，剔除掉固定点（fixed）之后，把所有的顶点按照**不被**margin在前，被margin在后的顺序排成vector类型，变量为_ivMap，这个变量很重要，基本上后面的所有程序都是用这个变量进行遍历的
以上就是初始化过程的全部，具体的代码如下：
bool SparseOptimizer::initializeOptimization(HyperGraph::VertexSet& vset, int level){
if (edges().size() == 0) {
cerr << __PRETTY_FUNCTION__ << ": Attempt to initialize an empty graph" << endl;
return false;
}
preIteration(-1);
bool workspaceAllocated = _jacobianWorkspace.allocate(); (void) workspaceAllocated;
assert(workspaceAllocated && "Error while allocating memory for the Jacobians");
clearIndexMapping();
_activeVertices.clear();
_activeVertices.reserve(vset.size());
_activeEdges.clear();
set<Edge*> auxEdgeSet; // temporary structure to avoid duplicates
for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it){
OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*) *it;
const OptimizableGraph::EdgeSet& vEdges=v->edges();
// count if there are edges in that level. If not remove from the pool
int levelEdges=0;
for (OptimizableGraph::EdgeSet::const_iterator it=vEdges.begin(); it!=vEdges.end(); ++it){
OptimizableGraph::Edge* e=reinterpret_cast<OptimizableGraph::Edge*>(*it);
if (level < 0 || e->level() == level) {
bool allVerticesOK = true;
for (vector<HyperGraph::Vertex*>::const_iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) {
if (vset.find(*vit) == vset.end()) {
allVerticesOK = false;
break;
}
}
if (allVerticesOK && !e->allVerticesFixed()) {
auxEdgeSet.insert(e);
levelEdges++;
}
}
}
if (levelEdges){
_activeVertices.push_back(v);
// test for NANs in the current estimate if we are debugging
# ifndef NDEBUG
int estimateDim = v->estimateDimension();
if (estimateDim > 0) {
VectorX estimateData(estimateDim);
if (v->getEstimateData(estimateData.data()) == true) {
int k;
bool hasNan = arrayHasNaN(estimateData.data(), estimateDim, &k);
if (hasNan)
cerr << __PRETTY_FUNCTION__ << ": Vertex " << v->id() << " contains a nan entry at index " << k << endl;
}
}
# endif
}
}
_activeEdges.reserve(auxEdgeSet.size());
for (set<Edge*>::iterator it = auxEdgeSet.begin(); it != auxEdgeSet.end(); ++it)
_activeEdges.push_back(*it);
sortVectorContainers();
bool indexMappingStatus = buildIndexMapping(_activeVertices);
postIteration(-1);
return indexMappingStatus;
}优化器（Optimizer）的优化（optimize）
过了初始化之后，我们基本上就可以调用optimize函数进行图优化了，如果只看optimize函数的代码，十分简单，首先判断_ivMap是否为空，之后直接调用_algorithm的solve函数，最后打印一些信息，之后循环一定的次数，此时就大功告成。所以下面的内容主要是围绕着_algorithm的solve函数进行。
算法（algorithm）的solve函数
- 在第一次进行迭代的时候，g2o要对整个矩阵块进行初始化，主要依靠块求解器的buildStructure()函数进行，那么想必大家也知道了，既然在块求解器中，那么必然就是对要用的矩阵块进行初始化，确实如此，我们稍微对这个函数进行展开：
（1） 对_ivMap进行遍历，如果顶点的margin标志为false，则认为是Pose，否则，认为是Landmark，所以在使用g2o的时候千万要注意，不要想当然的认为g2o会帮助我们区分Pose和Landmark
（2） 构建期待已久的 Hpp，Hll H p p ， H l l 等矩阵，每个矩阵的类型都是SparseBlockMatrix类型的
（3） 开始对应环节，这个过程首先是遍历_ivMap，将 Hpp和Hll H p p 和 H l l 中对应于该顶点的矩阵块映射到顶点内部的_hessian矩阵中，这步的作用在我的上个博客中已经讲过了，感兴趣的可以看g2o学习——再看顶点和边，之后开始遍历所有的边，取出边的两个顶点，如果两个顶点都不margin的话，则在把 Hpp H p p 中的两个顶点相交的地方的内存映射给边的内部_hessian矩阵；如果两个中一个margin一个不margin的话，则把 Hpl H p l 中相交的地方的内存映射给边的_hessian矩阵；如果两个都margin的话，则把 Hll H l l 中相交的地方的内存映射给边的_hessian矩阵
（4） 映射完成之后，如果我们使用schur消元的话，还要构建一个schur消元的中间矩阵，_Hschur，这个地方比较复杂，这里不做展开，对照公式看源码就可以
这部分的源码给出：
template <typename Traits>
bool BlockSolver<Traits>::buildStructure(bool zeroBlocks)
{
assert(_optimizer);
size_t sparseDim = 0;
_numPoses=0;
_numLandmarks=0;
_sizePoses=0;
_sizeLandmarks=0;
int* blockPoseIndices = new int[_optimizer->indexMapping().size()];
int* blockLandmarkIndices = new int[_optimizer->indexMapping().size()];
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
int dim = v->dimension();
if (! v->marginalized()){
v->setColInHessian(_sizePoses);
_sizePoses+=dim;
blockPoseIndices[_numPoses]=_sizePoses;
++_numPoses;
} else {
v->setColInHessian(_sizeLandmarks);
_sizeLandmarks+=dim;
blockLandmarkIndices[_numLandmarks]=_sizeLandmarks;
++_numLandmarks;
}
sparseDim += dim;
}
resize(blockPoseIndices, _numPoses, blockLandmarkIndices, _numLandmarks, sparseDim);
delete[] blockLandmarkIndices;
delete[] blockPoseIndices;
// allocate the diagonal on Hpp and Hll
int poseIdx = 0;
int landmarkIdx = 0;
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
if (! v->marginalized()){
//assert(poseIdx == v->hessianIndex());
PoseMatrixType* m = _Hpp->block(poseIdx, poseIdx, true);
if (zeroBlocks)
m->setZero();
v->mapHessianMemory(m->data());
++poseIdx;
} else {
LandmarkMatrixType* m = _Hll->block(landmarkIdx, landmarkIdx, true);
if (zeroBlocks)
m->setZero();
v->mapHessianMemory(m->data());
++landmarkIdx;
}
}
assert(poseIdx == _numPoses && landmarkIdx == _numLandmarks);
// temporary structures for building the pattern of the Schur complement
SparseBlockMatrixHashMap<PoseMatrixType>* schurMatrixLookup = 0;
if (_doSchur) {
schurMatrixLookup = new SparseBlockMatrixHashMap<PoseMatrixType>(_Hschur->rowBlockIndices(), _Hschur->colBlockIndices());
schurMatrixLookup->blockCols().resize(_Hschur->blockCols().size());
}
// here we assume that the landmark indices start after the pose ones
// create the structure in Hpp, Hll and in Hpl
for (SparseOptimizer::EdgeContainer::const_iterator it=_optimizer->activeEdges().begin(); it!=_optimizer->activeEdges().end(); ++it){
OptimizableGraph::Edge* e = *it;
for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);
int ind1 = v1->hessianIndex();
if (ind1 == -1)
continue;
int indexV1Bak = ind1;
for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {
OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);
int ind2 = v2->hessianIndex();
if (ind2 == -1)
continue;
ind1 = indexV1Bak;
bool transposedBlock = ind1 > ind2;
if (transposedBlock){ // make sure, we allocate the upper triangle block
std::swap(ind1, ind2);
}
if (! v1->marginalized() && !v2->marginalized()){
PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
if (zeroBlocks)
m->setZero();
e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
if (_Hschur) {// assume this is only needed in case we solve with the schur complement
schurMatrixLookup->addBlock(ind1, ind2);
}
} else if (v1->marginalized() && v2->marginalized()){
// RAINER hmm.... should we ever reach this here????
LandmarkMatrixType* m = _Hll->block(ind1-_numPoses, ind2-_numPoses, true);
if (zeroBlocks)
m->setZero();
e->mapHessianMemory(m->data(), viIdx, vjIdx, false);
} else {
if (v1->marginalized()){
PoseLandmarkMatrixType* m = _Hpl->block(v2->hessianIndex(),v1->hessianIndex()-_numPoses, true);
if (zeroBlocks)
m->setZero();
e->mapHessianMemory(m->data(), viIdx, vjIdx, true); // transpose the block before writing to it
} else {
PoseLandmarkMatrixType* m = _Hpl->block(v1->hessianIndex(),v2->hessianIndex()-_numPoses, true);
if (zeroBlocks)
m->setZero();
e->mapHessianMemory(m->data(), viIdx, vjIdx, false); // directly the block
}
}
}
}
}
if (! _doSchur) {
delete schurMatrixLookup;
return true;
}
_DInvSchur->diagonal().resize(landmarkIdx);
_Hpl->fillSparseBlockMatrixCCS(*_HplCCS);
for (OptimizableGraph::Vertex* v : _optimizer->indexMapping()) {
if (v->marginalized()){
const HyperGraph::EdgeSet& vedges=v->edges();
for (HyperGraph::EdgeSet::const_iterator it1=vedges.begin(); it1!=vedges.end(); ++it1){
for (size_t i=0; i<(*it1)->vertices().size(); ++i)
{
OptimizableGraph::Vertex* v1= (OptimizableGraph::Vertex*) (*it1)->vertex(i);
if (v1->hessianIndex()==-1 || v1==v)
continue;
for (HyperGraph::EdgeSet::const_iterator it2=vedges.begin(); it2!=vedges.end(); ++it2){
for (size_t j=0; j<(*it2)->vertices().size(); ++j)
{
OptimizableGraph::Vertex* v2= (OptimizableGraph::Vertex*) (*it2)->vertex(j);
if (v2->hessianIndex()==-1 || v2==v)
continue;
int i1=v1->hessianIndex();
int i2=v2->hessianIndex();
if (i1<=i2) {
schurMatrixLookup->addBlock(i1, i2);
}
}
}
}
}
}
}
_Hschur->takePatternFromHash(*schurMatrixLookup);
delete schurMatrixLookup;
_Hschur->fillSparseBlockMatrixCCSTransposed(*_HschurTransposedCCS);
return true;
}- 上述块矩阵以及映射关系对应好以后，接下来比较重要的函数就是solver的buildSystem函数，这个函数的主要功能就是将增量方程（
HΔx=−b
H
Δ
x
=
−
b
）中的H矩阵和b向量赋予该有的值，具体过程如下：
（1） 遍历所有的边，计算该边误差所产生的jacobian矩阵
（2）根据公式 H=JTWJ H = J T W J 计算每个顶点的_hessian矩阵， b=JTWδ b = J T W δ 计算b向量，如果该边是个二元边，且两个定点都没有被fix的时候，还会根据 H=JT1WJ2 H = J 1 T W J 2 计算相交部分的_hessian矩阵。
（3） 由于顶点内部类型b并不是映射，因此最后还要遍历所有的顶点的b并将其真值拷贝到增量方程的b内
这部分的代码如下：
template <typename Traits>
bool BlockSolver<Traits>::buildSystem()
{
// clear b vector
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)
# endif
for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i) {
OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i];
assert(v);
v->clearQuadraticForm();
}
_Hpp->clear();
if (_doSchur) {
_Hll->clear();
_Hpl->clear();
}
// resetting the terms for the pairwise constraints
// built up the current system by storing the Hessian blocks in the edges and vertices
# ifndef G2O_OPENMP
// no threading, we do not need to copy the workspace
JacobianWorkspace& jacobianWorkspace = _optimizer->jacobianWorkspace();
# else
// if running with threads need to produce copies of the workspace for each thread
JacobianWorkspace jacobianWorkspace = _optimizer->jacobianWorkspace();
# pragma omp parallel for default (shared) firstprivate(jacobianWorkspace) if (_optimizer->activeEdges().size() > 100)
# endif
for (int k = 0; k < static_cast<int>(_optimizer->activeEdges().size()); ++k) {
OptimizableGraph::Edge* e = _optimizer->activeEdges()[k];
e->linearizeOplus(jacobianWorkspace); // jacobian of the nodes' oplus (manifold)
e->constructQuadraticForm();
# ifndef NDEBUG
for (size_t i = 0; i < e->vertices().size(); ++i) {
const OptimizableGraph::Vertex* v = static_cast<const OptimizableGraph::Vertex*>(e->vertex(i));
if (! v->fixed()) {
bool hasANan = arrayHasNaN(jacobianWorkspace.workspaceForVertex(i), e->dimension() * v->dimension());
if (hasANan) {
std::cerr << "buildSystem(): NaN within Jacobian for edge " << e << " for vertex " << i << std::endl;
break;
}
}
}
# endif
}
// flush the current system in a sparse block matrix
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)
# endif
for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i) {
OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i];
int iBase = v->colInHessian();
if (v->marginalized())
iBase+=_sizePoses;
v->copyB(_b+iBase);
}
return 0;
}- 上述完成整个矩阵块的构建之后，接下来就是激动人心的求解线性方程的时候了，主要依靠块求解器BlockSolver的solve函数，下面是过程：
（1） 判断是否要做schur消元，如果不做的话，则认为整个图中没有要margin的点，因此直接求解 HppΔx=−b H p p Δ x = − b 就可以了；
（2） 如果要进行schur消元，则在后面构建出schur需要的矩阵和向量，由于这部分内容比较大，这里就不做展开，感兴趣的可以看SLAM中的marginalization 和 Schur complement，里面讲的也比较清楚
该部分源码如下：
template <typename Traits>
bool BlockSolver<Traits>::solve(){
//cerr << __PRETTY_FUNCTION__ << endl;
if (! _doSchur){
number_t t=get_monotonic_time();
bool ok = _linearSolver->solve(*_Hpp, _x, _b);
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats) {
globalStats->timeLinearSolver = get_monotonic_time() - t;
globalStats->hessianDimension = globalStats->hessianPoseDimension = _Hpp->cols();
}
return ok;
}
// schur thing
// backup the coefficient matrix
number_t t=get_monotonic_time();
// _Hschur = _Hpp, but keeping the pattern of _Hschur
_Hschur->clear();
_Hpp->add(*_Hschur);
//_DInvSchur->clear();
memset(_coefficients.get(), 0, _sizePoses*sizeof(number_t));
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) schedule(dynamic, 10)
# endif
for (int landmarkIndex = 0; landmarkIndex < static_cast<int>(_Hll->blockCols().size()); ++landmarkIndex) {
const typename SparseBlockMatrix<LandmarkMatrixType>::IntBlockMap& marginalizeColumn = _Hll->blockCols()[landmarkIndex];
assert(marginalizeColumn.size() == 1 && "more than one block in _Hll column");
// calculate inverse block for the landmark
const LandmarkMatrixType * D = marginalizeColumn.begin()->second;
assert (D && D->rows()==D->cols() && "Error in landmark matrix");
LandmarkMatrixType& Dinv = _DInvSchur->diagonal()[landmarkIndex];
Dinv = D->inverse();
LandmarkVectorType db(D->rows());
for (int j=0; j<D->rows(); ++j) {
db[j]=_b[_Hll->rowBaseOfBlock(landmarkIndex) + _sizePoses + j];
}
db=Dinv*db;
assert((size_t)landmarkIndex < _HplCCS->blockCols().size() && "Index out of bounds");
const typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn& landmarkColumn = _HplCCS->blockCols()[landmarkIndex];
for (typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn::const_iterator it_outer = landmarkColumn.begin();
it_outer != landmarkColumn.end(); ++it_outer) {
int i1 = it_outer->row;
const PoseLandmarkMatrixType* Bi = it_outer->block;
assert(Bi);
PoseLandmarkMatrixType BDinv = (*Bi)*(Dinv);
assert(_HplCCS->rowBaseOfBlock(i1) < _sizePoses && "Index out of bounds");
typename PoseVectorType::MapType Bb(&_coefficients[_HplCCS->rowBaseOfBlock(i1)], Bi->rows());
# ifdef G2O_OPENMP
ScopedOpenMPMutex mutexLock(&_coefficientsMutex[i1]);
# endif
Bb.noalias() += (*Bi)*db;
assert(i1 >= 0 && i1 < static_cast<int>(_HschurTransposedCCS->blockCols().size()) && "Index out of bounds");
typename SparseBlockMatrixCCS<PoseMatrixType>::SparseColumn::iterator targetColumnIt = _HschurTransposedCCS->blockCols()[i1].begin();
typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::RowBlock aux(i1, 0);
typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn::const_iterator it_inner = lower_bound(landmarkColumn.begin(), landmarkColumn.end(), aux);
for (; it_inner != landmarkColumn.end(); ++it_inner) {
int i2 = it_inner->row;
const PoseLandmarkMatrixType* Bj = it_inner->block;
assert(Bj);
while (targetColumnIt->row < i2 /*&& targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end()*/)
++targetColumnIt;
assert(targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end() && targetColumnIt->row == i2 && "invalid iterator, something wrong with the matrix structure");
PoseMatrixType* Hi1i2 = targetColumnIt->block;//_Hschur->block(i1,i2);
assert(Hi1i2);
(*Hi1i2).noalias() -= BDinv*Bj->transpose();
}
}
}
//cerr << "Solve [marginalize] = " << get_monotonic_time()-t << endl;
// _bschur = _b for calling solver, and not touching _b
memcpy(_bschur.get(), _b, _sizePoses * sizeof(number_t));
for (int i=0; i<_sizePoses; ++i){
_bschur[i]-=_coefficients[i];
}
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats){
globalStats->timeSchurComplement = get_monotonic_time() - t;
}
t=get_monotonic_time();
bool solvedPoses = _linearSolver->solve(*_Hschur, _x, _bschur.get());
if (globalStats) {
globalStats->timeLinearSolver = get_monotonic_time() - t;
globalStats->hessianPoseDimension = _Hpp->cols();
globalStats->hessianLandmarkDimension = _Hll->cols();
globalStats->hessianDimension = globalStats->hessianPoseDimension + globalStats->hessianLandmarkDimension;
}
//cerr << "Solve [decompose and solve] = " << get_monotonic_time()-t << endl;
if (! solvedPoses)
return false;
// _x contains the solution for the poses, now applying it to the landmarks to get the new part of the
// solution;
number_t* xp = _x;
number_t* cp = _coefficients.get();
number_t* xl=_x+_sizePoses;
number_t* cl=_coefficients.get() + _sizePoses;
number_t* bl=_b+_sizePoses;
// cp = -xp
for (int i=0; i<_sizePoses; ++i)
cp[i]=-xp[i];
// cl = bl
memcpy(cl,bl,_sizeLandmarks*sizeof(number_t));
// cl = bl - Bt * xp
//Bt->multiply(cl, cp);
_HplCCS->rightMultiply(cl, cp);
// xl = Dinv * cl
memset(xl,0, _sizeLandmarks*sizeof(number_t));
_DInvSchur->multiply(xl,cl);
//_DInvSchur->rightMultiply(xl,cl);
//cerr << "Solve [landmark delta] = " << get_monotonic_time()-t << endl;
return true;
}- 最后就是把得到的 Δx Δ x 对应的调用顶点中的oplusImpl函数对状态变量进行更新，这部分就不细讲啦～
总结
以上就是整个g2o在私底下为我们做的事情了，当然其中很多细节这里没有展现出来，本来看这部分源码的意图也是为了验证过程是否和自己想象的一样，最后的感觉是从数学上来讲大致上是相同的，但是要是从程序上觉得还是受益匪浅，作者的很多编程方式和方法还是很值得学习的。这部分内容就先更新到这里，如果以后有更新的发现也会及时的记录下来，一方面是帮助自己总结，一方面希望能帮助刚入门的同学。
以上观点仅仅代表个人学习时候的观点，如有不对的地方还请各位大神指正！这里不胜感激！
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
