#include <graph_optimizer_sparse_incremental.h>

Inheritance diagram for g2o::SparseOptimizerIncremental:

[legend]

Collaboration diagram for g2o::SparseOptimizerIncremental:

[legend]

Public Member Functions
	SparseOptimizerIncremental ()

	~SparseOptimizerIncremental ()

int	optimize (int iterations, bool online=false)

virtual bool	updateInitialization (HyperGraph::VertexSet &vset, HyperGraph::EdgeSet &eset)

virtual bool	initSolver (int dimension, int batchEveryN)

Public Member Functions inherited from g2o::SparseOptimizerOnline
	SparseOptimizerOnline (bool pcg=false)

virtual	~SparseOptimizerOnline ()

int	optimize (int iterations, bool online=false)

void	update (double *update)

virtual void	gnuplotVisualization ()

Public Member Functions inherited from g2o::SparseOptimizer
	SparseOptimizer ()

virtual	~SparseOptimizer ()

virtual bool	initializeOptimization (HyperGraph::EdgeSet &eset)

virtual bool	initializeOptimization (HyperGraph::VertexSet &vset, int level=0)

virtual bool	initializeOptimization (int level=0)

virtual void	computeInitialGuess ()

virtual void	computeInitialGuess (EstimatePropagatorCost &propagator)

virtual void	setToOrigin ()

bool	computeMarginals (SparseBlockMatrix< MatrixX > &spinv, const std::vector< std::pair< int, int > > &blockIndices)

bool	computeMarginals (SparseBlockMatrix< MatrixX > &spinv, const Vertex *vertex)

bool	computeMarginals (SparseBlockMatrix< MatrixX > &spinv, const VertexContainer &vertices)

virtual Vertex *	findGauge ()
	finds a gauge in the graph to remove the undefined dof.

bool	gaugeFreedom ()

double	activeChi2 () const

double	activeRobustChi2 () const

bool	verbose () const
	verbose information during optimization

void	setVerbose (bool verbose)

void	setForceStopFlag (bool *flag)

bool *	forceStopFlag () const

bool	terminate ()
	if external stop flag is given, return its state. False otherwise

const VertexContainer &	indexMapping () const
	the index mapping of the vertices

const VertexContainer &	activeVertices () const
	the vertices active in the current optimization

const EdgeContainer &	activeEdges () const
	the edges active in the current optimization

virtual bool	removeVertex (HyperGraph::Vertex *v, bool detach=false)

VertexContainer::const_iterator	findActiveVertex (const OptimizableGraph::Vertex *v) const

EdgeContainer::const_iterator	findActiveEdge (const OptimizableGraph::Edge *e) const

const OptimizationAlgorithm *	algorithm () const
	the solver used by the optimizer

OptimizationAlgorithm *	solver ()

void	setAlgorithm (OptimizationAlgorithm *algorithm)

void	push (SparseOptimizer::VertexContainer &vlist)
	push the estimate of a subset of the variables onto a stack

void	push (HyperGraph::VertexSet &vlist)
	push the estimate of a subset of the variables onto a stack

void	push ()
	push all the active vertices onto a stack

void	pop (SparseOptimizer::VertexContainer &vlist)
	pop (restore) the estimate a subset of the variables from the stack

void	pop (HyperGraph::VertexSet &vlist)
	pop (restore) the estimate a subset of the variables from the stack

void	pop ()
	pop (restore) the estimate of the active vertices from the stack

void	discardTop (SparseOptimizer::VertexContainer &vlist)

void	discardTop ()
	same as above, but for the active vertices

virtual void	clear ()

void	computeActiveErrors ()

	G2O_ATTRIBUTE_DEPRECATED (void linearizeSystem())

void	update (const double *update)

const BatchStatisticsContainer &	batchStatistics () const

BatchStatisticsContainer &	batchStatistics ()

void	setComputeBatchStatistics (bool computeBatchStatistics)

bool	computeBatchStatistics () const

bool	addComputeErrorAction (HyperGraphAction *action)
	add an action to be executed before the error vectors are computed

bool	removeComputeErrorAction (HyperGraphAction *action)

virtual void	discardTop ()

virtual void	discardTop (HyperGraph::VertexSet &vset)

Public Member Functions inherited from g2o::OptimizableGraph
Vertex *	vertex (int id)
	returns the vertex number id appropriately casted

const Vertex *	vertex (int id) const
	returns the vertex number id appropriately casted

	OptimizableGraph ()
	empty constructor

virtual	~OptimizableGraph ()

virtual bool	addVertex (HyperGraph::Vertex v, Data userData)

virtual bool	addVertex (HyperGraph::Vertex *v)

bool	addVertex (OptimizableGraph::Vertex v, Data userData)

bool	addVertex (OptimizableGraph::Vertex *v)

virtual bool	addEdge (HyperGraph::Edge *e)

bool	addEdge (OptimizableGraph::Edge *e)

virtual bool	setEdgeVertex (HyperGraph::Edge e, int pos, HyperGraph::Vertex v)

double	chi2 () const
	returns the chi2 of the current configuration

int	maxDimension () const
	return the maximum dimension of all vertices in the graph

void	recomputeJacobianWorkspaceSize ()

