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 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; }
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; }