void SparseOptimizer::computeInitialGuess(EstimatePropagatorCost& costFunction) { OptimizableGraph::VertexSet emptySet; std::set<Vertex*> backupVertices; HyperGraph::VertexSet fixedVertices; // these are the root nodes where to start the initialization 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->vertex(i)); if (!v) continue; 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->hessianIndex() == -1) { std::set<Vertex*>::const_iterator foundIt = backupVertices.find(v); if (foundIt == backupVertices.end()) { v->push(); backupVertices.insert(v); } } } } EstimatePropagator estimatePropagator(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 " << costFunction.name() << ")" << endl; } }
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; }
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; }
void EstimatePropagator::propagate(OptimizableGraph::VertexSet& vset, const EstimatePropagator::PropagateCost& cost, const EstimatePropagator::PropagateAction& action, double maxDistance, double maxEdgeCost) { reset(); PriorityQueue frontier; for (OptimizableGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){ OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit); AdjacencyMap::iterator it = _adjacencyMap.find(v); assert(it != _adjacencyMap.end()); it->second._distance = 0.; it->second._parent.clear(); it->second._frontierLevel = 0; frontier.push(&it->second); } while(! frontier.empty()){ AdjacencyMapEntry* entry = frontier.pop(); OptimizableGraph::Vertex* u = entry->child(); double uDistance = entry->distance(); //cerr << "uDistance " << uDistance << endl; // initialize the vertex if (entry->_frontierLevel > 0) { action(entry->edge(), entry->parent(), u); } /* std::pair< OptimizableGraph::VertexSet::iterator, bool> insertResult = */ _visited.insert(u); OptimizableGraph::EdgeSet::iterator et = u->edges().begin(); while (et != u->edges().end()){ OptimizableGraph::Edge* edge = static_cast<OptimizableGraph::Edge*>(*et); ++et; int maxFrontier = -1; OptimizableGraph::VertexSet initializedVertices; for (size_t i = 0; i < edge->vertices().size(); ++i) { OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i)); AdjacencyMap::iterator ot = _adjacencyMap.find(z); if (ot->second._distance != numeric_limits<double>::max()) { initializedVertices.insert(z); maxFrontier = (max)(maxFrontier, ot->second._frontierLevel); } } assert(maxFrontier >= 0); for (size_t i = 0; i < edge->vertices().size(); ++i) { OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i)); if (z == u) continue; size_t wasInitialized = initializedVertices.erase(z); double edgeDistance = cost(edge, initializedVertices, z); if (edgeDistance > 0. && edgeDistance != std::numeric_limits<double>::max() && edgeDistance < maxEdgeCost) { double zDistance = uDistance + edgeDistance; //cerr << z->id() << " " << zDistance << endl; AdjacencyMap::iterator ot = _adjacencyMap.find(z); assert(ot!=_adjacencyMap.end()); if (zDistance < ot->second.distance() && zDistance < maxDistance){ //if (ot->second.inQueue) //cerr << "Updating" << endl; ot->second._distance = zDistance; ot->second._parent = initializedVertices; ot->second._edge = edge; ot->second._frontierLevel = maxFrontier + 1; frontier.push(&ot->second); } } if (wasInitialized > 0) initializedVertices.insert(z); } } } // writing debug information like cost for reaching each vertex and the parent used to initialize #ifdef DEBUG_ESTIMATE_PROPAGATOR cerr << "Writing cost.dat" << endl; ofstream costStream("cost.dat"); for (AdjacencyMap::const_iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) { HyperGraph::Vertex* u = it->second.child(); costStream << "vertex " << u->id() << " cost " << it->second._distance << endl; } cerr << "Writing init.dat" << endl; ofstream initStream("init.dat"); vector<AdjacencyMapEntry*> frontierLevels; for (AdjacencyMap::iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) { if (it->second._frontierLevel > 0) frontierLevels.push_back(&it->second); } sort(frontierLevels.begin(), frontierLevels.end(), FrontierLevelCmp()); for (vector<AdjacencyMapEntry*>::const_iterator it = frontierLevels.begin(); it != frontierLevels.end(); ++it) { AdjacencyMapEntry* entry = *it; OptimizableGraph::Vertex* to = entry->child(); initStream << "calling init level = " << entry->_frontierLevel << "\t ("; for (OptimizableGraph::VertexSet::iterator pit = entry->parent().begin(); pit != entry->parent().end(); ++pit) { initStream << " " << (*pit)->id(); } initStream << " ) -> " << to->id() << endl; } #endif }