std::set< int >	dimensions () const

virtual void	preIteration (int)

virtual void	postIteration (int)

bool	addPreIterationAction (HyperGraphAction *action)
	add an action to be executed before each iteration

bool	addPostIterationAction (HyperGraphAction *action)
	add an action to be executed after each iteration

bool	removePreIterationAction (HyperGraphAction *action)
	remove an action that should no longer be executed before each iteration

bool	removePostIterationAction (HyperGraphAction *action)
	remove an action that should no longer be executed after each iteration

virtual bool	load (std::istream &is)

bool	load (const char *filename)

virtual bool	save (std::ostream &os, int level=0) const
	save the graph to a stream. Again uses the Factory system.

bool	save (const char *filename, int level=0) const
	function provided for convenience, see save() above

bool	saveSubset (std::ostream &os, HyperGraph::VertexSet &vset, int level=0)
	save a subgraph to a stream. Again uses the Factory system.

bool	saveSubset (std::ostream &os, HyperGraph::EdgeSet &eset)
	save a subgraph to a stream. Again uses the Factory system.

virtual void	setFixed (HyperGraph::VertexSet &vset, bool fixed)
	fixes/releases a set of vertices

void	setRenamedTypesFromString (const std::string &types)

bool	isSolverSuitable (const OptimizationAlgorithmProperty &solverProperty, const std::set< int > &vertDims=std::set< int >()) const

virtual void	clearParameters ()

bool	addParameter (Parameter *p)

Parameter *	parameter (int id)

bool	verifyInformationMatrices (bool verbose=false) const

bool	saveVertex (std::ostream &os, Vertex *v) const

bool	saveParameter (std::ostream &os, Parameter *v) const

bool	saveEdge (std::ostream &os, Edge *e) const

bool	saveUserData (std::ostream &os, HyperGraph::Data *v) const

JacobianWorkspace &	jacobianWorkspace ()
	the workspace for storing the Jacobians of the graph

const JacobianWorkspace &	jacobianWorkspace () const

ParameterContainer &	parameters ()

const ParameterContainer &	parameters () const

void	forEachVertex (std::function< void(OptimizableGraph::Vertex *)> fn)
	apply a unary function to all vertices

void	forEachVertex (HyperGraph::VertexSet &vset, std::function< void(OptimizableGraph::Vertex *)> fn)
	apply a unary function to the vertices in vset

Public Member Functions inherited from g2o::HyperGraph
	HyperGraph ()
	constructs an empty hyper graph

virtual	~HyperGraph ()
	destroys the hyper-graph and all the vertices of the graph

Vertex *	vertex (int id)

const Vertex *	vertex (int id) const

virtual bool	removeEdge (Edge *e)

const VertexIDMap &	vertices () const

VertexIDMap &	vertices ()

const EdgeSet &	edges () const

EdgeSet &	edges ()

virtual bool	mergeVertices (Vertex vBig, Vertex vSmall, bool erase)

virtual bool	detachVertex (Vertex *v)

virtual bool	changeId (Vertex *v, int newId)

Protected Member Functions
bool	computeCholeskyUpdate ()

void	convertTripletUpdateToSparse ()

Protected Member Functions inherited from g2o::SparseOptimizer
void	sortVectorContainers ()

bool	buildIndexMapping (SparseOptimizer::VertexContainer &vlist)

void	clearIndexMapping ()

Protected Member Functions inherited from g2o::OptimizableGraph
void	performActions (int iter, HyperGraphActionSet &actions)

Protected Attributes
SparseBlockMatrix< Eigen::MatrixXd >	_updateMat

cholmod_common	_cholmodCommon

cholmod::CholmodExt *	_cholmodSparse

cholmod_factor *	_cholmodFactor

cholmod_triplet *	_permutedUpdate

cholmod_factor *	_L

LinearSolverCholmodOnlineInterface *	_solverInterface

HyperGraph::VertexSet	_touchedVertices

Eigen::VectorXi	_perm

Eigen::VectorXi	_cmember

Eigen::VectorXi	_tripletWorkspace

cholmod::CholmodExt *	_permutedUpdateAsSparse

Protected Attributes inherited from g2o::SparseOptimizerOnline
FILE *	_gnuplot

bool	_usePcg

Solver *	_underlyingSolver

Protected Attributes inherited from g2o::SparseOptimizer
bool *	_forceStopFlag

bool	_verbose

VertexContainer	_ivMap

VertexContainer	_activeVertices
	sorted according to VertexIDCompare

EdgeContainer	_activeEdges
	sorted according to EdgeIDCompare

OptimizationAlgorithm *	_algorithm

BatchStatisticsContainer	_batchStatistics

bool	_computeBatchStatistics

Protected Attributes inherited from g2o::OptimizableGraph
std::map< std::string, std::string >	_renamedTypesLookup

long long	_nextEdgeId

std::vector< HyperGraphActionSet >	_graphActions

ParameterContainer	_parameters

JacobianWorkspace	_jacobianWorkspace

Protected Attributes inherited from g2o::HyperGraph
VertexIDMap	_vertices

EdgeSet	_edges

