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;
}
}
}
}
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 transformPointsFromVSet(OptimizableGraph::VertexSet& vset, OptimizableGraph::Vertex* _referenceVertex, RawLaser::Point2DVector& scansInRefVertex){
VertexSE2* referenceVertex=dynamic_cast<VertexSE2*>(_referenceVertex);
scansInRefVertex.clear();
for (OptimizableGraph::VertexSet::iterator it = vset.begin(); it != vset.end(); it++){
VertexSE2 *vertex = (VertexSE2*) *it;
RobotLaser* laserv = dynamic_cast<RobotLaser*>(vertex->userData());
if (laserv){
RawLaser::Point2DVector vscan = laserv->cartesian();
SE2 trl = laserv->laserParams().laserPose;
RawLaser::Point2DVector scanInRefVertex;
if (vertex->id() == referenceVertex->id()){
ScanMatcher::applyTransfToScan(trl, vscan, scanInRefVertex);
}else{
SE2 trel = referenceVertex->estimate().inverse() * vertex->estimate();
SE2 transf = trel * trl;
ScanMatcher::applyTransfToScan(transf, vscan, scanInRefVertex);
}
scansInRefVertex.insert(scansInRefVertex.end(), scanInRefVertex.begin(), scanInRefVertex.end());
}
}
}
bool ScanMatcher::verifyMatching(OptimizableGraph::VertexSet& vset1, OptimizableGraph::Vertex* _referenceVertex1,
OptimizableGraph::VertexSet& vset2, OptimizableGraph::Vertex* _referenceVertex2,
SE2 trel12, double *score){
VertexSE2* referenceVertex2=dynamic_cast<VertexSE2*>(_referenceVertex2);
resetGrid();
CharGrid auxGrid = _grid;
//Transform points from vset2 in the reference of referenceVertex1 using trel12
RawLaser::Point2DVector scansvset2inref1;
for (OptimizableGraph::VertexSet::iterator it = vset2.begin(); it != vset2.end(); it++){
VertexSE2 *vertex = (VertexSE2*) *it;
RobotLaser* laserv = dynamic_cast<RobotLaser*>(vertex->userData());
RawLaser::Point2DVector vscan = laserv->cartesian();
SE2 trl = laserv->laserParams().laserPose;
RawLaser::Point2DVector scanInRefVertex1;
if (vertex->id() == referenceVertex2->id()){
applyTransfToScan(trel12 * trl, vscan, scanInRefVertex1);
}else{
//Transform scans to the referenceVertex2 coordinates
SE2 tref2_v = referenceVertex2->estimate().inverse() * vertex->estimate();
applyTransfToScan(trel12 * tref2_v * trl, vscan, scanInRefVertex1);
}
scansvset2inref1.insert(scansvset2inref1.end(), scanInRefVertex1.begin(), scanInRefVertex1.end());
}
//Scans in vset1
RawLaser::Point2DVector scansvset1;
transformPointsFromVSet(vset1, _referenceVertex1, scansvset1);
//Add local map from vset2 into the grid
_grid.addAndConvolvePoints<RawLaser::Point2DVector>(scansvset2inref1.begin(), scansvset2inref1.end(), _kernel);
//Find points from vset1 not explained by map vset2
RawLaser::Point2DVector nonmatchedpoints;
_grid.searchNonMatchedPoints(scansvset1, nonmatchedpoints, .3);
//Add those points to a grid to count them
auxGrid.addAndConvolvePoints<RawLaser::Point2DVector>(nonmatchedpoints.begin(), nonmatchedpoints.end(), _kernel);
// ofstream image1;
// std::stringstream filename1;
// filename1 << "map2.ppm";
// image1.open(filename1.str().c_str());
// _LCGrid.grid().saveAsPPM(image1, false);
// ofstream image2;
// std::stringstream filename2;
// filename2 << "mapnonmatched.ppm";
// image2.open(filename2.str().c_str());
// auxGrid.grid().saveAsPPM(image2, false);
// //Just for saving the image
// resetLCGrid();
// _LCGrid.addAndConvolvePoints<RawLaser::Point2DVector>(scansvset1.begin(), scansvset1.end(), _LCKernel);
// ofstream image3;
// std::stringstream filename3;
// filename3 << "map1.ppm";
// image3.open(filename3.str().c_str());
// _LCGrid.grid().saveAsPPM(image3, false);
//Counting points around trel12
Vector2f lower(-.3+trel12.translation().x(), -.3+trel12.translation().y());
Vector2f upper(+.3+trel12.translation().x(), +.3+trel12.translation().y());
auxGrid.countPoints(lower, upper, score);
cerr << "Score: " << *score << endl;
double threshold = 40.0;
if (*score <= threshold)
return true;
return false;
}
bool ScanMatcher::scanMatchingLC(OptimizableGraph::VertexSet& referenceVset, OptimizableGraph::Vertex* _referenceVertex, OptimizableGraph::VertexSet& currvset, OptimizableGraph::Vertex* _currentVertex, std::vector<SE2>& trel, double maxScore){
cerr << "Loop Closing Scan Matching" << endl;
//cerr << "Size of Vset " << referenceVset.size() << endl;
VertexSE2* referenceVertex =dynamic_cast<VertexSE2*>(_referenceVertex);
resetGrid();
trel.clear();
RawLaser::Point2DVector scansInRefVertex;
transformPointsFromVSet(referenceVset, _referenceVertex, scansInRefVertex);
_grid.addAndConvolvePoints<RawLaser::Point2DVector>(scansInRefVertex.begin(), scansInRefVertex.end(), _kernel);
RawLaser::Point2DVector scansInCurVertex;
transformPointsFromVSet(currvset, _currentVertex, scansInCurVertex);
Vector2dVector reducedScans;
CharGrid::subsample(reducedScans, scansInCurVertex, 0.1);
RegionVector regions;
RegionVector regionspi;
for (OptimizableGraph::VertexSet::iterator it = referenceVset.begin(); it != referenceVset.end(); it++){
VertexSE2 *vertex = (VertexSE2*) *it;
Region reg;
SE2 relposv(.0, .0, .0);
if (vertex->id() != referenceVertex->id())
relposv = referenceVertex->estimate().inverse() * vertex->estimate();
Vector3f lower(-.5+relposv.translation().x(), -2.+relposv.translation().y(), -1.+relposv.rotation().angle());
Vector3f upper( .5+relposv.translation().x(), 2.+relposv.translation().y(), 1.+relposv.rotation().angle());
reg.lowerLeft = lower;
reg.upperRight = upper;
regions.push_back(reg);
lower[2] += M_PI;
upper[2] += M_PI;
reg.lowerLeft = lower;
reg.upperRight = upper;
regionspi.push_back(reg);
}
std::vector<MatcherResult> mresvec;
double thetaRes = 0.025; // was 0.0125*.5
//Results discretization
double dx = 0.5, dy = 0.5, dth = 0.2;
std::map<DiscreteTriplet, MatcherResult> resultsMap;
clock_t t_ini, t_fin;
double secs;
t_ini = clock();
_grid.greedySearch(mresvec, reducedScans, regions, thetaRes, maxScore, dx, dy, dth);
t_fin = clock();
secs = (double)(t_fin - t_ini) / CLOCKS_PER_SEC;
printf("%.16g ms. Matcher results: %i\n", secs * 1000.0, (int) mresvec.size());
if (mresvec.size()){
mresvec[0].transformation[2] = normalize_theta(mresvec[0].transformation[2]);
cerr << "Found Loop Closure Edge. Transf: " << mresvec[0].transformation.x() << " " << mresvec[0].transformation.y() << " " << mresvec[0].transformation.z() << endl;
CharGrid::addToPrunedMap(resultsMap, mresvec[0], dx, dy, dth);
}
t_ini = clock();
_grid.greedySearch(mresvec, reducedScans, regionspi, thetaRes, maxScore, dx, dy, dth);
t_fin = clock();
secs = (double)(t_fin - t_ini) / CLOCKS_PER_SEC;
printf("%.16g ms. Matcher results: %i\n", secs * 1000.0, (int) mresvec.size());
if (mresvec.size()){
mresvec[0].transformation[2] = normalize_theta(mresvec[0].transformation[2]);
cerr << "Found Loop Closure Edge PI. Transf: " << mresvec[0].transformation.x() << " " << mresvec[0].transformation.y() << " " << mresvec[0].transformation.z() << endl;
CharGrid::addToPrunedMap(resultsMap, mresvec[0], dx, dy, dth);
}
for (std::map<DiscreteTriplet, MatcherResult>::iterator it = resultsMap.begin(); it!= resultsMap.end(); it++){
MatcherResult res = it->second;
Vector3d adj=res.transformation;
SE2 transf;
transf.setTranslation(Vector2d(adj.x(), adj.y()));
transf.setRotation(normalize_theta(adj.z()));
trel.push_back(transf);
std::cerr << "Final result: " << transf.translation().x() << " " << transf.translation().y() << " " << transf.rotation().angle() << std::endl;
}
if (trel.size())
return true;
return false;
}
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
}