void GraphSLAM::addNeighboringVertices(OptimizableGraph::VertexSet& vset, int gap){
OptimizableGraph::VertexSet temp = vset;
for (OptimizableGraph::VertexSet::iterator it = temp.begin(); it!=temp.end(); it++){
OptimizableGraph::Vertex* vertex = (OptimizableGraph::Vertex*) *it;
for (int i = 1; i <= gap; i++){
OptimizableGraph::Vertex *v = (OptimizableGraph::Vertex *) _graph->vertex(vertex->id()+i);
if (v && v->id() != _lastVertex->id()){
OptimizableGraph::VertexSet::iterator itv = vset.find(v);
if (itv == vset.end())
vset.insert(v);
else
break;
}
}
for (int i = 1; i <= gap; i++){
OptimizableGraph::Vertex* v = (OptimizableGraph::Vertex*) _graph->vertex(vertex->id()-i);
if (v && v->id() != _lastVertex->id()){
OptimizableGraph::VertexSet::iterator itv = vset.find(v);
if (itv == vset.end())
vset.insert(v);
else
break;
}
}
}
}
bool ScanMatcher::globalMatching(OptimizableGraph::VertexSet& referenceVset, OptimizableGraph::Vertex* _referenceVertex, OptimizableGraph::Vertex* _currentVertex, SE2 *trel, double maxScore){
OptimizableGraph::VertexSet vset;
vset.insert(_currentVertex);
return globalMatching(referenceVset, _referenceVertex, vset, _currentVertex, trel, maxScore);
}
bool ScanMatcher::scanMatchingLC(OptimizableGraph::VertexSet& referenceVset, OptimizableGraph::Vertex* _referenceVertex, OptimizableGraph::Vertex* _currentVertex, std::vector<SE2>& trel, double maxScore){
OptimizableGraph::VertexSet currvset;
currvset.insert(_currentVertex);
return scanMatchingLC(referenceVset, _referenceVertex, currvset, _currentVertex, trel, maxScore);
//return scanMatchingLChierarchical(referenceVset, _originVertex, currvset, _currentVertex, trel, maxScore);
}
void EstimatePropagator::propagate(OptimizableGraph::Vertex* v,
const EstimatePropagator::PropagateCost& cost,
const EstimatePropagator::PropagateAction& action,
double maxDistance,
double maxEdgeCost)
{
OptimizableGraph::VertexSet vset;
vset.insert(v);
propagate(vset, cost, action, maxDistance, maxEdgeCost);
}
void computeSimpleStars(StarSet& stars,
SparseOptimizer* optimizer,
EdgeLabeler* labeler,
EdgeCreator* creator,
OptimizableGraph::Vertex* gauge_,
std::string edgeTag,
std::string vertexTag,
int level,
int step,
int backboneIterations,
int starIterations,
double rejectionThreshold,
bool debug){
cerr << "preforming the tree actions" << endl;
HyperDijkstra d(optimizer);
// compute a spanning tree based on the types of edges and vertices in the pool
EdgeTypesCostFunction f(edgeTag, vertexTag, level);
d.shortestPaths(gauge_,
&f,
std::numeric_limits< double >::max(),
1e-6,
false,
std::numeric_limits< double >::max()/2);
HyperDijkstra::computeTree(d.adjacencyMap());
// constructs the stars on the backbone
BackBoneTreeAction bact(optimizer, vertexTag, level, step);
bact.init();
cerr << "free edges size " << bact.freeEdges().size() << endl;
// perform breadth-first visit of the visit tree and create the stars on the backbone
d.visitAdjacencyMap(d.adjacencyMap(),&bact,true);
stars.clear();
for (VertexStarMultimap::iterator it=bact.vertexStarMultiMap().begin();
it!=bact.vertexStarMultiMap().end(); it++){
stars.insert(it->second);
}
cerr << "stars.size: " << stars.size() << endl;
cerr << "size: " << bact.vertexStarMultiMap().size() << endl;
// for each star
// for all vertices in the backbone, select all edges leading/leaving from that vertex
// that are contained in freeEdges.
// mark the corresponding "open" vertices and add them to a multimap (vertex->star)
// select a gauge in the backbone
// push all vertices on the backbone
// compute an initial guess on the backbone
// one round of optimization backbone
// lock all vertices in the backbone
// push all "open" vertices
// for each open vertex,
// compute an initial guess given the backbone
// do some rounds of solveDirect
// if (fail)
// - remove the vertex and the edges in that vertex from the star
// - make the structures consistent
// pop all "open" vertices
// pop all "vertices" in the backbone
// unfix the vertices in the backbone
int starNum=0;
for (StarSet::iterator it=stars.begin(); it!=stars.end(); it++){
Star* s =*it;
HyperGraph::VertexSet backboneVertices = s->_lowLevelVertices;
HyperGraph::EdgeSet backboneEdges = s->_lowLevelEdges;
if (backboneEdges.empty())
continue;
// cerr << "optimizing backbone" << endl;
// one of these should be the gauge, to be simple we select the fisrt one in the backbone
OptimizableGraph::VertexSet gauge;
gauge.insert(*backboneVertices.begin());
s->gauge()=gauge;
s->optimizer()->push(backboneVertices);
s->optimizer()->setFixed(gauge,true);
s->optimizer()->initializeOptimization(backboneEdges);
s->optimizer()->computeInitialGuess();
s->optimizer()->optimize(backboneIterations);
s->optimizer()->setFixed(backboneVertices, true);
// cerr << "assignind edges.vertices not in bbone" << endl;
HyperGraph::EdgeSet otherEdges;
HyperGraph::VertexSet otherVertices;
std::multimap<HyperGraph::Vertex*, HyperGraph::Edge*> vemap;
for (HyperGraph::VertexSet::iterator bit=backboneVertices.begin(); bit!=backboneVertices.end(); bit++){
HyperGraph::Vertex* v=*bit;
for (HyperGraph::EdgeSet::iterator eit=v->edges().begin(); eit!=v->edges().end(); eit++){
OptimizableGraph::Edge* e = (OptimizableGraph::Edge*) *eit;
HyperGraph::EdgeSet::iterator feit=bact.freeEdges().find(e);
if (feit!=bact.freeEdges().end()){ // edge is admissible
otherEdges.insert(e);
bact.freeEdges().erase(feit);
for (size_t i=0; i<e->vertices().size(); i++){
OptimizableGraph::Vertex* ve= (OptimizableGraph::Vertex*)e->vertices()[i];
if (backboneVertices.find(ve)==backboneVertices.end()){
otherVertices.insert(ve);
vemap.insert(make_pair(ve,e));
}
}
}
}
}
// RAINER TODO maybe need a better solution than dynamic casting here??
OptimizationAlgorithmWithHessian* solverWithHessian = dynamic_cast<OptimizationAlgorithmWithHessian*>(s->optimizer()->solver());
if (solverWithHessian) {
s->optimizer()->push(otherVertices);
// cerr << "optimizing vertices out of bbone" << endl;
// cerr << "push" << endl;
// cerr << "init" << endl;
s->optimizer()->initializeOptimization(otherEdges);
// cerr << "guess" << endl;
s->optimizer()->computeInitialGuess();
// cerr << "solver init" << endl;
s->optimizer()->solver()->init();
// cerr << "structure" << endl;
if (!solverWithHessian->buildLinearStructure())
cerr << "FATAL: failure while building linear structure" << endl;
// cerr << "errors" << endl;
s->optimizer()->computeActiveErrors();
// cerr << "system" << endl;
solverWithHessian->updateLinearSystem();
// cerr << "directSolove" << endl;
} else {
cerr << "FATAL: hierarchical thing cannot be used with a solver that does not support the system structure construction" << endl;
}
// // then optimize the vertices one at a time to check if a solution is good
for (HyperGraph::VertexSet::iterator vit=otherVertices.begin(); vit!=otherVertices.end(); vit++){
OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)(*vit);
v->solveDirect();
// cerr << " " << d;
// if a solution is found, add a vertex and all the edges in
//othervertices that are pointing to that edge to the star
s->_lowLevelVertices.insert(v);
for (HyperGraph::EdgeSet::iterator eit=v->edges().begin(); eit!=v->edges().end(); eit++){
OptimizableGraph::Edge* e = (OptimizableGraph::Edge*) *eit;
if (otherEdges.find(e)!=otherEdges.end())
s->_lowLevelEdges.insert(e);
}
}
//cerr << endl;
// relax the backbone and optimize it all
// cerr << "relax bbone" << endl;
s->optimizer()->setFixed(backboneVertices, false);
//cerr << "fox gauge bbone" << endl;
s->optimizer()->setFixed(s->gauge(),true);
//cerr << "opt init" << endl;
s->optimizer()->initializeOptimization(s->_lowLevelEdges);
optimizer->computeActiveErrors();
double initialChi = optimizer->activeChi2();
int starOptResult = s->optimizer()->optimize(starIterations);
//cerr << starOptResult << "(" << starIterations << ") " << endl;
double finalchi=-1.;
cerr << "computing star: " << starNum << endl;
int vKept=0, vDropped=0;
if (!starIterations || starOptResult > 0 ){
optimizer->computeActiveErrors();
finalchi = optimizer->activeChi2();
#if 1
s->optimizer()->computeActiveErrors();
// cerr << "system" << endl;
if (solverWithHessian)
solverWithHessian->updateLinearSystem();
HyperGraph::EdgeSet prunedStarEdges = backboneEdges;
HyperGraph::VertexSet prunedStarVertices = backboneVertices;
for (HyperGraph::VertexSet::iterator vit=otherVertices.begin(); vit!=otherVertices.end(); vit++){
//discard the vertices whose error is too big
OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)(*vit);
MatrixXd h(v->dimension(), v->dimension());
for (int i=0; i<v->dimension(); i++){
for (int j=0; j<v->dimension(); j++)
h(i,j)=v->hessian(i,j);
}
EigenSolver<Eigen::MatrixXd> esolver;
esolver.compute(h);
VectorXcd ev= esolver.eigenvalues();
double emin = std::numeric_limits<double>::max();
double emax = -std::numeric_limits<double>::max();
for (int i=0; i<ev.size(); i++){
emin = ev(i).real()>emin ? emin : ev(i).real();
emax = ev(i).real()<emax ? emax : ev(i).real();
}
double d=emin/emax;
// cerr << " " << d;
if (d>rejectionThreshold){
// if a solution is found, add a vertex and all the edges in
//othervertices that are pointing to that edge to the star
prunedStarVertices.insert(v);
for (HyperGraph::EdgeSet::iterator eit=v->edges().begin(); eit!=v->edges().end(); eit++){
OptimizableGraph::Edge* e = (OptimizableGraph::Edge*) *eit;
if (otherEdges.find(e)!=otherEdges.end())
prunedStarEdges.insert(e);
}
//cerr << "K( " << v->id() << "," << d << ")" ;
vKept ++;
} else {
vDropped++;
//cerr << "R( " << v->id() << "," << d << ")" ;
}
}
s->_lowLevelEdges=prunedStarEdges;
s->_lowLevelVertices=prunedStarVertices;
#endif
//cerr << "addHedges" << endl;
//now add to the star the hierarchical edges
std::vector<OptimizableGraph::Vertex*> vertices(2);
vertices[0]= (OptimizableGraph::Vertex*) *s->_gauge.begin();
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;
OptimizableGraph::Edge* e=creator->createEdge(vertices);
//rr << "creating edge" << e << Factory::instance()->tag(vertices[0]) << "->" << Factory::instance()->tag(v) <endl;
if (e) {
e->setLevel(level+1);
optimizer->addEdge(e);
s->_starEdges.insert(e);
} else {
cerr << "HERE" << endl;
cerr << "FATAL, cannot create edge" << endl;
}
}
}
cerr << " gauge: " << (*s->_gauge.begin())->id()
<< " kept: " << vKept
<< " dropped: " << vDropped
<< " edges:" << s->_lowLevelEdges.size()
<< " hedges" << s->_starEdges.size()
<< " initial chi " << initialChi
<< " final chi " << finalchi << endl;
if (debug) {
char starLowName[100];
sprintf(starLowName, "star-%04d-low.g2o", starNum);
ofstream starLowStream(starLowName);
optimizer->saveSubset(starLowStream, s->_lowLevelEdges);
}
bool labelOk=false;
if (!starIterations || starOptResult > 0)
labelOk = s->labelStarEdges(0, 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: " << starOptResult << endl;
}
starNum++;
//label each hierarchical edge
s->optimizer()->pop(otherVertices);
s->optimizer()->pop(backboneVertices);
s->optimizer()->setFixed(s->gauge(),false);
}
StarSet stars2;
// now erase the stars that have 0 edges. They r useless
for (StarSet::iterator it=stars.begin(); it!=stars.end(); it++){
Star* s=*it;
if (s->lowLevelEdges().size()==0) {
delete s;
} else
stars2.insert(s);
}
stars=stars2;
}
void EstimatePropagator::propagate(OptimizableGraph::VertexSet& vset,
const EstimatePropagator::PropagateCost& cost,
const EstimatePropagator::PropagateAction& action,
double maxDistance,
double maxEdgeCost)
{
reset();
PriorityQueue frontier;
for (OptimizableGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit);
AdjacencyMap::iterator it = _adjacencyMap.find(v);
assert(it != _adjacencyMap.end());
it->second._distance = 0.;
it->second._parent.clear();
it->second._frontierLevel = 0;
frontier.push(&it->second);
}
while(! frontier.empty()){
AdjacencyMapEntry* entry = frontier.pop();
OptimizableGraph::Vertex* u = entry->child();
double uDistance = entry->distance();
//cerr << "uDistance " << uDistance << endl;
// initialize the vertex
if (entry->_frontierLevel > 0) {
action(entry->edge(), entry->parent(), u);
}
/* std::pair< OptimizableGraph::VertexSet::iterator, bool> insertResult = */ _visited.insert(u);
OptimizableGraph::EdgeSet::iterator et = u->edges().begin();
while (et != u->edges().end()){
OptimizableGraph::Edge* edge = static_cast<OptimizableGraph::Edge*>(*et);
++et;
int maxFrontier = -1;
OptimizableGraph::VertexSet initializedVertices;
for (size_t i = 0; i < edge->vertices().size(); ++i) {
OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
AdjacencyMap::iterator ot = _adjacencyMap.find(z);
if (ot->second._distance != numeric_limits<double>::max()) {
initializedVertices.insert(z);
maxFrontier = (max)(maxFrontier, ot->second._frontierLevel);
}
}
assert(maxFrontier >= 0);
for (size_t i = 0; i < edge->vertices().size(); ++i) {
OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
if (z == u)
continue;
size_t wasInitialized = initializedVertices.erase(z);
double edgeDistance = cost(edge, initializedVertices, z);
if (edgeDistance > 0. && edgeDistance != std::numeric_limits<double>::max() && edgeDistance < maxEdgeCost) {
double zDistance = uDistance + edgeDistance;
//cerr << z->id() << " " << zDistance << endl;
AdjacencyMap::iterator ot = _adjacencyMap.find(z);
assert(ot!=_adjacencyMap.end());
if (zDistance < ot->second.distance() && zDistance < maxDistance){
//if (ot->second.inQueue)
//cerr << "Updating" << endl;
ot->second._distance = zDistance;
ot->second._parent = initializedVertices;
ot->second._edge = edge;
ot->second._frontierLevel = maxFrontier + 1;
frontier.push(&ot->second);
}
}
if (wasInitialized > 0)
initializedVertices.insert(z);
}
}
}
// writing debug information like cost for reaching each vertex and the parent used to initialize
#ifdef DEBUG_ESTIMATE_PROPAGATOR
cerr << "Writing cost.dat" << endl;
ofstream costStream("cost.dat");
for (AdjacencyMap::const_iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {
HyperGraph::Vertex* u = it->second.child();
costStream << "vertex " << u->id() << " cost " << it->second._distance << endl;
}
cerr << "Writing init.dat" << endl;
ofstream initStream("init.dat");
vector<AdjacencyMapEntry*> frontierLevels;
for (AdjacencyMap::iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {
if (it->second._frontierLevel > 0)
frontierLevels.push_back(&it->second);
}
sort(frontierLevels.begin(), frontierLevels.end(), FrontierLevelCmp());
for (vector<AdjacencyMapEntry*>::const_iterator it = frontierLevels.begin(); it != frontierLevels.end(); ++it) {
AdjacencyMapEntry* entry = *it;
OptimizableGraph::Vertex* to = entry->child();
initStream << "calling init level = " << entry->_frontierLevel << "\t (";
for (OptimizableGraph::VertexSet::iterator pit = entry->parent().begin(); pit != entry->parent().end(); ++pit) {
initStream << " " << (*pit)->id();
}
initStream << " ) -> " << to->id() << endl;
}
#endif
}
void GraphSLAM::addDataSM(SE2 currentOdom, RobotLaser* laser){
boost::mutex::scoped_lock lockg(graphMutex);
//Add current vertex
VertexSE2 *v = new VertexSE2;
SE2 displacement = _lastOdom.inverse() * currentOdom;
SE2 currEst = _lastVertex->estimate() * displacement;
v->setEstimate(currEst);
v->setId(++_runningVertexId + idRobot() * baseId());
//Add covariance information
//VertexEllipse *ellipse = new VertexEllipse;
//Matrix3f cov = Matrix3f::Zero(); //last vertex has zero covariance
//ellipse->setCovariance(cov);
//v->setUserData(ellipse);
v->addUserData(laser);
std::cout << endl <<
"Current vertex: " << v->id() <<
" Estimate: "<< v->estimate().translation().x() <<
" " << v->estimate().translation().y() <<
" " << v->estimate().rotation().angle() << std::endl;
_graph->addVertex(v);
//Add current odometry edge
EdgeSE2 *e = new EdgeSE2;
e->setId(++_runningEdgeId + idRobot() * baseId());
e->vertices()[0] = _lastVertex;
e->vertices()[1] = v;
OptimizableGraph::VertexSet vset;
vset.insert(_lastVertex);
int j = 1;
int gap = 5;
while (j <= gap){
VertexSE2 *vj = dynamic_cast<VertexSE2 *>(graph()->vertex(_lastVertex->id()-j));
if (vj)
vset.insert(vj);
else
break;
j++;
}
SE2 transf;
bool shouldIAdd = _closeMatcher.closeScanMatching(vset, _lastVertex, v, &transf, maxScore);
if (shouldIAdd){
e->setMeasurement(transf);
e->setInformation(_SMinf);
}else{ //Trust the odometry
e->setMeasurement(displacement);
// Vector3d dis = displacement.toVector();
// dis.x() = fabs(dis.x());
// dis.y() = fabs(dis.y());
// dis.z() = fabs(dis.z());
// dis += Vector3d(1e-3,1e-3,1e-2);
// Matrix3d dis2 = dis*dis.transpose();
// Matrix3d newcov = dis2.cwiseProduct(_odomK);
// e->setInformation(newcov.inverse());
e->setInformation(_odominf);
}
_graph->addEdge(e);
_lastOdom = currentOdom;
_lastVertex = v;
}