Additional Inherited Members
Public Types inherited from g2o::SparseOptimizer
enum	{ AT_COMPUTEACTIVERROR = OptimizableGraph::AT_NUM_ELEMENTS , AT_NUM_ELEMENTS }

Public Types inherited from g2o::OptimizableGraph
enum	ActionType { AT_PREITERATION , AT_POSTITERATION , AT_NUM_ELEMENTS }

typedef std::set< HyperGraphAction * >	HyperGraphActionSet

typedef std::vector< OptimizableGraph::Vertex * >	VertexContainer
	vector container for vertices

typedef std::vector< OptimizableGraph::Edge * >	EdgeContainer
	vector container for edges

Public Types inherited from g2o::HyperGraph
typedef std::bitset< HyperGraph::HGET_NUM_ELEMS >	GraphElemBitset

typedef std::set< Edge * >	EdgeSet

typedef std::set< Vertex * >	VertexSet

typedef std::unordered_map< int, Vertex * >	VertexIDMap

typedef std::vector< Vertex * >	VertexContainer

Static Public Member Functions inherited from g2o::OptimizableGraph
static bool	initMultiThreading ()

Public Attributes inherited from g2o::SparseOptimizerOnline
int	slamDimension

HyperGraph::EdgeSet *	newEdges

bool	batchStep

bool	vizWithGnuplot

Public Attributes inherited from g2o::OptimizableGraph
class G2O_CORE_API	Vertex

class G2O_CORE_API	Edge

Public Attributes inherited from g2o::HyperGraph
class G2O_CORE_API	Data

class G2O_CORE_API	DataContainer

class G2O_CORE_API	Vertex

class G2O_CORE_API	Edge

Static Public Attributes inherited from g2o::HyperGraph
static const int	UnassignedId = -1

static const int	InvalidId = -2

Detailed Description

Definition at line 31 of file graph_optimizer_sparse_incremental.h.

Constructor & Destructor Documentation

◆ SparseOptimizerIncremental()

g2o::SparseOptimizerIncremental::SparseOptimizerIncremental ( )

Definition at line 57 of file graph_optimizer_sparse_incremental.cpp.

                                                       {
  _cholmodSparse = new cholmod::CholmodExt();
  _cholmodFactor = 0;
  cholmod_start(&_cholmodCommon);
 
  // setup ordering strategy to not permute the matrix
  _cholmodCommon.nmethods = 1;
  _cholmodCommon.method[0].ordering = CHOLMOD_NATURAL;
  _cholmodCommon.postorder = 0;
  _cholmodCommon.supernodal = CHOLMOD_SIMPLICIAL;
 
  _permutedUpdate = cholmod_allocate_triplet(1000, 1000, 1024, 0, CHOLMOD_REAL,
                                             &_cholmodCommon);
  _L = 0;
  _cholmodFactor = 0;
  _solverInterface = 0;
 
  _permutedUpdateAsSparse = new cholmod::CholmodExt;
}

References _cholmodCommon, _cholmodFactor, _cholmodSparse, _L, _permutedUpdate, _permutedUpdateAsSparse, and _solverInterface.

◆ ~SparseOptimizerIncremental()

g2o::SparseOptimizerIncremental::~SparseOptimizerIncremental ( )

Definition at line 77 of file graph_optimizer_sparse_incremental.cpp.

                                                        {
  delete _permutedUpdateAsSparse;
  _updateMat.clear(true);
  delete _cholmodSparse;
  if (_cholmodFactor) {
    cholmod_free_factor(&_cholmodFactor, &_cholmodCommon);
    _cholmodFactor = 0;
  }
  cholmod_free_triplet(&_permutedUpdate, &_cholmodCommon);
  cholmod_finish(&_cholmodCommon);
}

References _cholmodCommon, _cholmodFactor, _cholmodSparse, _permutedUpdate, _permutedUpdateAsSparse, _updateMat, and g2o::SparseBlockMatrix< MatrixType >::clear().

Member Function Documentation

◆ computeCholeskyUpdate()

bool g2o::SparseOptimizerIncremental::computeCholeskyUpdate ( )

protected

