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