OptimizableGraph::Vertex* SparseOptimizer::findGauge(){
if (vertices().empty())
return 0;
int maxDim=0;
for (HyperGraph::VertexIDMap::iterator
it = vertices().begin();
it != vertices().end();
it++)
{
OptimizableGraph::Vertex* v =
static_cast<OptimizableGraph::Vertex*>(it->second);
maxDim = std::max(maxDim,v->dimension());
}
OptimizableGraph::Vertex* rut=0;
for (HyperGraph::VertexIDMap::iterator
it = vertices().begin();
it != vertices().end();
it++)
{
OptimizableGraph::Vertex* v =
static_cast<OptimizableGraph::Vertex*>(it->second);
if (v->dimension()==maxDim)
{
rut=v;
break;
}
}
return rut;
}
void BaseMultiEdge<D, E>::mapHessianMemory(double* d, int i, int j, bool rowMajor)
{
int idx = computeUpperTriangleIndex(i, j);
assert(idx < (int)_hessian.size());
OptimizableGraph::Vertex* vi = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
OptimizableGraph::Vertex* vj = static_cast<OptimizableGraph::Vertex*>(_vertices[j]);
HessianHelper& h = _hessian[idx];
if (rowMajor) {
if (h.matrix.data() != d || h.transposed != rowMajor)
new (&h.matrix) HessianBlockType(d, vj->dimension(), vi->dimension());
} else {
if (h.matrix.data() != d || h.transposed != rowMajor)
new (&h.matrix) HessianBlockType(d, vi->dimension(), vj->dimension());
}
h.transposed = rowMajor;
}
void BaseMultiEdge<D, E>::linearizeOplus(JacobianWorkspace& jacobianWorkspace)
{
for (size_t i = 0; i < _vertices.size(); ++i) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
assert(v->dimension() >= 0);
new (&_jacobianOplus[i]) JacobianType(jacobianWorkspace.workspaceForVertex(i), D, v->dimension());
}
linearizeOplus();
}
bool edgeAllVertsSameDim(OptimizableGraph::Edge* e, int dim)
{
for (size_t i = 0; i < e->vertices().size(); ++i) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(e->vertices()[i]);
if (v->dimension() != dim)
return false;
}
return true;
}
void SparseOptimizer::update(double* update)
{
// update the graph by calling oplus on the vertices
for (size_t i=0; i < _ivMap.size(); ++i)
{
OptimizableGraph::Vertex* v = _ivMap[i];
v->oplus(update);
update += v->dimension();
}
}
void BaseMultiEdge<D, E>::constructQuadraticForm()
{
const InformationType& omega = _information;
Matrix<double, D, 1> omega_r = - omega * _error;
for (size_t i = 0; i < _vertices.size(); ++i) {
OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
bool istatus = !(from->fixed());
if (istatus) {
const MatrixXd& A = _jacobianOplus[i];
MatrixXd AtO = A.transpose() * omega;
int fromDim = from->dimension();
Map<MatrixXd> fromMap(from->hessianData(), fromDim, fromDim);
Map<VectorXd> fromB(from->bData(), fromDim);
// ii block in the hessian
#ifdef G2O_OPENMP
from->lockQuadraticForm();
#endif
fromMap.noalias() += AtO * A;
fromB.noalias() += A.transpose() * omega_r;
// compute the off-diagonal blocks ij for all j
for (size_t j = i+1; j < _vertices.size(); ++j) {
OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(_vertices[j]);
#ifdef G2O_OPENMP
to->lockQuadraticForm();
#endif
bool jstatus = !(to->fixed());
if (jstatus) {
const MatrixXd& B = _jacobianOplus[j];
int idx = computeUpperTriangleIndex(i, j);
assert(idx < (int)_hessian.size());
HessianHelper& hhelper = _hessian[idx];
if (hhelper.transposed) { // we have to write to the block as transposed
hhelper.matrix.noalias() += B.transpose() * AtO.transpose();
} else {
hhelper.matrix.noalias() += AtO * B;
}
}
#ifdef G2O_OPENMP
to->unlockQuadraticForm();
#endif
}
#ifdef G2O_OPENMP
from->unlockQuadraticForm();
#endif
}
}
}
bool BlockSolver<Traits>::updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges)
{
for (std::vector<HyperGraph::Vertex*>::const_iterator vit = vset.begin(); vit != vset.end(); ++vit) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit);
int dim = v->dimension();
if (! v->marginalized()){
v->setColInHessian(_sizePoses);
_sizePoses+=dim;
_Hpp->rowBlockIndices().push_back(_sizePoses);
_Hpp->colBlockIndices().push_back(_sizePoses);
_Hpp->blockCols().push_back(typename SparseBlockMatrix<PoseMatrixType>::IntBlockMap());
++_numPoses;
int ind = v->hessianIndex();
PoseMatrixType* m = _Hpp->block(ind, ind, true);
v->mapHessianMemory(m->data());
} else {
std::cerr << "updateStructure(): Schur not supported" << std::endl;
abort();
}
}
resizeVector(_sizePoses + _sizeLandmarks);
for (HyperGraph::EdgeSet::const_iterator it = edges.begin(); it != edges.end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);
int ind1 = v1->hessianIndex();
int indexV1Bak = ind1;
if (ind1 == -1)
continue;
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 triangular block
std::swap(ind1, ind2);
if (! v1->marginalized() && !v2->marginalized()) {
PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
} else {
std::cerr << __PRETTY_FUNCTION__ << ": not supported" << std::endl;
}
}
}
}
return true;
}
bool SparseOptimizer::gaugeFreedom()
{
if (vertices().empty())
return false;
int maxDim=0;
for (HyperGraph::VertexIDMap::iterator
it = vertices().begin();
it != vertices().end();
it++)
{
OptimizableGraph::Vertex* v =
static_cast<OptimizableGraph::Vertex*>(it->second);
maxDim = std::max(maxDim,v->dimension());
}
for (HyperGraph::VertexIDMap::iterator
it = vertices().begin();
it != vertices().end();
it++)
{
OptimizableGraph::Vertex* v =
static_cast<OptimizableGraph::Vertex*>(it->second);
if (v->dimension() == maxDim)
{
// test for full dimension prior
for (HyperGraph::EdgeSet::const_iterator
eit = v->edges().begin();
eit != v->edges().end();
++eit)
{
OptimizableGraph::Edge* e =
static_cast<OptimizableGraph::Edge*>(*eit);
if (e->vertices().size() == 1 && e->dimension() == maxDim)
return false;
}
}
}
return true;
}
double OptimizationAlgorithmLevenberg::computeLambdaInit() const
{
if (_userLambdaInit->value() > 0)
return _userLambdaInit->value();
double maxDiagonal=0.;
for (size_t k = 0; k < _optimizer->indexMapping().size(); k++) {
OptimizableGraph::Vertex* v = _optimizer->indexMapping()[k];
assert(v);
int dim = v->dimension();
for (int j = 0; j < dim; ++j){
maxDiagonal = std::max(fabs(v->hessian(j,j)),maxDiagonal);
}
}
return _tau*maxDiagonal;
}
bool MainWindow::prepare()
{
SparseOptimizer* optimizer = viewer->graph;
if (_currentOptimizationAlgorithmProperty.requiresMarginalize) {
cerr << "Marginalizing Landmarks" << endl;
for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer->vertices().begin(); it != optimizer->vertices().end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
int vdim = v->dimension();
v->setMarginalized((vdim == _currentOptimizationAlgorithmProperty.landmarkDim));
}
}
else {
cerr << "Preparing (no marginalization of Landmarks)" << endl;
for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer->vertices().begin(); it != optimizer->vertices().end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
v->setMarginalized(false);
}
}
viewer->graph->initializeOptimization();
return true;
}
bool SparseOptimizerIncremental::updateInitialization(HyperGraph::VertexSet& vset, HyperGraph::EdgeSet& eset)
{
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
VertexBackup backupIdx[_touchedVertices.size()];
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;
}
}
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();
}
for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
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 = (int*)updateAsSparseFactor->p;
int* Ai = (int*)updateAsSparseFactor->i;
double* Ax = (double*)updateAsSparseFactor->x;
int* Bj = (int*)_permutedUpdate->j;
int* Bi = (int*)_permutedUpdate->i;
double* Bx = (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);
#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;
}
void BaseMultiEdge<D, E>::linearizeOplus()
{
#ifdef G2O_OPENMP
for (size_t i = 0; i < _vertices.size(); ++i) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
v->lockQuadraticForm();
}
#endif
const double delta = 1e-9;
const double scalar = 1.0 / (2*delta);
ErrorVector errorBak;
ErrorVector errorBeforeNumeric = _error;
for (size_t i = 0; i < _vertices.size(); ++i) {
//Xi - estimate the jacobian numerically
OptimizableGraph::Vertex* vi = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
if (vi->fixed())
continue;
const int vi_dim = vi->dimension();
#ifdef _MSC_VER
double* add_vi = new double[vi_dim];
#else
double add_vi[vi_dim];
#endif
std::fill(add_vi, add_vi + vi_dim, 0.0);
if (_jacobianOplus[i].rows() != _dimension || _jacobianOplus[i].cols() != vi_dim)
_jacobianOplus[i].resize(_dimension, vi_dim);
// add small step along the unit vector in each dimension
for (int d = 0; d < vi_dim; ++d) {
vi->push();
add_vi[d] = delta;
vi->oplus(add_vi);
computeError();
errorBak = _error;
vi->pop();
vi->push();
add_vi[d] = -delta;
vi->oplus(add_vi);
computeError();
errorBak -= _error;
vi->pop();
add_vi[d] = 0.0;
_jacobianOplus[i].col(d) = scalar * errorBak;
} // end dimension
#ifdef _MSC_VER
delete[] add_vi;
#endif
}
_error = errorBeforeNumeric;
#ifdef G2O_OPENMP
for (int i = (int)(_vertices.size()) - 1; i >= 0; --i) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
v->unlockQuadraticForm();
}
#endif
}
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)
return true;
_DInvSchur->diagonal().resize(landmarkIdx);
_Hpl->fillSparseBlockMatrixCCS(*_HplCCS);
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
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;
}
bool saveGnuplot(const std::string& gnudump, const HyperGraph::VertexSet& vertices, const HyperGraph::EdgeSet& edges)
{
// seek for an action whose name is writeGnuplot in the library
HyperGraphElementAction* saveGnuplot = HyperGraphActionLibrary::instance()->actionByName("writeGnuplot");
if (! saveGnuplot ){
cerr << __PRETTY_FUNCTION__ << ": no action \"writeGnuplot\" registered" << endl;
return false;
}
WriteGnuplotAction::Parameters params;
int maxDim = -1;
int minDim = numeric_limits<int>::max();
for (HyperGraph::VertexSet::const_iterator it = vertices.begin(); it != vertices.end(); ++it){
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
int vdim = v->dimension();
maxDim = (std::max)(vdim, maxDim);
minDim = (std::min)(vdim, minDim);
}
string extension = getFileExtension(gnudump);
if (extension.size() == 0)
extension = "dat";
string baseFilename = getPureFilename(gnudump);
// check for odometry edges
bool hasOdomEdge = false;
bool hasLandmarkEdge = false;
for (HyperGraph::EdgeSet::const_iterator it = edges.begin(); it != edges.end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
if (e->vertices().size() == 2) {
if (edgeAllVertsSameDim(e, maxDim))
hasOdomEdge = true;
else
hasLandmarkEdge = true;
}
if (hasOdomEdge && hasLandmarkEdge)
break;
}
bool fileStatus = true;
if (hasOdomEdge) {
string odomFilename = baseFilename + "_odom_edges." + extension;
cerr << "# saving " << odomFilename << " ... ";
ofstream fout(odomFilename.c_str());
if (! fout) {
cerr << "Unable to open file" << endl;
return false;
}
params.os = &fout;
// writing odometry edges
for (HyperGraph::EdgeSet::const_iterator it = edges.begin(); it != edges.end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
if (e->vertices().size() != 2 || ! edgeAllVertsSameDim(e, maxDim))
continue;
(*saveGnuplot)(e, ¶ms);
}
cerr << "done." << endl;
}
if (hasLandmarkEdge) {
string filename = baseFilename + "_landmarks_edges." + extension;
cerr << "# saving " << filename << " ... ";
ofstream fout(filename.c_str());
if (! fout) {
cerr << "Unable to open file" << endl;
return false;
}
params.os = &fout;
// writing landmark edges
for (HyperGraph::EdgeSet::const_iterator it = edges.begin(); it != edges.end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
if (e->vertices().size() != 2 || edgeAllVertsSameDim(e, maxDim))
continue;
(*saveGnuplot)(e, ¶ms);
}
cerr << "done." << endl;
}
if (1) {
string filename = baseFilename + "_edges." + extension;
cerr << "# saving " << filename << " ... ";
ofstream fout(filename.c_str());
if (! fout) {
cerr << "Unable to open file" << endl;
return false;
}
params.os = &fout;
// writing all edges
for (HyperGraph::EdgeSet::const_iterator it = edges.begin(); it != edges.end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
(*saveGnuplot)(e, ¶ms);
}
cerr << "done." << endl;
}
if (1) {
string filename = baseFilename + "_vertices." + extension;
cerr << "# saving " << filename << " ... ";
ofstream fout(filename.c_str());
if (! fout) {
cerr << "Unable to open file" << endl;
return false;
}
params.os = &fout;
for (HyperGraph::VertexSet::const_iterator it = vertices.begin(); it != vertices.end(); ++it){
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
(*saveGnuplot)(v, ¶ms);
}
cerr << "done." << endl;
}
return fileStatus;
}