Definition at line 400 of file graph_optimizer_sparse_incremental.cpp.

                                                       {
  if (_cholmodFactor) {
    cholmod_free_factor(&_cholmodFactor, &_cholmodCommon);
    _cholmodFactor = 0;
  }
 
  const SparseBlockMatrix<MatrixXd>& A = _updateMat;
  size_t m = A.rows();
  size_t n = A.cols();
 
  if (_cholmodSparse->columnsAllocated < n) {
    // std::cerr << __PRETTY_FUNCTION__ << ": reallocating columns" <<
    // std::endl;
    _cholmodSparse->columnsAllocated =
        _cholmodSparse->columnsAllocated == 0
            ? n
            : 2 * n;  // pre-allocate more space if re-allocating
    delete[] static_cast<int*>(_cholmodSparse->p);
    _cholmodSparse->p = new int[_cholmodSparse->columnsAllocated + 1];
  }
  size_t nzmax = A.nonZeros();
  if (_cholmodSparse->nzmax < nzmax) {
    // std::cerr << __PRETTY_FUNCTION__ << ": reallocating row + values" <<
    // std::endl;
    _cholmodSparse->nzmax =
        _cholmodSparse->nzmax == 0
            ? nzmax
            : 2 * nzmax;  // pre-allocate more space if re-allocating
    delete[] static_cast<double*>(_cholmodSparse->x);
    delete[] static_cast<int*>(_cholmodSparse->i);
    _cholmodSparse->i = new int[_cholmodSparse->nzmax];
    _cholmodSparse->x = new double[_cholmodSparse->nzmax];
  }
  _cholmodSparse->ncol = n;
  _cholmodSparse->nrow = m;
 
  A.fillCCS(static_cast<int*>(_cholmodSparse->p),
            static_cast<int*>(_cholmodSparse->i),
            static_cast<double*>(_cholmodSparse->x), true);
  // writeCCSMatrix("updatesparse.txt", _cholmodSparse->nrow,
  // _cholmodSparse->ncol, (int*)_cholmodSparse->p, (int*)_cholmodSparse->i,
  // (double*)_cholmodSparse->x, true);
 
  _cholmodFactor = cholmod_analyze(_cholmodSparse, &_cholmodCommon);
  cholmod_factorize(_cholmodSparse, _cholmodFactor, &_cholmodCommon);
 
#if 0
    int* p = (int*)_cholmodFactor->Perm;
    for (int i = 0; i < (int)n; ++i)
      if (i != p[i])
        cerr << "wrong permutation" << i << " -> " << p[i] << endl;
#endif
 
  if (_cholmodCommon.status == CHOLMOD_NOT_POSDEF) {
    // std::cerr << "Cholesky failure, writing debug.txt (Hessian loadable by
    // Octave)" << std::endl; writeCCSMatrix("debug.txt", _cholmodSparse->nrow,
    // _cholmodSparse->ncol, (int*)_cholmodSparse->p, (int*)_cholmodSparse->i,
    // (double*)_cholmodSparse->x, true);
    return false;
  }
 
  // change to the specific format we need to have a pretty normal L
  int change_status = cholmod_change_factor(CHOLMOD_REAL, 1, 0, 1, 1,
                                            _cholmodFactor, &_cholmodCommon);
  if (!change_status) {
    return false;
  }
 
  return true;
}

References _cholmodCommon, _cholmodFactor, _cholmodSparse, _updateMat, g2o::SparseBlockMatrix< MatrixType >::cols(), g2o::cholmod::CholmodExt::columnsAllocated, g2o::SparseBlockMatrix< MatrixType >::fillCCS(), g2o::SparseBlockMatrix< MatrixType >::nonZeros(), and g2o::SparseBlockMatrix< MatrixType >::rows().

Referenced by updateInitialization().

◆ convertTripletUpdateToSparse()

void g2o::SparseOptimizerIncremental::convertTripletUpdateToSparse ( )

protected

Definition at line 530 of file graph_optimizer_sparse_incremental.cpp.

                                                              {
  // re-allocate the memory
  if (_tripletWorkspace.size() < (int)_permutedUpdate->ncol) {
    _tripletWorkspace.resize(_permutedUpdate->ncol * 2);
  }
 
  // reallocate num-zeros
  if (_permutedUpdateAsSparse->nzmax < _permutedUpdate->nzmax) {
    _permutedUpdateAsSparse->nzmax = _permutedUpdate->nzmax;
    delete[] static_cast<int*>(_permutedUpdateAsSparse->i);
    delete[] static_cast<double*>(_permutedUpdateAsSparse->x);
    _permutedUpdateAsSparse->x = new double[_permutedUpdateAsSparse->nzmax];
    _permutedUpdateAsSparse->i = new int[_permutedUpdateAsSparse->nzmax];
  }
 
  if (_permutedUpdateAsSparse->columnsAllocated < _permutedUpdate->ncol) {
    _permutedUpdateAsSparse->columnsAllocated = 2 * _permutedUpdate->ncol;
    delete[] static_cast<int*>(_permutedUpdateAsSparse->p);
    _permutedUpdateAsSparse->p =
        new int[_permutedUpdateAsSparse->columnsAllocated + 1];
  }
 
  _permutedUpdateAsSparse->ncol = _permutedUpdate->ncol;
  _permutedUpdateAsSparse->nrow = _permutedUpdate->nrow;
 
  int* w = _tripletWorkspace.data();
  memset(w, 0, sizeof(int) * _permutedUpdate->ncol);
 
  int* Ti = static_cast<int*>(_permutedUpdate->i);
  int* Tj = static_cast<int*>(_permutedUpdate->j);
  double* Tx = static_cast<double*>(_permutedUpdate->x);
 
  int* Cp = static_cast<int*>(_permutedUpdateAsSparse->p);
  int* Ci = static_cast<int*>(_permutedUpdateAsSparse->i);
  double* Cx = static_cast<double*>(_permutedUpdateAsSparse->x);
 
  for (size_t k = 0; k < _permutedUpdate->nnz; ++k) /* column counts */
    w[Tj[k]]++;
 
  /* column pointers */
  const int n = _permutedUpdate->ncol;
  int nz = 0;
  for (int i = 0; i < n; i++) {
    Cp[i] = nz;
    nz += w[i];
    w[i] = Cp[i];
  }
  Cp[n] = nz;
  assert((size_t)nz == _permutedUpdate->nnz);
 
  for (size_t k = 0; k < _permutedUpdate->nnz; ++k) {
    int p = w[Tj[k]]++;
    Ci[p] = Ti[k]; /* A(i,j) is the pth entry in C */
    Cx[p] = Tx[k];
  }
}

