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 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 SparseOptimizer::initializeOptimization(HyperGraph::VertexSet& vset, int level){
if (edges().size() == 0) {
cerr << __PRETTY_FUNCTION__ << ": Attempt to initialize an empty graph" << endl;
return false;
}
bool workspaceAllocated = _jacobianWorkspace.allocate(); (void) workspaceAllocated;
assert(workspaceAllocated && "Error while allocating memory for the Jacobians");
clearIndexMapping();
_activeVertices.clear();
_activeVertices.reserve(vset.size());
_activeEdges.clear();
set<Edge*> auxEdgeSet; // temporary structure to avoid duplicates
for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it){
OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*) *it;
const OptimizableGraph::EdgeSet& vEdges=v->edges();
// count if there are edges in that level. If not remove from the pool
int levelEdges=0;
for (OptimizableGraph::EdgeSet::const_iterator it=vEdges.begin(); it!=vEdges.end(); ++it){
OptimizableGraph::Edge* e=reinterpret_cast<OptimizableGraph::Edge*>(*it);
if (level < 0 || e->level() == level) {
bool allVerticesOK = true;
for (vector<HyperGraph::Vertex*>::const_iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) {
if (vset.find(*vit) == vset.end()) {
allVerticesOK = false;
break;
}
}
if (allVerticesOK && !e->allVerticesFixed()) {
auxEdgeSet.insert(e);
levelEdges++;
}
}
}
if (levelEdges){
_activeVertices.push_back(v);
// test for NANs in the current estimate if we are debugging
# ifndef NDEBUG
int estimateDim = v->estimateDimension();
if (estimateDim > 0) {
Eigen::VectorXd estimateData(estimateDim);
if (v->getEstimateData(estimateData.data()) == true) {
int k;
bool hasNan = arrayHasNaN(estimateData.data(), estimateDim, &k);
if (hasNan)
cerr << __PRETTY_FUNCTION__ << ": Vertex " << v->id() << " contains a nan entry at index " << k << endl;
}
}
# endif
}
}
_activeEdges.reserve(auxEdgeSet.size());
for (set<Edge*>::iterator it = auxEdgeSet.begin(); it != auxEdgeSet.end(); ++it)
_activeEdges.push_back(*it);
sortVectorContainers();
return buildIndexMapping(_activeVertices);
}
bool SparseOptimizer::initializeOptimization
(HyperGraph::VertexSet& vset, int level)
{
// Recorre todos los vertices introducidos en el optimizador.
// Para cada vertice 'V' obtiene los edges de los que forma parte.
// Para cada uno de esos edges, se mira si todos sus vertices estan en el
// optimizador. Si lo estan, el edge se aniade a _activeEdges.
// Si el vertice 'V' tiene algun edge con todos los demas vertices en el
// optimizador, se aniade 'V' a _activeVertices
// Al final se asignan unos indices internos para los vertices:
// -1: vertices fijos
// 0..n: vertices no fijos y NO marginalizables
// n+1..m: vertices no fijos y marginalizables
clearIndexMapping();
_activeVertices.clear();
_activeVertices.reserve(vset.size());
_activeEdges.clear();
set<Edge*> auxEdgeSet; // temporary structure to avoid duplicates
for (HyperGraph::VertexSet::iterator
it = vset.begin();
it != vset.end();
it++)
{
OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*) *it;
const OptimizableGraph::EdgeSet& vEdges=v->edges();
// count if there are edges in that level. If not remove from the pool
int levelEdges=0;
for (OptimizableGraph::EdgeSet::const_iterator
it = vEdges.begin();
it != vEdges.end();
it++)
{
OptimizableGraph::Edge* e =
reinterpret_cast<OptimizableGraph::Edge*>(*it);
if (level < 0 || e->level() == level)
{
bool allVerticesOK = true;
for (vector<HyperGraph::Vertex*>::const_iterator
vit = e->vertices().begin();
vit != e->vertices().end();
++vit)
{
if (vset.find(*vit) == vset.end())
{
allVerticesOK = false;
break;
}
}
if (allVerticesOK)
{
auxEdgeSet.insert(reinterpret_cast<OptimizableGraph::Edge*>(*it));
levelEdges++;
}
}
}
if (levelEdges) _activeVertices.push_back(v);
}
_activeEdges.reserve(auxEdgeSet.size());
for (set<Edge*>::iterator
it = auxEdgeSet.begin();
it != auxEdgeSet.end();
++it)
_activeEdges.push_back(*it);
sortVectorContainers();
return buildIndexMapping(_activeVertices);
}
bool SolverSLAM2DLinear::solveOrientation()
{
assert(_optimizer->indexMapping().size() + 1 == _optimizer->vertices().size() && "Needs to operate on full graph");
assert(_optimizer->vertex(0)->fixed() && "Graph is not fixed by vertex 0");
VectorXD b, x; // will be used for theta and x/y update
b.setZero(_optimizer->indexMapping().size());
x.setZero(_optimizer->indexMapping().size());
typedef Eigen::Matrix<double, 1, 1, Eigen::ColMajor> ScalarMatrix;
ScopedArray<int> blockIndeces(new int[_optimizer->indexMapping().size()]);
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i)
blockIndeces[i] = i+1;
SparseBlockMatrix<ScalarMatrix> H(blockIndeces.get(), blockIndeces.get(), _optimizer->indexMapping().size(), _optimizer->indexMapping().size());
// building the structure, diagonal for each active vertex
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
int poseIdx = v->hessianIndex();
ScalarMatrix* m = H.block(poseIdx, poseIdx, true);
m->setZero();
}
HyperGraph::VertexSet fixedSet;
// off diagonal for each edge
for (SparseOptimizer::EdgeContainer::const_iterator it = _optimizer->activeEdges().begin(); it != _optimizer->activeEdges().end(); ++it) {
# ifndef NDEBUG
EdgeSE2* e = dynamic_cast<EdgeSE2*>(*it);
assert(e && "Active edges contain non-odometry edge"); //
# else
EdgeSE2* e = static_cast<EdgeSE2*>(*it);
# endif
OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
int ind1 = from->hessianIndex();
int ind2 = to->hessianIndex();
if (ind1 == -1 || ind2 == -1) {
if (ind1 == -1) fixedSet.insert(from); // collect the fixed vertices
if (ind2 == -1) fixedSet.insert(to);
continue;
}
bool transposedBlock = ind1 > ind2;
if (transposedBlock){ // make sure, we allocate the upper triangle block
std::swap(ind1, ind2);
}
ScalarMatrix* m = H.block(ind1, ind2, true);
m->setZero();
}
// walk along the Minimal Spanning Tree to compute the guess for the robot orientation
assert(fixedSet.size() == 1);
VertexSE2* root = static_cast<VertexSE2*>(*fixedSet.begin());
VectorXD thetaGuess;
thetaGuess.setZero(_optimizer->indexMapping().size());
UniformCostFunction uniformCost;
HyperDijkstra hyperDijkstra(_optimizer);
hyperDijkstra.shortestPaths(root, &uniformCost);
HyperDijkstra::computeTree(hyperDijkstra.adjacencyMap());
ThetaTreeAction thetaTreeAction(thetaGuess.data());
HyperDijkstra::visitAdjacencyMap(hyperDijkstra.adjacencyMap(), &thetaTreeAction);
// construct for the orientation
for (SparseOptimizer::EdgeContainer::const_iterator it = _optimizer->activeEdges().begin(); it != _optimizer->activeEdges().end(); ++it) {
EdgeSE2* e = static_cast<EdgeSE2*>(*it);
VertexSE2* from = static_cast<VertexSE2*>(e->vertices()[0]);
VertexSE2* to = static_cast<VertexSE2*>(e->vertices()[1]);
double omega = e->information()(2,2);
double fromThetaGuess = from->hessianIndex() < 0 ? 0. : thetaGuess[from->hessianIndex()];
double toThetaGuess = to->hessianIndex() < 0 ? 0. : thetaGuess[to->hessianIndex()];
double error = normalize_theta(-e->measurement().rotation().angle() + toThetaGuess - fromThetaGuess);
bool fromNotFixed = !(from->fixed());
bool toNotFixed = !(to->fixed());
if (fromNotFixed || toNotFixed) {
double omega_r = - omega * error;
if (fromNotFixed) {
b(from->hessianIndex()) -= omega_r;
(*H.block(from->hessianIndex(), from->hessianIndex()))(0,0) += omega;
if (toNotFixed) {
if (from->hessianIndex() > to->hessianIndex())
(*H.block(to->hessianIndex(), from->hessianIndex()))(0,0) -= omega;
else
(*H.block(from->hessianIndex(), to->hessianIndex()))(0,0) -= omega;
}
}
if (toNotFixed ) {
b(to->hessianIndex()) += omega_r;
(*H.block(to->hessianIndex(), to->hessianIndex()))(0,0) += omega;
}
}
}
// solve orientation
typedef LinearSolverCSparse<ScalarMatrix> SystemSolver;
SystemSolver linearSystemSolver;
linearSystemSolver.init();
bool ok = linearSystemSolver.solve(H, x.data(), b.data());
if (!ok) {
cerr << __PRETTY_FUNCTION__ << "Failure while solving linear system" << endl;
return false;
}
// update the orientation of the 2D poses and set translation to 0, GN shall solve that
root->setToOrigin();
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
VertexSE2* v = static_cast<VertexSE2*>(_optimizer->indexMapping()[i]);
int poseIdx = v->hessianIndex();
SE2 poseUpdate(0, 0, normalize_theta(thetaGuess(poseIdx) + x(poseIdx)));
v->setEstimate(poseUpdate);
}
return true;
}