bool SparseOptimizer::updateInitialization(HyperGraph::VertexSet& vset, HyperGraph::EdgeSet& eset) { std::vector<HyperGraph::Vertex*> newVertices; newVertices.reserve(vset.size()); _activeVertices.reserve(_activeVertices.size() + vset.size()); //for (HyperGraph::VertexSet::iterator it = vset.begin(); it != vset.end(); ++it) //_activeVertices.push_back(static_cast<OptimizableGraph::Vertex*>(*it)); _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)); // 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); } } //if (newVertices.size() != vset.size()) //cerr << __PRETTY_FUNCTION__ << ": something went wrong " << PVAR(vset.size()) << " " << PVAR(newVertices.size()) << endl; return _solver->updateStructure(newVertices, eset); }
bool SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset) { clearIndexMapping(); _activeVertices.clear(); _activeEdges.clear(); _activeEdges.reserve(eset.size()); set<Vertex*> auxVertexSet; // temporary structure to avoid duplicates for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end(); it++) { OptimizableGraph::Edge* e=(OptimizableGraph::Edge*)(*it); for (vector<HyperGraph::Vertex*>::const_iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) auxVertexSet.insert(static_cast<OptimizableGraph::Vertex*>(*vit)); _activeEdges.push_back(reinterpret_cast<OptimizableGraph::Edge*>(*it)); } _activeVertices.reserve(auxVertexSet.size()); for (set<Vertex*>::iterator it = auxVertexSet.begin(); it != auxVertexSet.end(); ++it) _activeVertices.push_back(*it); sortVectorContainers(); return buildIndexMapping(_activeVertices); }
bool SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset){ preIteration(-1); bool workspaceAllocated = _jacobianWorkspace.allocate(); (void) workspaceAllocated; assert(workspaceAllocated && "Error while allocating memory for the Jacobians"); clearIndexMapping(); _activeVertices.clear(); _activeEdges.clear(); _activeEdges.reserve(eset.size()); set<Vertex*> auxVertexSet; // temporary structure to avoid duplicates for (HyperGraph::EdgeSet::iterator it=eset.begin(); it!=eset.end(); ++it){ OptimizableGraph::Edge* e=(OptimizableGraph::Edge*)(*it); if (e->numUndefinedVertices()) continue; for (vector<HyperGraph::Vertex*>::const_iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) { auxVertexSet.insert(static_cast<OptimizableGraph::Vertex*>(*vit)); } _activeEdges.push_back(reinterpret_cast<OptimizableGraph::Edge*>(*it)); } _activeVertices.reserve(auxVertexSet.size()); for (set<Vertex*>::iterator it = auxVertexSet.begin(); it != auxVertexSet.end(); ++it) _activeVertices.push_back(*it); sortVectorContainers(); bool indexMappingStatus = buildIndexMapping(_activeVertices); postIteration(-1); return indexMappingStatus; }
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; }
int main(int argc, char** argv) { CommandArgs arg; std::string outputFilename; std::string inputFilename; arg.param("o", outputFilename, "", "output file name"); arg.paramLeftOver("input-filename ", inputFilename, "", "graph file to read", true); arg.parseArgs(argc, argv); OptimizableGraph graph; if (!graph.load(inputFilename.c_str())){ cerr << "Error: cannot load a file from \"" << inputFilename << "\", aborting." << endl; return 0; } HyperGraph::EdgeSet removedEdges; HyperGraph::VertexSet removedVertices; for (HyperGraph::EdgeSet::iterator it = graph.edges().begin(); it!=graph.edges().end(); it++) { HyperGraph::Edge* e = *it; EdgeSE2PointXY* edgePointXY = dynamic_cast<EdgeSE2PointXY*>(e); if (edgePointXY) { VertexSE2* pose = dynamic_cast<VertexSE2*>(edgePointXY->vertex(0)); VertexPointXY* landmark = dynamic_cast<VertexPointXY*>(edgePointXY->vertex(1)); FeaturePointXYData * feature = new FeaturePointXYData(); feature->setPositionMeasurement(edgePointXY->measurement()); feature->setPositionInformation(edgePointXY->information()); pose->addUserData(feature); removedEdges.insert(edgePointXY); removedVertices.insert(landmark); } } for (HyperGraph::EdgeSet::iterator it = removedEdges.begin(); it!=removedEdges.end(); it++){ OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(*it); graph.removeEdge(e); } for (HyperGraph::VertexSet::iterator it = removedVertices.begin(); it!=removedVertices.end(); it++){ OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it); graph.removeVertex(v); } if (outputFilename.length()){ graph.save(outputFilename.c_str()); } }
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 assignHierarchicalEdges(StarSet& stars, EdgeStarMap& esmap, EdgeLabeler* labeler, EdgeCreator* creator, SparseOptimizer* optimizer, int minNumEdges, int maxIterations){ // now construct the hierarchical edges for all the stars int starNum=0; for (StarSet::iterator it=stars.begin(); it!=stars.end(); it++){ cerr << "STAR# " << starNum << endl; Star* s=*it; std::vector<OptimizableGraph::Vertex*> vertices(2); vertices[0]= (OptimizableGraph::Vertex*) *s->_gauge.begin(); cerr << "eIs" << endl; HyperGraph::VertexSet vNew =s->lowLevelVertices(); for (HyperGraph::VertexSet::iterator vit=s->_lowLevelVertices.begin(); vit!=s->_lowLevelVertices.end(); vit++){ OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)*vit; vertices[1]=v; if (v==vertices[0]) continue; HyperGraph::EdgeSet eInSt; int numEdges = vertexEdgesInStar(eInSt, v, s, esmap); if (Factory::instance()->tag(v)==Factory::instance()->tag(vertices[0]) || numEdges>minNumEdges) { OptimizableGraph::Edge* e=creator->createEdge(vertices); //cerr << "creating edge" << e << endl; if (e) { e->setLevel(1); optimizer->addEdge(e); s->_starEdges.insert(e); } else { cerr << "THERE" << endl; cerr << "FATAL, cannot create edge" << endl; } } else { vNew.erase(v); // cerr << numEdges << " "; // cerr << "r " << v-> id() << endl; // remove from the star all edges that are not sufficiently connected for (HyperGraph::EdgeSet::iterator it=eInSt.begin(); it!=eInSt.end(); it++){ HyperGraph::Edge* e=*it; s->lowLevelEdges().erase(e); } } } s->lowLevelVertices()=vNew; //cerr << endl; cerr << "gauge: " << (*s->_gauge.begin())->id() << " edges:" << s->_lowLevelEdges.size() << " hedges" << s->_starEdges.size() << endl; const bool debug = false; if (debug){ char starLowName[100]; sprintf(starLowName, "star-%04d-low.g2o", starNum); ofstream starLowStream(starLowName); optimizer->saveSubset(starLowStream, s->_lowLevelEdges); } bool labelOk=s->labelStarEdges(maxIterations, labeler); if (labelOk) { if (debug) { char starHighName[100]; sprintf(starHighName, "star-%04d-high.g2o", starNum); ofstream starHighStream(starHighName); optimizer->saveSubset(starHighStream, s->_starEdges); } } else { cerr << "FAILURE" << endl; } starNum++; } }
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; }