References _permutedUpdate, _permutedUpdateAsSparse, _tripletWorkspace, and g2o::cholmod::CholmodExt::columnsAllocated.

Referenced by updateInitialization().

◆ initSolver()

bool g2o::SparseOptimizerIncremental::initSolver	(	int	dimension,
		int	batchEveryN
	)

virtual

Reimplemented from g2o::SparseOptimizerOnline.

Definition at line 485 of file graph_optimizer_sparse_incremental.cpp.

                                                                          {
  // cerr << __PRETTY_FUNCTION__ << endl;
  slamDimension = dimension;
  if (dimension == 3) {
    setAlgorithm(createSolver("fix3_2_cholmod"));
    OptimizationAlgorithmGaussNewton* gaussNewton =
        dynamic_cast<OptimizationAlgorithmGaussNewton*>(solver());
    assert(gaussNewton);
    BlockSolver<BlockSolverTraits<3, 2>>* bs =
        dynamic_cast<BlockSolver<BlockSolverTraits<3, 2>>*>(
            &gaussNewton->solver());
    assert(bs && "Unable to get internal block solver");
    LinearSolverCholmodOnline<Matrix3d>* s =
        dynamic_cast<LinearSolverCholmodOnline<Matrix3d>*>(&bs->linearSolver());
    bs->setAdditionalVectorSpace(300);
    bs->setSchur(false);
    _solverInterface = s;
    _underlyingSolver = bs;
  } else {
    setAlgorithm(createSolver("fix6_3_cholmod"));
    OptimizationAlgorithmGaussNewton* gaussNewton =
        dynamic_cast<OptimizationAlgorithmGaussNewton*>(solver());
    assert(gaussNewton);
    BlockSolver<BlockSolverTraits<6, 3>>* bs =
        dynamic_cast<BlockSolver<BlockSolverTraits<6, 3>>*>(
            &gaussNewton->solver());
    assert(bs && "Unable to get internal block solver");
    LinearSolverCholmodOnline<Matrix<double, 6, 6>>* s =
        dynamic_cast<LinearSolverCholmodOnline<Matrix<double, 6, 6>>*>(
            &bs->linearSolver());
    bs->setAdditionalVectorSpace(600);
    bs->setSchur(false);
    _solverInterface = s;
    _underlyingSolver = bs;
  }
  _solverInterface->cmember = &_cmember;
  _solverInterface->batchEveryN = batchEveryN;
  if (!solver()) {
    cerr << "Error allocating solver. Allocating CHOLMOD solver failed!"
         << endl;
    return false;
  }
  return true;
}

References _cmember, _solverInterface, g2o::SparseOptimizerOnline::_underlyingSolver, g2o::LinearSolverCholmodOnlineInterface::batchEveryN, g2o::LinearSolverCholmodOnlineInterface::cmember, g2o::createSolver(), g2o::BlockSolver< Traits >::linearSolver(), g2o::Solver::setAdditionalVectorSpace(), g2o::SparseOptimizer::setAlgorithm(), g2o::BlockSolver< Traits >::setSchur(), g2o::SparseOptimizerOnline::slamDimension, g2o::OptimizationAlgorithmWithHessian::solver(), and g2o::SparseOptimizer::solver().

◆ optimize()

int g2o::SparseOptimizerIncremental::optimize	(	int	iterations,
		bool	online = `false`
	)

virtual

starts one optimization run given the current configuration of the graph, and the current settings stored in the class instance. It can be called only after initializeOptimization

Reimplemented from g2o::SparseOptimizer.

