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;
}
}
}
}
double EstimatePropagatorCostOdometry::operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* to_) const
{
OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(edge);
OptimizableGraph::Vertex* from = dynamic_cast<OptimizableGraph::Vertex*>(*from_.begin());
OptimizableGraph::Vertex* to = dynamic_cast<OptimizableGraph::Vertex*>(to_);
if (std::abs(from->id() - to->id()) != 1) // simple method to identify odometry edges in a pose graph
return std::numeric_limits<double>::max();
SparseOptimizer::EdgeContainer::const_iterator it = _graph->findActiveEdge(e);
if (it == _graph->activeEdges().end()) // it has to be an active edge
return std::numeric_limits<double>::max();
return e->initialEstimatePossible(from_, to);
}
bool OptimizableGraph::load(istream& is, bool createEdges)
{
// scna for the paramers in the whole file
if (!_parameters.read(is,&_renamedTypesLookup))
return false;
#ifndef NDEBUG
cerr << "Loaded " << _parameters.size() << " parameters" << endl;
#endif
is.clear();
is.seekg(ios_base::beg);
set<string> warnedUnknownTypes;
stringstream currentLine;
string token;
Factory* factory = Factory::instance();
HyperGraph::GraphElemBitset elemBitset;
elemBitset[HyperGraph::HGET_PARAMETER] = 1;
elemBitset.flip();
Vertex* previousVertex = 0;
Data* previousData = 0;
while (1) {
int bytesRead = readLine(is, currentLine);
if (bytesRead == -1)
break;
currentLine >> token;
//cerr << "Token=" << token << endl;
if (bytesRead == 0 || token.size() == 0 || token[0] == '#')
continue;
// handle commands encoded in the file
bool handledCommand = false;
if (token == "FIX") {
handledCommand = true;
int id;
while (currentLine >> id) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(vertex(id));
if (v) {
# ifndef NDEBUG
cerr << "Fixing vertex " << v->id() << endl;
# endif
v->setFixed(true);
} else {
cerr << "Warning: Unable to fix vertex with id " << id << ". Not found in the graph." << endl;
}
}
}
if (handledCommand)
continue;
// do the mapping to an internal type if it matches
if (_renamedTypesLookup.size() > 0) {
map<string, string>::const_iterator foundIt = _renamedTypesLookup.find(token);
if (foundIt != _renamedTypesLookup.end()) {
token = foundIt->second;
}
}
if (! factory->knowsTag(token)) {
if (warnedUnknownTypes.count(token) != 1) {
warnedUnknownTypes.insert(token);
cerr << CL_RED(__PRETTY_FUNCTION__ << " unknown type: " << token) << endl;
}
continue;
}
HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset);
if (dynamic_cast<Vertex*>(element)) { // it's a vertex type
//cerr << "it is a vertex" << endl;
previousData = 0;
Vertex* v = static_cast<Vertex*>(element);
int id;
currentLine >> id;
bool r = v->read(currentLine);
if (! r)
cerr << __PRETTY_FUNCTION__ << ": Error reading vertex " << token << " " << id << endl;
v->setId(id);
if (!addVertex(v)) {
cerr << __PRETTY_FUNCTION__ << ": Failure adding Vertex, " << token << " " << id << endl;
delete v;
} else {
previousVertex = v;
}
}
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
}
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;
}
int main(int argc, char** argv)
{
bool fixLaser;
int maxIterations;
bool verbose;
string inputFilename;
string outputfilename;
string rawFilename;
string odomTestFilename;
string dumpGraphFilename;
// command line parsing
CommandArgs commandLineArguments;
commandLineArguments.param("i", maxIterations, 10, "perform n iterations");
commandLineArguments.param("v", verbose, false, "verbose output of the optimization process");
commandLineArguments.param("o", outputfilename, "", "output final version of the graph");
commandLineArguments.param("test", odomTestFilename, "", "apply odometry calibration to some test data");
commandLineArguments.param("dump", dumpGraphFilename, "", "write the graph to the disk");
commandLineArguments.param("fixLaser", fixLaser, false, "keep the laser offset fixed during optimization");
commandLineArguments.paramLeftOver("gm2dl-input", inputFilename, "", "gm2dl file which will be processed");
commandLineArguments.paramLeftOver("raw-log", rawFilename, "", "raw log file containing the odometry");
commandLineArguments.parseArgs(argc, argv);
SparseOptimizer optimizer;
optimizer.setVerbose(verbose);
optimizer.setForceStopFlag(&hasToStop);
allocateSolverForSclam(optimizer);
// loading
if (! Gm2dlIO::readGm2dl(inputFilename, optimizer, false)) {
cerr << "Error while loading gm2dl file" << endl;
}
DataQueue robotLaserQueue;
int numLaserOdom = Gm2dlIO::readRobotLaser(rawFilename, robotLaserQueue);
if (numLaserOdom == 0) {
cerr << "No raw information read" << endl;
return 0;
}
cerr << "Read " << numLaserOdom << " laser readings from file" << endl;
bool gaugeFreedom = optimizer.gaugeFreedom();
OptimizableGraph::Vertex* gauge = optimizer.findGauge();
if (gaugeFreedom) {
if (! gauge) {
cerr << "# cannot find a vertex to fix in this thing" << endl;
return 2;
} else {
cerr << "# graph is fixed by node " << gauge->id() << endl;
gauge->setFixed(true);
}
} else {
cerr << "# graph is fixed by priors" << endl;
}
addOdometryCalibLinksDifferential(optimizer, robotLaserQueue);
// sanity check
HyperDijkstra d(&optimizer);
UniformCostFunction f;
d.shortestPaths(gauge, &f);
//cerr << PVAR(d.visited().size()) << endl;
if (d.visited().size()!=optimizer.vertices().size()) {
cerr << CL_RED("Warning: d.visited().size() != optimizer.vertices().size()") << endl;
cerr << "visited: " << d.visited().size() << endl;
cerr << "vertices: " << optimizer.vertices().size() << endl;
if (1)
for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
if (d.visited().count(v) == 0) {
cerr << "\t unvisited vertex " << it->first << " " << (void*)v << endl;
v->setFixed(true);
}
}
}
for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
if (v->fixed()) {
cerr << "\t fixed vertex " << it->first << endl;
}
}
VertexSE2* laserOffset = dynamic_cast<VertexSE2*>(optimizer.vertex(Gm2dlIO::ID_LASERPOSE));
VertexOdomDifferentialParams* odomParamsVertex = dynamic_cast<VertexOdomDifferentialParams*>(optimizer.vertex(Gm2dlIO::ID_ODOMCALIB));
if (fixLaser) {
cerr << "Fix position of the laser offset" << endl;
laserOffset->setFixed(true);
}
signal(SIGINT, sigquit_handler);
cerr << "Doing full estimation" << endl;
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
cerr << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl;
int i=optimizer.optimize(maxIterations);
if (maxIterations > 0 && !i){
cerr << "optimize failed, result might be invalid" << endl;
}
if (laserOffset) {
cerr << "Calibrated laser offset (x, y, theta):" << laserOffset->estimate().toVector().transpose() << endl;
}
if (odomParamsVertex) {
cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << odomParamsVertex->estimate().transpose() << endl;
}
cerr << "vertices: " << optimizer.vertices().size() << endl;
cerr << "edges: " << optimizer.edges().size() << endl;
if (dumpGraphFilename.size() > 0) {
cerr << "Writing " << dumpGraphFilename << " ... ";
optimizer.save(dumpGraphFilename.c_str());
cerr << "done." << endl;
}
// optional input of a seperate file for applying the odometry calibration
if (odomTestFilename.size() > 0) {
DataQueue testRobotLaserQueue;
int numTestOdom = Gm2dlIO::readRobotLaser(odomTestFilename, testRobotLaserQueue);
if (numTestOdom == 0) {
cerr << "Unable to read test data" << endl;
} else {
ofstream rawStream("odometry_raw.txt");
ofstream calibratedStream("odometry_calibrated.txt");
const Vector3d& odomCalib = odomParamsVertex->estimate();
RobotLaser* prev = dynamic_cast<RobotLaser*>(testRobotLaserQueue.buffer().begin()->second);
SE2 prevCalibratedPose = prev->odomPose();
for (DataQueue::Buffer::const_iterator it = testRobotLaserQueue.buffer().begin(); it != testRobotLaserQueue.buffer().end(); ++it) {
RobotLaser* cur = dynamic_cast<RobotLaser*>(it->second);
assert(cur);
double dt = cur->timestamp() - prev->timestamp();
SE2 motion = prev->odomPose().inverse() * cur->odomPose();
// convert to velocity measurment
MotionMeasurement motionMeasurement(motion.translation().x(), motion.translation().y(), motion.rotation().angle(), dt);
VelocityMeasurement velocityMeasurement = OdomConvert::convertToVelocity(motionMeasurement);
// apply calibration
VelocityMeasurement calibratedVelocityMeasurment = velocityMeasurement;
calibratedVelocityMeasurment.setVl(odomCalib(0) * calibratedVelocityMeasurment.vl());
calibratedVelocityMeasurment.setVr(odomCalib(1) * calibratedVelocityMeasurment.vr());
MotionMeasurement mm = OdomConvert::convertToMotion(calibratedVelocityMeasurment, odomCalib(2));
// combine calibrated odometry with the previous pose
SE2 remappedOdom;
remappedOdom.fromVector(mm.measurement());
SE2 calOdomPose = prevCalibratedPose * remappedOdom;
// write output
rawStream << prev->odomPose().translation().x() << " " << prev->odomPose().translation().y() << " " << prev->odomPose().rotation().angle() << endl;
calibratedStream << calOdomPose.translation().x() << " " << calOdomPose.translation().y() << " " << calOdomPose.rotation().angle() << endl;
prevCalibratedPose = calOdomPose;
prev = cur;
}
}
}
if (outputfilename.size() > 0) {
Gm2dlIO::updateLaserData(optimizer);
cerr << "Writing " << outputfilename << " ... ";
bool writeStatus = Gm2dlIO::writeGm2dl(outputfilename, optimizer);
cerr << (writeStatus ? "done." : "failed") << endl;
}
return 0;
}
void GraphSLAM::findConstraints(){
boost::mutex::scoped_lock lockg(graphMutex);
//graph is quickly optimized first so last added edge is satisfied
_graph->initializeOptimization();
_graph->optimize(1);
OptimizableGraph::VertexSet vset;
_vf.findVerticesScanMatching( _lastVertex, vset);
checkCovariance(vset);
addNeighboringVertices(vset, 8);
checkHaveLaser(vset);
std::set<OptimizableGraph::VertexSet> setOfVSet;
_vf.findSetsOfVertices(vset, setOfVSet);
OptimizableGraph::EdgeSet loopClosingEdges;
for (std::set<OptimizableGraph::VertexSet>::iterator it = setOfVSet.begin(); it != setOfVSet.end(); it++) {
OptimizableGraph::VertexSet myvset = *it;
OptimizableGraph::Vertex* closestV = _vf.findClosestVertex(myvset, _lastVertex);
if (closestV->id() == _lastVertex->id() - 1) //Already have this edge
continue;
SE2 transf;
if (!isMyVertex(closestV) || (isMyVertex(closestV) && abs(_lastVertex->id() - closestV->id()) > 10)){
/*VertexEllipse* ellipse = findEllipseData(_lastVertex);
if (ellipse){
for (OptimizableGraph::VertexSet::iterator itv = myvset.begin(); itv != myvset.end(); itv++){
VertexSE2 *vertex = (VertexSE2*) *itv;
SE2 relativetransf = _lastVertex->estimate().inverse() * vertex->estimate();
ellipse->addMatchingVertex(relativetransf.translation().x(), relativetransf.translation().y());
ellipse->addMatchingVertexID(vertex->id());
}
}*/
std::vector<SE2> results;
/*OptimizableGraph::VertexSet referenceVset;
referenceVset.insert(_lastVertex);
int j = 1;
int comm_gap = 5;
while (j <= comm_gap){
VertexSE2 *vj = dynamic_cast<VertexSE2 *>(graph()->vertex(_lastVertex->id()-j));
if (vj)
referenceVset.insert(vj);
else
break;
j++;
}*/
//Loop Closing Edge
bool shouldIAdd = _LCMatcher.scanMatchingLC(myvset, closestV, _lastVertex, results, maxScore);
//bool shouldIAdd = _mf.scanMatchingLC(myvset, closestV, referenceVset, _lastVertex, results, maxScore);
if (shouldIAdd){
for (unsigned int i =0; i< results.size(); i++){
EdgeSE2 *ne = new EdgeSE2;
ne->setId(++_runningEdgeId + _baseId);
ne->vertices()[0] = closestV;
ne->vertices()[1] = _lastVertex;
ne->setMeasurement(results[i]);
ne->setInformation(_SMinf);
loopClosingEdges.insert(ne);
_SMEdges.insert(ne);
}
}else {
std::cout << "Rejecting LC edge between " << closestV->id() << " and " << _lastVertex->id() << " [matching fail] " << std::endl;
}
}else{
//Edge between close vertices
bool shouldIAdd = _closeMatcher.closeScanMatching(myvset, closestV, _lastVertex, &transf, maxScore);
if (shouldIAdd){
EdgeSE2 *ne = new EdgeSE2;
ne->setId(++_runningEdgeId + _baseId);
ne->vertices()[0] = closestV;
ne->vertices()[1] = _lastVertex;
ne->setMeasurement(transf);
ne->setInformation(_SMinf);
_graph->addEdge(ne);
_SMEdges.insert(ne);
}else {
std::cout << "Rejecting edge between " << closestV->id() << " and " << _lastVertex->id() << " [matching fail] " << std::endl;
}
}
}
if (loopClosingEdges.size())
addClosures(loopClosingEdges);
checkClosures();
updateClosures();
}