void GraphSLAM::checkHaveLaser(OptimizableGraph::VertexSet& vset){
OptimizableGraph::VertexSet tmpvset = vset;
for (OptimizableGraph::VertexSet::iterator it = tmpvset.begin(); it != tmpvset.end(); it++){
OptimizableGraph::Vertex *vertex = (OptimizableGraph::Vertex*) *it;
if (!findLaserData(vertex))
vset.erase(*it);
}
}
void GraphSLAM::checkCovariance(OptimizableGraph::VertexSet& vset){
///////////////////////////////////
// we need now to compute the marginal covariances of all other vertices w.r.t the newly inserted one
CovarianceEstimator ce(_graph);
ce.setVertices(vset);
ce.setGauge(_lastVertex);
ce.compute();
assert(!_lastVertex->fixed() && "last Vertex is fixed");
assert(_firstRobotPose->fixed() && "first Vertex is not fixed");
OptimizableGraph::VertexSet tmpvset = vset;
for (OptimizableGraph::VertexSet::iterator it = tmpvset.begin(); it != tmpvset.end(); it++){
VertexSE2 *vertex = (VertexSE2*) *it;
MatrixXd Pv = ce.getCovariance(vertex);
Matrix2d Pxy; Pxy << Pv(0,0), Pv(0,1), Pv(1,0), Pv(1,1);
SE2 delta = vertex->estimate().inverse() * _lastVertex->estimate();
Vector2d hxy (delta.translation().x(), delta.translation().y());
double perceptionRange =1;
if (hxy.x()-perceptionRange>0)
hxy.x() -= perceptionRange;
else if (hxy.x()+perceptionRange<0)
hxy.x() += perceptionRange;
else
hxy.x() = 0;
if (hxy.y()-perceptionRange>0)
hxy.y() -= perceptionRange;
else if (hxy.y()+perceptionRange<0)
hxy.y() += perceptionRange;
else
hxy.y() = 0;
double d2 = hxy.transpose() * Pxy.inverse() * hxy;
if (d2 > 5.99)
vset.erase(*it);
}
}
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
}