Definition at line 89 of file graph_optimizer_sparse_incremental.cpp.

                                                                    {
  (void)iterations;  // we only do one iteration anyhow
  OptimizationAlgorithm* solver = _algorithm;
  solver->init(online);
 
  bool ok = true;
 
  if (!online || batchStep) {
    // cerr << "performing batch step" << endl;
    if (!online) {
      ok = _underlyingSolver->buildStructure();
      if (!ok) {
        cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure"
             << endl;
        return 0;
      }
    }
 
    // copy over the updated estimate as new linearization point
    if (slamDimension == 3) {
      for (size_t i = 0; i < indexMapping().size(); ++i) {
        OnlineVertexSE2* v = static_cast<OnlineVertexSE2*>(indexMapping()[i]);
        v->setEstimate(v->updatedEstimate);
      }
    } else if (slamDimension == 6) {
      for (size_t i = 0; i < indexMapping().size(); ++i) {
        OnlineVertexSE3* v = static_cast<OnlineVertexSE3*>(indexMapping()[i]);
        v->setEstimate(v->updatedEstimate);
      }
    }
 
    SparseOptimizer::computeActiveErrors();
    // SparseOptimizer::linearizeSystem();
    _underlyingSolver->buildSystem();
 
    // mark vertices to be sorted as last
    int numBlocksRequired = _ivMap.size();
    if (_cmember.size() < numBlocksRequired) {
      _cmember.resize(2 * numBlocksRequired);
    }
    memset(_cmember.data(), 0, numBlocksRequired * sizeof(int));
    if (_ivMap.size() > 100) {
      for (size_t i = _ivMap.size() - 20; i < _ivMap.size(); ++i) {
        const HyperGraph::EdgeSet& eset = _ivMap[i]->edges();
        for (HyperGraph::EdgeSet::const_iterator it = eset.begin();
             it != eset.end(); ++it) {
          OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
          OptimizableGraph::Vertex* v1 =
              static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
          OptimizableGraph::Vertex* v2 =
              static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
          if (v1->hessianIndex() >= 0) _cmember(v1->hessianIndex()) = 1;
          if (v2->hessianIndex() >= 0) _cmember(v2->hessianIndex()) = 1;
        }
      }
      // OptimizableGraph::Vertex* lastPose = _ivMap.back();
      //_cmember(lastPose->hessianIndex()) = 2;
    }
 
    ok = _underlyingSolver->solve();
 
    // get the current cholesky factor along with the permutation
    _L = _solverInterface->L();
    if (_perm.size() < (int)_L->n) _perm.resize(2 * _L->n);
    int* p = static_cast<int*>(_L->Perm);
    for (size_t i = 0; i < _L->n; ++i) _perm[p[i]] = i;
 
  } else {
    // update the b vector
    for (HyperGraph::VertexSet::iterator it = _touchedVertices.begin();
         it != _touchedVertices.end(); ++it) {
      OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
      int iBase = v->colInHessian();
      v->copyB(_underlyingSolver->b() + iBase);
    }
    _solverInterface->solve(_underlyingSolver->x(), _underlyingSolver->b());
  }
 
  update(_underlyingSolver->x());
 
  if (verbose()) {
    computeActiveErrors();
    cerr << "nodes = " << vertices().size()
         << "\t edges= " << _activeEdges.size()
         << "\t chi2= " << FIXED(activeChi2()) << endl;
  }
 
  if (vizWithGnuplot) gnuplotVisualization();
 
  if (!ok) return 0;
  return 1;
}

◆ updateInitialization()

bool g2o::SparseOptimizerIncremental::updateInitialization	(	HyperGraph::VertexSet &	vset,
		HyperGraph::EdgeSet &	eset
	)

virtual

HACK updating the internal structures for online processing

Reimplemented from g2o::SparseOptimizerOnline.

Definition at line 182 of file graph_optimizer_sparse_incremental.cpp.

                                                        {
  if (batchStep) {
    return SparseOptimizerOnline::updateInitialization(vset, eset);
  }
 
  for (HyperGraph::VertexSet::iterator it = vset.begin(); it != vset.end();
       ++it) {
    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
    v->clearQuadraticForm();  // be sure that b is zero for this vertex
  }
 
  // get the touched vertices
  _touchedVertices.clear();
  for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end();
       ++it) {
    OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
    OptimizableGraph::Vertex* v1 =
        static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
    OptimizableGraph::Vertex* v2 =
        static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
    if (!v1->fixed()) _touchedVertices.insert(v1);
    if (!v2->fixed()) _touchedVertices.insert(v2);
  }
  // cerr << PVAR(_touchedVertices.size()) << endl;
 
  // updating the internal structures
  std::vector<HyperGraph::Vertex*> newVertices;
  newVertices.reserve(vset.size());
  _activeVertices.reserve(_activeVertices.size() + vset.size());
  _activeEdges.reserve(_activeEdges.size() + eset.size());
  for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end(); ++it)
    _activeEdges.push_back(static_cast<OptimizableGraph::Edge*>(*it));
  // cerr << "updating internal done." << endl;
 
  // update the index mapping
  size_t next = _ivMap.size();
  for (HyperGraph::VertexSet::iterator it = vset.begin(); it != vset.end();
       ++it) {
    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
    if (!v->fixed()) {
      if (!v->marginalized()) {
        v->setHessianIndex(next);
        _ivMap.push_back(v);
        newVertices.push_back(v);
        _activeVertices.push_back(v);
        next++;
      } else  // not supported right now
        abort();
    } else {
      v->setHessianIndex(-1);
    }
  }
  // cerr << "updating index mapping done." << endl;
 
  // backup the tempindex and prepare sorting structure
#ifdef _MSC_VER
  VertexBackup* backupIdx = new VertexBackup[_touchedVertices.size()];
#else
  VertexBackup backupIdx[_touchedVertices.size()];
