void OptimizableGraph::push(HyperGraph::VertexSet& vset)
{
for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
v->push();
}
}
void OptimizableGraph::push()
{
for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
v->push();
}
}
void SparseOptimizer::push(HyperGraph::VertexSet& vlist)
{
for (HyperGraph::VertexSet::iterator it = vlist.begin(); it != vlist.end(); ++it) {
OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
if (v)
v->push();
else
cerr << __FUNCTION__ << ": FATAL PUSH SET" << endl;
}
}
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;
}
}
void BaseMultiEdge<D, E>::linearizeOplus()
{
#ifdef G2O_OPENMP
for (size_t i = 0; i < _vertices.size(); ++i) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
v->lockQuadraticForm();
}
#endif
const double delta = 1e-9;
const double scalar = 1.0 / (2*delta);
ErrorVector errorBak;
ErrorVector errorBeforeNumeric = _error;
for (size_t i = 0; i < _vertices.size(); ++i) {
//Xi - estimate the jacobian numerically
OptimizableGraph::Vertex* vi = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
if (vi->fixed())
continue;
const int vi_dim = vi->dimension();
#ifdef _MSC_VER
double* add_vi = new double[vi_dim];
#else
double add_vi[vi_dim];
#endif
std::fill(add_vi, add_vi + vi_dim, 0.0);
if (_jacobianOplus[i].rows() != _dimension || _jacobianOplus[i].cols() != vi_dim)
_jacobianOplus[i].resize(_dimension, vi_dim);
// add small step along the unit vector in each dimension
for (int d = 0; d < vi_dim; ++d) {
vi->push();
add_vi[d] = delta;
vi->oplus(add_vi);
computeError();
errorBak = _error;
vi->pop();
vi->push();
add_vi[d] = -delta;
vi->oplus(add_vi);
computeError();
errorBak -= _error;
vi->pop();
add_vi[d] = 0.0;
_jacobianOplus[i].col(d) = scalar * errorBak;
} // end dimension
#ifdef _MSC_VER
delete[] add_vi;
#endif
}
_error = errorBeforeNumeric;
#ifdef G2O_OPENMP
for (int i = (int)(_vertices.size()) - 1; i >= 0; --i) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
v->unlockQuadraticForm();
}
#endif
}
bool Star::labelStarEdges(int iterations, EdgeLabeler* labeler){
// mark all vertices in the lowLevelEdges as floating
bool ok=true;
std::set<OptimizableGraph::Vertex*> vset;
for (HyperGraph::EdgeSet::iterator it=_lowLevelEdges.begin(); it!=_lowLevelEdges.end(); it++){
HyperGraph::Edge* e=*it;
for (size_t i=0; i<e->vertices().size(); i++){
OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)e->vertices()[i];
v->setFixed(false);
vset.insert(v);
}
}
for (std::set<OptimizableGraph::Vertex*>::iterator it=vset.begin(); it!=vset.end(); it++){
OptimizableGraph::Vertex* v = *it;
v->push();
}
// fix all vertices in the gauge
//cerr << "fixing gauge: ";
for (HyperGraph::VertexSet::iterator it = _gauge.begin(); it!=_gauge.end(); it++){
OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)*it;
//cerr << v->id() << " ";
v->setFixed(true);
}
//cerr << endl;
if (iterations>0){
_optimizer->initializeOptimization(_lowLevelEdges);
_optimizer->computeInitialGuess();
int result=_optimizer->optimize(iterations);
if (result<1){
cerr << "Vertices num: " << _optimizer->activeVertices().size() << "ids: ";
for (size_t i=0; i<_optimizer->indexMapping().size(); i++){
cerr << _optimizer->indexMapping()[i]->id() << " " ;
}
cerr << endl;
cerr << "!!! optimization failure" << endl;
cerr << "star size=" << _lowLevelEdges.size() << endl;
cerr << "gauge: ";
for (HyperGraph::VertexSet::iterator it=_gauge.begin(); it!=_gauge.end(); it++){
OptimizableGraph::Vertex* v = (OptimizableGraph::Vertex*)*it;
cerr << "[" << v->id() << " " << v->hessianIndex() << "] ";
}
cerr << endl;
ok=false;
}
} else {
optimizer()->initializeOptimization(_lowLevelEdges);
// cerr << "guess" << endl;
//optimizer()->computeInitialGuess();
// cerr << "solver init" << endl;
optimizer()->solver()->init();
// cerr << "structure" << endl;
OptimizationAlgorithmWithHessian* solverWithHessian = dynamic_cast<OptimizationAlgorithmWithHessian*> (optimizer()->solver());
if (!solverWithHessian->buildLinearStructure())
cerr << "FATAL: failure while building linear structure" << endl;
// cerr << "errors" << endl;
optimizer()->computeActiveErrors();
// cerr << "system" << endl;
solverWithHessian->updateLinearSystem();
}
std::set<OptimizableGraph::Edge*> star;
for(HyperGraph::EdgeSet::iterator it=_starEdges.begin(); it!=_starEdges.end(); it++){
star.insert((OptimizableGraph::Edge*)*it);
}
if (ok) {
int result = labeler->labelEdges(star);
if (result < 0)
ok=false;
}
// release all vertices in the gauge
for (std::set<OptimizableGraph::Vertex*>::iterator it=vset.begin(); it!=vset.end(); it++){
OptimizableGraph::Vertex* v = *it;
v->pop();
}
for (HyperGraph::VertexSet::iterator it=_gauge.begin(); it!=_gauge.end(); it++){
OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)*it;
v->setFixed(false);
}
return ok;
}