void OptimizableGraph::pop(HyperGraph::VertexSet& vset)
{
for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
v->pop();
}
}
void OptimizableGraph::pop() {
for (OptimizableGraph::VertexIDMap::iterator it = _vertices.begin();
it != _vertices.end(); it++) {
OptimizableGraph::Vertex* v =
static_cast<OptimizableGraph::Vertex*>(it->second);
v->pop();
}
}
void SparseOptimizer::pop(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->pop();
else
cerr << "FATAL POP SET" << 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;
}