写在前面
最近学习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;
};
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:
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;
for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it){
OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*) *it;
const OptimizableGraph::EdgeSet& vEdges=v->edges();
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);
# 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()
{
# 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();
}
# ifndef G2O_OPENMP
JacobianWorkspace& jacobianWorkspace = _optimizer->jacobianWorkspace();
# else
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);
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
}
# 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(){
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;
}
number_t t=get_monotonic_time();
_Hschur->clear();
_Hpp->add(*_Hschur);
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");
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;
assert(targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end() && targetColumnIt->row == i2 &&
"invalid iterator, something wrong with the matrix structure");
PoseMatrixType* Hi1i2 = targetColumnIt->block;
assert(Hi1i2);
(*Hi1i2).noalias() -= BDinv*Bj->transpose();
}
}
}
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;
}
if (! solvedPoses)
return false;
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;
for (
int i=
0; i<_sizePoses; ++i)
cp[i]=-xp[i];
memcpy(cl,bl,_sizeLandmarks*
sizeof(number_t));
_HplCCS->rightMultiply(cl, cp);
memset(xl,
0, _sizeLandmarks*
sizeof(number_t));
_DInvSchur->multiply(xl,cl);
return true;
}
最后就是把得到的
Δx
Δ
x
对应的调用顶点中的oplusImpl函数对状态变量进行更新，这部分就不细讲啦～
总结
以上就是整个g2o在私底下为我们做的事情了，当然其中很多细节这里没有展现出来，本来看这部分源码的意图也是为了验证过程是否和自己想象的一样，最后的感觉是从数学上来讲大致上是相同的，但是要是从程序上觉得还是受益匪浅，作者的很多编程方式和方法还是很值得学习的。这部分内容就先更新到这里，如果以后有更新的发现也会及时的记录下来，一方面是帮助自己总结，一方面希望能帮助刚入门的同学。
以上观点仅仅代表个人学习时候的观点，如有不对的地方还请各位大神指正！这里不胜感激！
