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(); }