#endif
  memset(backupIdx, 0, sizeof(VertexBackup) * _touchedVertices.size());
  int idx = 0;
  for (HyperGraph::VertexSet::iterator it = _touchedVertices.begin();
       it != _touchedVertices.end(); ++it) {
    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
    backupIdx[idx].hessianIndex = v->hessianIndex();
    backupIdx[idx].vertex = v;
    backupIdx[idx].hessianData = v->hessianData();
    ++idx;
  }
  sort(backupIdx,
       backupIdx +
           _touchedVertices
               .size());  // sort according to the hessianIndex which is the
                          // same order as used later by the optimizer
  for (int i = 0; i < idx; ++i) {
    backupIdx[i].vertex->setHessianIndex(i);
  }
  // cerr << "backup tempindex done." << endl;
 
  // building the structure of the update
  _updateMat.clear(true);  // get rid of the old matrix structure
  _updateMat.rowBlockIndices().clear();
  _updateMat.colBlockIndices().clear();
  _updateMat.blockCols().clear();
 
  // placing the current stuff in _updateMat
  MatrixXd* lastBlock = 0;
  int sizePoses = 0;
  for (int i = 0; i < idx; ++i) {
    OptimizableGraph::Vertex* v = backupIdx[i].vertex;
    int dim = v->dimension();
    sizePoses += dim;
    _updateMat.rowBlockIndices().push_back(sizePoses);
    _updateMat.colBlockIndices().push_back(sizePoses);
    _updateMat.blockCols().push_back(
        SparseBlockMatrix<MatrixXd>::IntBlockMap());
    int ind = v->hessianIndex();
    // cerr << PVAR(ind) << endl;
    if (ind >= 0) {
      MatrixXd* m = _updateMat.block(ind, ind, true);
      v->mapHessianMemory(m->data());
      lastBlock = m;
    }
  }
  if (lastBlock) {
    lastBlock->diagonal().array() += 1e-6;  // HACK to get Eigen value > 0
  }
 
  for (HyperGraph::EdgeSet::const_iterator it = eset.begin(); it != eset.end();
       ++it) {
    OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
    OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*)e->vertices()[0];
    OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*)e->vertices()[1];
 
    int ind1 = v1->hessianIndex();
    if (ind1 == -1) continue;
    int ind2 = v2->hessianIndex();
    if (ind2 == -1) continue;
    bool transposedBlock = ind1 > ind2;
    if (transposedBlock)  // make sure, we allocate the upper triangular block
      swap(ind1, ind2);
 
    MatrixXd* m = _updateMat.block(ind1, ind2, true);
    e->mapHessianMemory(m->data(), 0, 1, transposedBlock);
  }
 
  // build the system into _updateMat
  for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end();
       ++it) {
    OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
    e->computeError();
  }
  for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end();
       ++it) {
    OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
    e->linearizeOplus(jacobianWorkspace());
    e->constructQuadraticForm();
  }
 
  // restore the original data for the vertex
  for (int i = 0; i < idx; ++i) {
    backupIdx[i].vertex->setHessianIndex(backupIdx[i].hessianIndex);
    if (backupIdx[i].hessianData)
      backupIdx[i].vertex->mapHessianMemory(backupIdx[i].hessianData);
  }
 
  // update the structure of the real block matrix
  bool solverStatus = _algorithm->updateStructure(newVertices, eset);
 
  bool updateStatus = computeCholeskyUpdate();
  if (!updateStatus) {
    cerr << "Error while computing update" << endl;
  }
 
  cholmod_sparse* updateAsSparseFactor =
      cholmod_factor_to_sparse(_cholmodFactor, &_cholmodCommon);
 
  // convert CCS update by permuting back to the permutation of L
  if (updateAsSparseFactor->nzmax > _permutedUpdate->nzmax) {
    // cerr << "realloc _permutedUpdate" << endl;
    cholmod_reallocate_triplet(updateAsSparseFactor->nzmax, _permutedUpdate,
                               &_cholmodCommon);
  }
  _permutedUpdate->nnz = 0;
  _permutedUpdate->nrow = _permutedUpdate->ncol = _L->n;
  {
    int* Ap = static_cast<int*>(updateAsSparseFactor->p);
    int* Ai = static_cast<int*>(updateAsSparseFactor->i);
    double* Ax = static_cast<double*>(updateAsSparseFactor->x);
    int* Bj = static_cast<int*>(_permutedUpdate->j);
    int* Bi = static_cast<int*>(_permutedUpdate->i);
    double* Bx = static_cast<double*>(_permutedUpdate->x);
    for (size_t c = 0; c < updateAsSparseFactor->ncol; ++c) {
      const int& rbeg = Ap[c];
      const int& rend = Ap[c + 1];
      int cc = c / slamDimension;
      int coff = c % slamDimension;
      const int& cbase = backupIdx[cc].vertex->colInHessian();
      const int& ccol = _perm(cbase + coff);
      for (int j = rbeg; j < rend; j++) {
        const int& r = Ai[j];
        const double& val = Ax[j];
 
        int rr = r / slamDimension;
        int roff = r % slamDimension;
        const int& rbase = backupIdx[rr].vertex->colInHessian();
 
        int row = _perm(rbase + roff);
        int col = ccol;
        if (col > row)  // lower triangular entry
          swap(col, row);
        Bi[_permutedUpdate->nnz] = row;
        Bj[_permutedUpdate->nnz] = col;
        Bx[_permutedUpdate->nnz] = val;
        ++_permutedUpdate->nnz;
      }
    }
  }
  cholmod_free_sparse(&updateAsSparseFactor, &_cholmodCommon);
#ifdef _MSC_VER
  delete[] backupIdx;
#endif
 
