bool SparseOptimizerIncremental::updateInitialization(HyperGraph::VertexSet& vset, HyperGraph::EdgeSet& eset) { if (batchStep) { return SparseOptimizerOnline::updateInitialization(vset, eset); } //cerr << __PRETTY_FUNCTION__ << endl; 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->setTempIndex(next); _ivMap.push_back(v); newVertices.push_back(v); _activeVertices.push_back(v); next++; } else // not supported right now abort(); } else { v->setTempIndex(-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].tempIndex = v->tempIndex(); backupIdx[idx].vertex = v; backupIdx[idx].hessianData = v->hessianData(); ++idx; } sort(backupIdx, backupIdx + _touchedVertices.size()); // sort according to the tempIndex which is the same order as used later by the optimizer for (int i = 0; i < idx; ++i) { backupIdx[i].vertex->setTempIndex(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->tempIndex(); //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->tempIndex(); if (ind1 == -1) continue; int ind2 = v2->tempIndex(); 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->setTempIndex(backupIdx[i].tempIndex); if (backupIdx[i].hessianData) backupIdx[i].vertex->mapHessianMemory(backupIdx[i].hessianData); } // update the structure of the real block matrix bool solverStatus = _solver->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); 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*)updatePermuted2->x, false); _solverInterface->choleskyUpdate(updatePermuted); cholmod_free_sparse(&updatePermuted, &_cholmodCommon); return solverStatus; }
void SparseOptimizer::computeInitialGuess() { OptimizableGraph::VertexSet emptySet; std::set<Vertex*> backupVertices; // these are the root nodes where to start the initialization HyperGraph::VertexSet fixedVertices; for (EdgeContainer::iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) { OptimizableGraph::Edge* e = *it; for (size_t i = 0; i < e->vertices().size(); ++i) { OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(e->vertices()[i]); if (v->fixed()) fixedVertices.insert(v); else { // check for having a prior which is able to fully initialize a vertex for (EdgeSet::const_iterator vedgeIt = v->edges().begin(); vedgeIt != v->edges().end(); ++vedgeIt) { OptimizableGraph::Edge* vedge = static_cast<OptimizableGraph::Edge*>(*vedgeIt); if ( vedge->vertices().size() == 1 && vedge->initialEstimatePossible(emptySet, v) > 0.) { //cerr << "Initialize with prior for " << v->id() << endl; vedge->initialEstimate(emptySet, v); fixedVertices.insert(v); } } } if (v->tempIndex() == -1) { std::set<Vertex*>::const_iterator foundIt = backupVertices.find(v); if (foundIt == backupVertices.end()) { v->push(); backupVertices.insert(v); } } } } EstimatePropagator estimatePropagator(this); EstimatePropagator::PropagateCost costFunction(this); estimatePropagator.propagate(fixedVertices, costFunction); // restoring the vertices that should not be initialized for (std::set<Vertex*>::iterator it = backupVertices.begin(); it != backupVertices.end(); ++it) { Vertex* v = *it; v->pop(); } if (verbose()) { computeActiveErrors(); cerr << "iteration= -1\t chi2= " << activeChi2() << "\t time= 0.0" << "\t cumTime= 0.0" << "\t (using initial guess from spanning tree)" << endl; } }