int SparseOptimizerIncremental::optimize(int iterations, bool online)
{
(void) iterations; // we only do one iteration anyhow
OptimizationAlgorithm* solver = _algorithm;
solver->init(online);
int cjIterations=0;
bool ok=true;
if (! online || batchStep) {
//cerr << "performing batch step" << endl;
if (! online) {
ok = _underlyingSolver->buildStructure();
if (! ok) {
cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl;
return 0;
}
}
// copy over the updated estimate as new linearization point
if (slamDimension == 3) {
for (size_t i = 0; i < indexMapping().size(); ++i) {
OnlineVertexSE2* v = static_cast<OnlineVertexSE2*>(indexMapping()[i]);
v->setEstimate(v->updatedEstimate);
}
}
else if (slamDimension == 6) {
for (size_t i = 0; i < indexMapping().size(); ++i) {
OnlineVertexSE3* v= static_cast<OnlineVertexSE3*>(indexMapping()[i]);
v->setEstimate(v->updatedEstimate);
}
}
SparseOptimizer::computeActiveErrors();
SparseOptimizer::linearizeSystem();
_underlyingSolver->buildSystem();
int numBlocksRequired = _ivMap.size();
if (_cmember.size() < numBlocksRequired) {
_cmember.resize(2 * numBlocksRequired);
}
memset(_cmember.data(), 0, numBlocksRequired * sizeof(int));
if (_ivMap.size() > 100) {
for (size_t i = _ivMap.size() - 20; i < _ivMap.size(); ++i) {
const HyperGraph::EdgeSet& eset = _ivMap[i]->edges();
for (HyperGraph::EdgeSet::const_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->hessianIndex() >= 0)
_cmember(v1->hessianIndex()) = 1;
if (v2->hessianIndex() >= 0)
_cmember(v2->hessianIndex()) = 1;
}
}
}
ok = _underlyingSolver->solve();
// get the current cholesky factor along with the permutation
_L = _solverInterface->L();
if (_perm.size() < (int)_L->n)
_perm.resize(2*_L->n);
int* p = (int*)_L->Perm;
for (size_t i = 0; i < _L->n; ++i)
_perm[p[i]] = i;
}
else {
// update the b vector
for (HyperGraph::VertexSet::iterator it = _touchedVertices.begin(); it != _touchedVertices.end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
int iBase = v->colInHessian();
v->copyB(_underlyingSolver->b() + iBase);
}
_solverInterface->solve(_underlyingSolver->x(), _underlyingSolver->b());
}
update(_underlyingSolver->x());
++cjIterations;
if (verbose()){
computeActiveErrors();
cerr
<< "nodes = " << vertices().size()
<< "\t edges= " << _activeEdges.size()
<< "\t chi2= " << FIXED(activeChi2())
<< endl;
}
if (vizWithGnuplot)
gnuplotVisualization();
if (! ok)
return 0;
return 1;
}