#if 0
    cholmod_sparse* updatePermuted = cholmod_triplet_to_sparse(_permutedUpdate, _permutedUpdate->nnz, &_cholmodCommon);
    //writeCCSMatrix("update-permuted.txt", updatePermuted->nrow, updatePermuted->ncol, (int*)updatePermuted->p, (int*)updatePermuted->i, (double*)updatePermuted->x, false);
    _solverInterface->choleskyUpdate(updatePermuted);
    cholmod_free_sparse(&updatePermuted, &_cholmodCommon);
#else
  convertTripletUpdateToSparse();
  _solverInterface->choleskyUpdate(_permutedUpdateAsSparse);
#endif
 
  return solverStatus;
}

Member Data Documentation

◆ _cholmodCommon

cholmod_common g2o::SparseOptimizerIncremental::_cholmodCommon

protected

Definition at line 46 of file graph_optimizer_sparse_incremental.h.

Referenced by computeCholeskyUpdate(), SparseOptimizerIncremental(), updateInitialization(), and ~SparseOptimizerIncremental().

◆ _cholmodFactor

cholmod_factor* g2o::SparseOptimizerIncremental::_cholmodFactor

protected

Definition at line 48 of file graph_optimizer_sparse_incremental.h.

Referenced by computeCholeskyUpdate(), SparseOptimizerIncremental(), updateInitialization(), and ~SparseOptimizerIncremental().

◆ _cholmodSparse

cholmod::CholmodExt* g2o::SparseOptimizerIncremental::_cholmodSparse

protected

Definition at line 47 of file graph_optimizer_sparse_incremental.h.

Referenced by computeCholeskyUpdate(), SparseOptimizerIncremental(), and ~SparseOptimizerIncremental().

◆ _cmember

Eigen::VectorXi g2o::SparseOptimizerIncremental::_cmember

protected

Definition at line 55 of file graph_optimizer_sparse_incremental.h.

Referenced by initSolver(), and optimize().

◆ _L

cholmod_factor* g2o::SparseOptimizerIncremental::_L

protected

Definition at line 50 of file graph_optimizer_sparse_incremental.h.

Referenced by optimize(), SparseOptimizerIncremental(), and updateInitialization().

◆ _perm

Eigen::VectorXi g2o::SparseOptimizerIncremental::_perm

protected

Definition at line 54 of file graph_optimizer_sparse_incremental.h.

Referenced by optimize(), and updateInitialization().

◆ _permutedUpdate

cholmod_triplet* g2o::SparseOptimizerIncremental::_permutedUpdate

protected

Definition at line 49 of file graph_optimizer_sparse_incremental.h.

Referenced by convertTripletUpdateToSparse(), SparseOptimizerIncremental(), updateInitialization(), and ~SparseOptimizerIncremental().

◆ _permutedUpdateAsSparse

cholmod::CholmodExt* g2o::SparseOptimizerIncremental::_permutedUpdateAsSparse

protected

Definition at line 58 of file graph_optimizer_sparse_incremental.h.

Referenced by convertTripletUpdateToSparse(), SparseOptimizerIncremental(), updateInitialization(), and ~SparseOptimizerIncremental().

◆ _solverInterface

LinearSolverCholmodOnlineInterface* g2o::SparseOptimizerIncremental::_solverInterface

protected

Definition at line 51 of file graph_optimizer_sparse_incremental.h.

Referenced by initSolver(), optimize(), SparseOptimizerIncremental(), and updateInitialization().

◆ _touchedVertices

HyperGraph::VertexSet g2o::SparseOptimizerIncremental::_touchedVertices

protected

Definition at line 53 of file graph_optimizer_sparse_incremental.h.

Referenced by optimize(), and updateInitialization().

◆ _tripletWorkspace

Eigen::VectorXi g2o::SparseOptimizerIncremental::_tripletWorkspace

protected

Definition at line 57 of file graph_optimizer_sparse_incremental.h.

Referenced by convertTripletUpdateToSparse().

◆ _updateMat

SparseBlockMatrix<Eigen::MatrixXd> g2o::SparseOptimizerIncremental::_updateMat

protected

Definition at line 45 of file graph_optimizer_sparse_incremental.h.

Referenced by computeCholeskyUpdate(), updateInitialization(), and ~SparseOptimizerIncremental().

The documentation for this class was generated from the following files:

/build/reproducible-path/g2o-0~20230806/g2o/examples/interactive_slam/g2o_incremental/graph_optimizer_sparse_incremental.h
/build/reproducible-path/g2o-0~20230806/g2o/examples/interactive_slam/g2o_incremental/graph_optimizer_sparse_incremental.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ SparseOptimizerIncremental()

◆ ~SparseOptimizerIncremental()

Member Function Documentation

◆ computeCholeskyUpdate()

◆ convertTripletUpdateToSparse()

◆ initSolver()

◆ optimize()

◆ updateInitialization()

Member Data Documentation

◆ _cholmodCommon

◆ _cholmodFactor

◆ _cholmodSparse

◆ _cmember

◆ _L

◆ _perm

◆ _permutedUpdate

◆ _permutedUpdateAsSparse

◆ _solverInterface

◆ _touchedVertices

◆ _tripletWorkspace

◆ _updateMat