int main(int argc, char** argv) { bool pcg; int updateEachN; bool vis; bool verbose; // command line parsing CommandArgs arg; arg.param("update", updateEachN, 10, "update the graph after inserting N nodes"); arg.param("pcg", pcg, false, "use PCG instead of Cholesky"); arg.param("v", verbose, false, "verbose output of the optimization process"); arg.param("g", vis, false, "gnuplot visualization"); arg.parseArgs(argc, argv); SparseOptimizerOnline optimizer(pcg); //SparseOptimizer optimizer; optimizer.setVerbose(verbose); optimizer.setForceStopFlag(&hasToStop); optimizer.vizWithGnuplot = vis; G2oSlamInterface slamInterface(&optimizer); slamInterface.setUpdateGraphEachN(updateEachN); SlamParser::ParserInterface parserInterface(&slamInterface); while (parserInterface.parseCommand(cin)) { // do something additional if needed } return 0; }
int main(int argc, char** argv) { QApplication qapp(argc, argv); #ifdef G2O_HAVE_GLUT glutInit(&argc, argv); #endif string dummy; string inputFilename; CommandArgs arg; arg.param("solverlib", dummy, "", "specify a solver library which will be loaded"); arg.param("typeslib", dummy, "", "specify a types library which will be loaded"); arg.paramLeftOver("graph-input", inputFilename, "", "graph file which will be processed", true); arg.parseArgs(argc, argv); // loading the standard solver / types DlWrapper dlTypesWrapper; loadStandardTypes(dlTypesWrapper, argc, argv); // register all the solvers DlWrapper dlSolverWrapper; loadStandardSolver(dlSolverWrapper, argc, argv); MainWindow mw; mw.updateDisplayedSolvers(); mw.show(); // redirect the output that normally goes to cerr to the textedit in the viewer StreamRedirect redirect(cerr, mw.plainTextEdit); // setting up the optimizer SparseOptimizer* optimizer = new SparseOptimizer(); mw.viewer->graph = optimizer; // set up the GUI action GuiHyperGraphAction guiHyperGraphAction; guiHyperGraphAction.viewer = mw.viewer; optimizer->addPostIterationAction(&guiHyperGraphAction); if (inputFilename.size() > 0) { mw.loadFromFile(QString::fromStdString(inputFilename)); } while (mw.isVisible()) { guiHyperGraphAction.dumpScreenshots = mw.actionDump_Images->isChecked(); qapp.processEvents(); SleepThread::msleep(10); } delete optimizer; // destroy all the singletons //Factory::destroy(); //OptimizationAlgorithmFactory::destroy(); //HyperGraphActionLibrary::destroy(); return 0; }
int main(int argc, char** argv) { OptimizableGraph::initMultiThreading(); QApplication qapp(argc, argv); CommandArgs arg; #ifndef G2O_DISABLE_DYNAMIC_LOADING_OF_LIBRARIES string dummy; arg.param("solverlib", dummy, "", "specify a solver library which will be loaded"); arg.param("typeslib", dummy, "", "specify a types library which will be loaded"); // loading the standard solver / types DlWrapper dlTypesWrapper; loadStandardTypes(dlTypesWrapper, argc, argv); // register all the solvers DlWrapper dlSolverWrapper; loadStandardSolver(dlSolverWrapper, argc, argv); #endif // run the viewer return RunG2OViewer::run(argc, argv, arg); }
int main(int argc, char **argv){ CommandArgs arg; int nRobots; int idRobot; std::string base_addr; ros::init(argc, argv, "comm_publisher"); arg.param("idRobot", idRobot, 0, "robot identifier" ); arg.param("nRobots", nRobots, 1, "number of robots in the network" ); arg.param("base_addr", base_addr, "127.0.0.", "base network addres"); arg.parseArgs(argc, argv); CommPublisher cp(idRobot, nRobots, base_addr); cp.init(); cp.start(); ros::spin(); std::cerr << "Finished" << std::endl; }
int main(int argc, char** argv) { CommandArgs arg; std::string outputFilename; std::string inputFilename; arg.param("o", outputFilename, "", "output file name"); arg.paramLeftOver("input-filename ", inputFilename, "", "graph file to read", true); arg.parseArgs(argc, argv); OptimizableGraph graph; if (!graph.load(inputFilename.c_str())){ cerr << "Error: cannot load a file from \"" << inputFilename << "\", aborting." << endl; return 0; } HyperGraph::EdgeSet removedEdges; HyperGraph::VertexSet removedVertices; for (HyperGraph::EdgeSet::iterator it = graph.edges().begin(); it!=graph.edges().end(); it++) { HyperGraph::Edge* e = *it; EdgeSE2PointXY* edgePointXY = dynamic_cast<EdgeSE2PointXY*>(e); if (edgePointXY) { VertexSE2* pose = dynamic_cast<VertexSE2*>(edgePointXY->vertex(0)); VertexPointXY* landmark = dynamic_cast<VertexPointXY*>(edgePointXY->vertex(1)); FeaturePointXYData * feature = new FeaturePointXYData(); feature->setPositionMeasurement(edgePointXY->measurement()); feature->setPositionInformation(edgePointXY->information()); pose->addUserData(feature); removedEdges.insert(edgePointXY); removedVertices.insert(landmark); } } for (HyperGraph::EdgeSet::iterator it = removedEdges.begin(); it!=removedEdges.end(); it++){ OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(*it); graph.removeEdge(e); } for (HyperGraph::VertexSet::iterator it = removedVertices.begin(); it!=removedVertices.end(); it++){ OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it); graph.removeVertex(v); } if (outputFilename.length()){ graph.save(outputFilename.c_str()); } }
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); allocateSolverForSclam(optimizer); // loading DataQueue odometryQueue; int numLaserOdom = Gm2dlIO::readRobotLaser(rawFilename, odometryQueue); if (numLaserOdom == 0) { cerr << "No raw information read" << endl; return 0; } cerr << "Read " << numLaserOdom << " laser readings from file" << endl; Eigen::Vector3d odomCalib(1., 1., 1.); SE2 initialLaserPose; DataQueue robotLaserQueue; int numRobotLaser = Gm2dlIO::readRobotLaser(inputFilename, robotLaserQueue); if (numRobotLaser == 0) { cerr << "No robot laser read" << endl; return 0; } else { RobotLaser* rl = dynamic_cast<RobotLaser*>(robotLaserQueue.buffer().begin()->second); initialLaserPose = rl->odomPose().inverse() * rl->laserPose(); cerr << PVAR(initialLaserPose.toVector().transpose()) << endl; } // adding the measurements vector<MotionInformation, Eigen::aligned_allocator<MotionInformation> > motions; { std::map<double, RobotData*>::const_iterator it = robotLaserQueue.buffer().begin(); std::map<double, RobotData*>::const_iterator prevIt = it++; for (; it != robotLaserQueue.buffer().end(); ++it) { MotionInformation mi; RobotLaser* prevLaser = dynamic_cast<RobotLaser*>(prevIt->second); RobotLaser* curLaser = dynamic_cast<RobotLaser*>(it->second); mi.laserMotion = prevLaser->laserPose().inverse() * curLaser->laserPose(); // get the motion of the robot in that time interval RobotLaser* prevOdom = dynamic_cast<RobotLaser*>(odometryQueue.findClosestData(prevLaser->timestamp())); RobotLaser* curOdom = dynamic_cast<RobotLaser*>(odometryQueue.findClosestData(curLaser->timestamp())); mi.odomMotion = prevOdom->odomPose().inverse() * curOdom->odomPose(); mi.timeInterval = prevOdom->timestamp() - curOdom->timestamp(); prevIt = it; motions.push_back(mi); } } if (1) { VertexSE2* laserOffset = new VertexSE2; laserOffset->setId(Gm2dlIO::ID_LASERPOSE); laserOffset->setEstimate(initialLaserPose); optimizer.addVertex(laserOffset); VertexOdomDifferentialParams* odomParamsVertex = new VertexOdomDifferentialParams; odomParamsVertex->setId(Gm2dlIO::ID_ODOMCALIB); odomParamsVertex->setEstimate(Eigen::Vector3d(1., 1., 1.)); optimizer.addVertex(odomParamsVertex); for (size_t i = 0; i < motions.size(); ++i) { const SE2& odomMotion = motions[i].odomMotion; const SE2& laserMotion = motions[i].laserMotion; const double& timeInterval = motions[i].timeInterval; // add the edge MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval); OdomAndLaserMotion meas; meas.velocityMeasurement = OdomConvert::convertToVelocity(mm); meas.laserMotion = laserMotion; EdgeSE2PureCalib* calibEdge = new EdgeSE2PureCalib; calibEdge->setVertex(0, laserOffset); calibEdge->setVertex(1, odomParamsVertex); calibEdge->setInformation(Eigen::Matrix3d::Identity()); calibEdge->setMeasurement(meas); if (! optimizer.addEdge(calibEdge)) { cerr << "Error adding calib edge" << endl; delete calibEdge; } } if (fixLaser) { cerr << "Fix position of the laser offset" << endl; laserOffset->setFixed(true); } cerr << "\nPerforming full non-linear estimation" << endl; optimizer.initializeOptimization(); optimizer.computeActiveErrors(); optimizer.optimize(maxIterations); cerr << "Calibrated laser offset (x, y, theta):" << laserOffset->estimate().toVector().transpose() << endl; odomCalib = odomParamsVertex->estimate(); cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << odomParamsVertex->estimate().transpose() << endl; optimizer.clear(); } // linear least squares for some parameters { Eigen::MatrixXd A(motions.size(), 2); Eigen::VectorXd x(motions.size()); for (size_t i = 0; i < motions.size(); ++i) { const SE2& odomMotion = motions[i].odomMotion; const SE2& laserMotion = motions[i].laserMotion; const double& timeInterval = motions[i].timeInterval; MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval); VelocityMeasurement velMeas = OdomConvert::convertToVelocity(mm); A(i, 0) = velMeas.vl() * timeInterval; A(i, 1) = velMeas.vr() * timeInterval; x(i) = laserMotion.rotation().angle(); } //linearSolution = (A.transpose() * A).inverse() * A.transpose() * x; linearSolution = A.colPivHouseholderQr().solve(x); //cout << PVAR(linearSolution.transpose()) << endl; } //constructing non-linear least squares VertexSE2* laserOffset = new VertexSE2; laserOffset->setId(Gm2dlIO::ID_LASERPOSE); laserOffset->setEstimate(initialLaserPose); optimizer.addVertex(laserOffset); VertexBaseline* odomParamsVertex = new VertexBaseline; odomParamsVertex->setId(Gm2dlIO::ID_ODOMCALIB); odomParamsVertex->setEstimate(1.); optimizer.addVertex(odomParamsVertex); for (size_t i = 0; i < motions.size(); ++i) { const SE2& odomMotion = motions[i].odomMotion; const SE2& laserMotion = motions[i].laserMotion; const double& timeInterval = motions[i].timeInterval; // add the edge MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval); OdomAndLaserMotion meas; meas.velocityMeasurement = OdomConvert::convertToVelocity(mm); meas.laserMotion = laserMotion; EdgeCalib* calibEdge = new EdgeCalib; calibEdge->setVertex(0, laserOffset); calibEdge->setVertex(1, odomParamsVertex); calibEdge->setInformation(Eigen::Matrix3d::Identity()); calibEdge->setMeasurement(meas); if (! optimizer.addEdge(calibEdge)) { cerr << "Error adding calib edge" << endl; delete calibEdge; } } if (fixLaser) { cerr << "Fix position of the laser offset" << endl; laserOffset->setFixed(true); } cerr << "\nPerforming partial non-linear estimation" << endl; optimizer.initializeOptimization(); optimizer.computeActiveErrors(); optimizer.optimize(maxIterations); cerr << "Calibrated laser offset (x, y, theta):" << laserOffset->estimate().toVector().transpose() << endl; odomCalib(0) = -1. * linearSolution(0) * odomParamsVertex->estimate(); odomCalib(1) = linearSolution(1) * odomParamsVertex->estimate(); odomCalib(2) = odomParamsVertex->estimate(); cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << odomCalib.transpose() << endl; { SE2 closedFormLaser; Eigen::Vector3d closedFormOdom; ClosedFormCalibration::calibrate(motions, closedFormLaser, closedFormOdom); cerr << "\nObtaining closed form solution" << endl; cerr << "Calibrated laser offset (x, y, theta):" << closedFormLaser.toVector().transpose() << endl; cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << closedFormOdom.transpose() << endl; } if (dumpGraphFilename.size() > 0) { cerr << "Writing " << dumpGraphFilename << " ... "; optimizer.save(dumpGraphFilename.c_str()); cerr << "done." << endl; } // optional input of a separate 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"); 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 measurement 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; } } } return 0; }
int main(int argc, char** argv) { OptimizableGraph::initMultiThreading(); int maxIterations; bool verbose; string inputFilename; string gnudump; string outputfilename; string solverProperties; string strSolver; string loadLookup; bool initialGuess; bool initialGuessOdometry; bool marginalize; bool listTypes; bool listSolvers; bool listRobustKernels; bool incremental; bool guiOut; int gaugeId; string robustKernel; bool computeMarginals; bool printSolverProperties; double huberWidth; double gain; int maxIterationsWithGain; //double lambdaInit; int updateGraphEachN = 10; string statsFile; string summaryFile; bool nonSequential; // command line parsing std::vector<int> gaugeList; CommandArgs arg; arg.param("i", maxIterations, 5, "perform n iterations, if negative consider the gain"); arg.param("gain", gain, 1e-6, "the gain used to stop optimization (default = 1e-6)"); arg.param("ig",maxIterationsWithGain, std::numeric_limits<int>::max(), "Maximum number of iterations with gain enabled (default: inf)"); arg.param("v", verbose, false, "verbose output of the optimization process"); arg.param("guess", initialGuess, false, "initial guess based on spanning tree"); arg.param("guessOdometry", initialGuessOdometry, false, "initial guess based on odometry"); arg.param("inc", incremental, false, "run incremetally"); arg.param("update", updateGraphEachN, 10, "updates after x odometry nodes"); arg.param("guiout", guiOut, false, "gui output while running incrementally"); arg.param("marginalize", marginalize, false, "on or off"); arg.param("printSolverProperties", printSolverProperties, false, "print the properties of the solver"); arg.param("solverProperties", solverProperties, "", "set the internal properties of a solver,\n\te.g., initialLambda=0.0001,maxTrialsAfterFailure=2"); arg.param("gnudump", gnudump, "", "dump to gnuplot data file"); arg.param("robustKernel", robustKernel, "", "use this robust error function"); arg.param("robustKernelWidth", huberWidth, -1., "width for the robust Kernel (only if robustKernel)"); arg.param("computeMarginals", computeMarginals, false, "computes the marginal covariances of something. FOR TESTING ONLY"); arg.param("gaugeId", gaugeId, -1, "force the gauge"); arg.param("o", outputfilename, "", "output final version of the graph"); arg.param("solver", strSolver, "gn_var", "specify which solver to use underneat\n\t {gn_var, lm_fix3_2, gn_fix6_3, lm_fix7_3}"); #ifndef G2O_DISABLE_DYNAMIC_LOADING_OF_LIBRARIES string dummy; arg.param("solverlib", dummy, "", "specify a solver library which will be loaded"); arg.param("typeslib", dummy, "", "specify a types library which will be loaded"); #endif arg.param("stats", statsFile, "", "specify a file for the statistics"); arg.param("listTypes", listTypes, false, "list the registered types"); arg.param("listRobustKernels", listRobustKernels, false, "list the registered robust kernels"); arg.param("listSolvers", listSolvers, false, "list the available solvers"); arg.param("renameTypes", loadLookup, "", "create a lookup for loading types into other types,\n\t TAG_IN_FILE=INTERNAL_TAG_FOR_TYPE,TAG2=INTERNAL2\n\t e.g., VERTEX_CAM=VERTEX_SE3:EXPMAP"); arg.param("gaugeList", gaugeList, std::vector<int>(), "set the list of gauges separated by commas without spaces \n e.g: 1,2,3,4,5 "); arg.param("summary", summaryFile, "", "append a summary of this optimization run to the summary file passed as argument"); arg.paramLeftOver("graph-input", inputFilename, "", "graph file which will be processed", true); arg.param("nonSequential", nonSequential, false, "apply the robust kernel only on loop closures and not odometries"); arg.parseArgs(argc, argv); if (verbose) { cout << "# Used Compiler: " << G2O_CXX_COMPILER << endl; } #ifndef G2O_DISABLE_DYNAMIC_LOADING_OF_LIBRARIES // registering all the types from the libraries DlWrapper dlTypesWrapper; loadStandardTypes(dlTypesWrapper, argc, argv); // register all the solvers DlWrapper dlSolverWrapper; loadStandardSolver(dlSolverWrapper, argc, argv); #else if (verbose) cout << "# linked version of g2o" << endl; #endif OptimizationAlgorithmFactory* solverFactory = OptimizationAlgorithmFactory::instance(); if (listSolvers) { solverFactory->listSolvers(cout); } if (listTypes) { Factory::instance()->printRegisteredTypes(cout, true); } if (listRobustKernels) { std::vector<std::string> kernels; RobustKernelFactory::instance()->fillKnownKernels(kernels); cout << "Robust Kernels:" << endl; for (size_t i = 0; i < kernels.size(); ++i) { cout << kernels[i] << endl; } } SparseOptimizer optimizer; optimizer.setVerbose(verbose); optimizer.setForceStopFlag(&hasToStop); SparseOptimizerTerminateAction* terminateAction = 0; if (maxIterations < 0) { cerr << "# setup termination criterion based on the gain of the iteration" << endl; maxIterations = maxIterationsWithGain; terminateAction = new SparseOptimizerTerminateAction; terminateAction->setGainThreshold(gain); terminateAction->setMaxIterations(maxIterationsWithGain); optimizer.addPostIterationAction(terminateAction); } // allocating the desired solver + testing whether the solver is okay OptimizationAlgorithmProperty solverProperty; optimizer.setAlgorithm(solverFactory->construct(strSolver, solverProperty)); if (! optimizer.solver()) { cerr << "Error allocating solver. Allocating \"" << strSolver << "\" failed!" << endl; return 0; } if (solverProperties.size() > 0) { bool updateStatus = optimizer.solver()->updatePropertiesFromString(solverProperties); if (! updateStatus) { cerr << "Failure while updating the solver properties from the given string" << endl; } } if (solverProperties.size() > 0 || printSolverProperties) { optimizer.solver()->printProperties(cerr); } // Loading the input data if (loadLookup.size() > 0) { optimizer.setRenamedTypesFromString(loadLookup); } if (inputFilename.size() == 0) { cerr << "No input data specified" << endl; return 0; } else if (inputFilename == "-") { cerr << "Read input from stdin" << endl; if (!optimizer.load(cin)) { cerr << "Error loading graph" << endl; return 2; } } else { cerr << "Read input from " << inputFilename << endl; ifstream ifs(inputFilename.c_str()); if (!ifs) { cerr << "Failed to open file" << endl; return 1; } if (!optimizer.load(ifs)) { cerr << "Error loading graph" << endl; return 2; } } cerr << "Loaded " << optimizer.vertices().size() << " vertices" << endl; cerr << "Loaded " << optimizer.edges().size() << " edges" << endl; if (optimizer.vertices().size() == 0) { cerr << "Graph contains no vertices" << endl; return 1; } set<int> vertexDimensions = optimizer.dimensions(); if (! optimizer.isSolverSuitable(solverProperty, vertexDimensions)) { cerr << "The selected solver is not suitable for optimizing the given graph" << endl; return 3; } assert (optimizer.solver()); //optimizer.setMethod(str2method(strMethod)); //optimizer.setUserLambdaInit(lambdaInit); // check for vertices to fix to remove DoF bool gaugeFreedom = optimizer.gaugeFreedom(); OptimizableGraph::Vertex* gauge=0; if (gaugeList.size()){ cerr << "Fixing gauges: "; for (size_t i=0; i<gaugeList.size(); i++){ int id=gaugeList[i]; OptimizableGraph::Vertex* v=optimizer.vertex(id); if (!v){ cerr << "fatal, not found the vertex of id " << id << " in the gaugeList. Aborting"; return -1; } else { if (i==0) gauge = v; cerr << v->id() << " "; v->setFixed(1); } } cerr << endl; gaugeFreedom = false; } else { 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 or already fixed vertex" << endl; } // if schur, we wanna marginalize the landmarks... if (marginalize || solverProperty.requiresMarginalize) { int maxDim = *vertexDimensions.rbegin(); int minDim = *vertexDimensions.begin(); if (maxDim != minDim) { cerr << "# Preparing Marginalization of the Landmarks ... "; for (HyperGraph::VertexIDMap::iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){ OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second); if (v->dimension() != maxDim) { v->setMarginalized(true); } } cerr << "done." << endl; } } if (robustKernel.size() > 0) { AbstractRobustKernelCreator* creator = RobustKernelFactory::instance()->creator(robustKernel); cerr << "# Preparing robust error function ... "; if (creator) { if (nonSequential) { for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) { SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it); if (e->vertices().size() >= 2 && std::abs(e->vertex(0)->id() - e->vertex(1)->id()) != 1) { e->setRobustKernel(creator->construct()); if (huberWidth > 0) e->robustKernel()->setDelta(huberWidth); } } } else { for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) { SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it); e->setRobustKernel(creator->construct()); if (huberWidth > 0) e->robustKernel()->setDelta(huberWidth); } } cerr << "done." << endl; } else { cerr << "Unknown Robust Kernel: " << robustKernel << endl; } } // 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 (incremental) { cerr << CL_RED("# Note: this variant performs batch steps in each time step") << endl; cerr << CL_RED("# For a variant which updates the Cholesky factor use the binary g2o_incremental") << endl; int incIterations = maxIterations; if (! arg.parsedParam("i")) { cerr << "# Setting default number of iterations" << endl; incIterations = 1; } int updateDisplayEveryN = updateGraphEachN; int maxDim = 0; cerr << "# incremental settings" << endl; cerr << "#\t solve every " << updateGraphEachN << endl; cerr << "#\t iterations " << incIterations << endl; SparseOptimizer::VertexIDMap vertices = optimizer.vertices(); for (SparseOptimizer::VertexIDMap::const_iterator it = vertices.begin(); it != vertices.end(); ++it) { const SparseOptimizer::Vertex* v = static_cast<const SparseOptimizer::Vertex*>(it->second); maxDim = max(maxDim, v->dimension()); } vector<SparseOptimizer::Edge*> edges; for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) { SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it); edges.push_back(e); } optimizer.edges().clear(); optimizer.vertices().clear(); optimizer.setVerbose(false); // sort the edges in a way that inserting them makes sense sort(edges.begin(), edges.end(), IncrementalEdgesCompare()); double cumTime = 0.; int vertexCount=0; int lastOptimizedVertexCount = 0; int lastVisUpdateVertexCount = 0; bool freshlyOptimized=false; bool firstRound = true; HyperGraph::VertexSet verticesAdded; HyperGraph::EdgeSet edgesAdded; for (vector<SparseOptimizer::Edge*>::iterator it = edges.begin(); it != edges.end(); ++it) { SparseOptimizer::Edge* e = *it; int doInit = 0; SparseOptimizer::Vertex* v1 = optimizer.vertex(e->vertices()[0]->id()); SparseOptimizer::Vertex* v2 = optimizer.vertex(e->vertices()[1]->id()); if (! v1) { SparseOptimizer::Vertex* v = v1 = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[0]); bool v1Added = optimizer.addVertex(v); //cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl; assert(v1Added); if (! v1Added) cerr << "Error adding vertex " << v->id() << endl; else verticesAdded.insert(v); doInit = 1; if (v->dimension() == maxDim) vertexCount++; } if (! v2) { SparseOptimizer::Vertex* v = v2 = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[1]); bool v2Added = optimizer.addVertex(v); //cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl; assert(v2Added); if (! v2Added) cerr << "Error adding vertex " << v->id() << endl; else verticesAdded.insert(v); doInit = 2; if (v->dimension() == maxDim) vertexCount++; } // adding the edge and initialization of the vertices { //cerr << " adding edge " << e->vertices()[0]->id() << " " << e->vertices()[1]->id() << endl; if (! optimizer.addEdge(e)) { cerr << "Unable to add edge " << e->vertices()[0]->id() << " -> " << e->vertices()[1]->id() << endl; } else { edgesAdded.insert(e); } if (doInit) { OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]); OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]); switch (doInit){ case 1: // initialize v1 from v2 { HyperGraph::VertexSet toSet; toSet.insert(to); if (e->initialEstimatePossible(toSet, from) > 0.) { //cerr << "init: " //<< to->id() << "(" << to->dimension() << ") -> " //<< from->id() << "(" << from->dimension() << ") " << endl; e->initialEstimate(toSet, from); } else { assert(0 && "Added unitialized variable to the graph"); } break; } case 2: { HyperGraph::VertexSet fromSet; fromSet.insert(from); if (e->initialEstimatePossible(fromSet, to) > 0.) { //cerr << "init: " //<< from->id() << "(" << from->dimension() << ") -> " //<< to->id() << "(" << to->dimension() << ") " << endl; e->initialEstimate(fromSet, to); } else { assert(0 && "Added unitialized variable to the graph"); } break; } default: cerr << "doInit wrong value\n"; } } } freshlyOptimized=false; { //cerr << "Optimize" << endl; if (vertexCount - lastOptimizedVertexCount >= updateGraphEachN) { if (firstRound) { if (!optimizer.initializeOptimization()){ cerr << "initialization failed" << endl; return 0; } } else { if (! optimizer.updateInitialization(verticesAdded, edgesAdded)) { cerr << "updating initialization failed" << endl; return 0; } } verticesAdded.clear(); edgesAdded.clear(); double ts = get_monotonic_time(); int currentIt=optimizer.optimize(incIterations, !firstRound); double dts = get_monotonic_time() - ts; cumTime += dts; firstRound = false; //optimizer->setOptimizationTime(cumTime); if (verbose) { double chi2 = optimizer.chi2(); cerr << "nodes= " << optimizer.vertices().size() << "\t edges= " << optimizer.edges().size() << "\t chi2= " << chi2 << "\t time= " << dts << "\t iterations= " << currentIt << "\t cumTime= " << cumTime << endl; } lastOptimizedVertexCount = vertexCount; freshlyOptimized = true; if (guiOut) { if (vertexCount - lastVisUpdateVertexCount >= updateDisplayEveryN) { dumpEdges(cout, optimizer); lastVisUpdateVertexCount = vertexCount; } } } if (! verbose) cerr << "."; } } // for all edges if (! freshlyOptimized) { double ts = get_monotonic_time(); int currentIt=optimizer.optimize(incIterations, !firstRound); double dts = get_monotonic_time() - ts; cumTime += dts; //optimizer->setOptimizationTime(cumTime); if (verbose) { double chi2 = optimizer.chi2(); cerr << "nodes= " << optimizer.vertices().size() << "\t edges= " << optimizer.edges().size() << "\t chi2= " << chi2 << "\t time= " << dts << "\t iterations= " << currentIt << "\t cumTime= " << cumTime << endl; } } } else { // BATCH optimization if (statsFile!=""){ // allocate buffer for statistics; optimizer.setComputeBatchStatistics(true); } optimizer.initializeOptimization(); optimizer.computeActiveErrors(); double loadChi = optimizer.chi2(); cerr << "Initial chi2 = " << FIXED(loadChi) << endl; if (initialGuess) { optimizer.computeInitialGuess(); } else if (initialGuessOdometry) { EstimatePropagatorCostOdometry costFunction(&optimizer); optimizer.computeInitialGuess(costFunction); } double initChi = optimizer.chi2(); signal(SIGINT, sigquit_handler); int result=optimizer.optimize(maxIterations); if (maxIterations > 0 && result==OptimizationAlgorithm::Fail){ cerr << "Cholesky failed, result might be invalid" << endl; } else if (computeMarginals){ std::vector<std::pair<int, int> > blockIndices; for (size_t i=0; i<optimizer.activeVertices().size(); i++){ OptimizableGraph::Vertex* v=optimizer.activeVertices()[i]; if (v->hessianIndex()>=0){ blockIndices.push_back(make_pair(v->hessianIndex(), v->hessianIndex())); } if (v->hessianIndex()>0){ blockIndices.push_back(make_pair(v->hessianIndex()-1, v->hessianIndex())); } } SparseBlockMatrix<MatrixXd> spinv; if (optimizer.computeMarginals(spinv, blockIndices)) { for (size_t i=0; i<optimizer.activeVertices().size(); i++){ OptimizableGraph::Vertex* v=optimizer.activeVertices()[i]; cerr << "Vertex id:" << v->id() << endl; if (v->hessianIndex()>=0){ cerr << "inv block :" << v->hessianIndex() << ", " << v->hessianIndex()<< endl; cerr << *(spinv.block(v->hessianIndex(), v->hessianIndex())); cerr << endl; } if (v->hessianIndex()>0){ cerr << "inv block :" << v->hessianIndex()-1 << ", " << v->hessianIndex()<< endl; cerr << *(spinv.block(v->hessianIndex()-1, v->hessianIndex())); cerr << endl; } } } } optimizer.computeActiveErrors(); double finalChi=optimizer.chi2(); if (summaryFile!="") { PropertyMap summary; summary.makeProperty<StringProperty>("filename", inputFilename); summary.makeProperty<IntProperty>("n_vertices", optimizer.vertices().size()); summary.makeProperty<IntProperty>("n_edges", optimizer.edges().size()); int nLandmarks=0; int nPoses=0; int maxDim = *vertexDimensions.rbegin(); for (HyperGraph::VertexIDMap::iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){ OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second); if (v->dimension() != maxDim) { nLandmarks++; } else nPoses++; } set<string> edgeTypes; for (HyperGraph::EdgeSet::iterator it=optimizer.edges().begin(); it!=optimizer.edges().end(); it++){ edgeTypes.insert(Factory::instance()->tag(*it)); } stringstream edgeTypesString; for (std::set<string>::iterator it=edgeTypes.begin(); it!=edgeTypes.end(); it++){ edgeTypesString << *it << " "; } summary.makeProperty<IntProperty>("n_poses", nPoses); summary.makeProperty<IntProperty>("n_landmarks", nLandmarks); summary.makeProperty<StringProperty>("edge_types", edgeTypesString.str()); summary.makeProperty<DoubleProperty>("load_chi", loadChi); summary.makeProperty<StringProperty>("solver", strSolver); summary.makeProperty<BoolProperty>("robustKernel", robustKernel.size() > 0); summary.makeProperty<DoubleProperty>("init_chi", initChi); summary.makeProperty<DoubleProperty>("final_chi", finalChi); summary.makeProperty<IntProperty>("maxIterations", maxIterations); summary.makeProperty<IntProperty>("realIterations", result); ofstream os; os.open(summaryFile.c_str(), ios::app); summary.writeToCSV(os); } if (statsFile!=""){ cerr << "writing stats to file \"" << statsFile << "\" ... "; ofstream os(statsFile.c_str()); const BatchStatisticsContainer& bsc = optimizer.batchStatistics(); for (int i=0; i<maxIterations; i++) { os << bsc[i] << endl; } cerr << "done." << endl; } } // saving again if (gnudump.size() > 0) { bool gnuPlotStatus = saveGnuplot(gnudump, optimizer); if (! gnuPlotStatus) { cerr << "Error while writing gnuplot files" << endl; } } if (outputfilename.size() > 0) { if (outputfilename == "-") { cerr << "saving to stdout"; optimizer.save(cout); } else { cerr << "saving " << outputfilename << " ... "; optimizer.save(outputfilename.c_str()); } cerr << "done." << endl; } // destroy all the singletons //Factory::destroy(); //OptimizationAlgorithmFactory::destroy(); //HyperGraphActionLibrary::destroy(); return 0; }
int main(int argc, char** argv) { CommandArgs arg; int nlandmarks; int nSegments; int simSteps; double worldSize; bool hasOdom; bool hasPoseSensor; bool hasPointSensor; bool hasPointBearingSensor; bool hasSegmentSensor; bool hasCompass; bool hasGPS; int segmentGridSize; double minSegmentLenght, maxSegmentLenght; std::string outputFilename; arg.param("simSteps", simSteps, 100, "number of simulation steps"); arg.param("nLandmarks", nlandmarks, 1000, "number of landmarks"); arg.param("nSegments", nSegments, 1000, "number of segments"); arg.param("segmentGridSize", segmentGridSize, 50, "number of cells of the grid where to align the segments"); arg.param("minSegmentLenght", minSegmentLenght, 0.5, "minimal lenght of a segment in the world"); arg.param("maxSegmentLenght", maxSegmentLenght, 3, "maximal lenght of a segment in the world"); arg.param("worldSize", worldSize, 25.0, "size of the world"); arg.param("hasOdom", hasOdom, false, "the robot has an odometry" ); arg.param("hasPointSensor", hasPointSensor, false, "the robot has a point sensor" ); arg.param("hasPointBearingSensor", hasPointBearingSensor, false, "the robot has a point bearing sensor" ); arg.param("hasPoseSensor", hasPoseSensor, false, "the robot has a pose sensor" ); arg.param("hasCompass", hasCompass, false, "the robot has a compass"); arg.param("hasGPS", hasGPS, false, "the robot has a GPS"); arg.param("hasSegmentSensor", hasSegmentSensor, false, "the robot has a segment sensor" ); arg.paramLeftOver("graph-output", outputFilename, "simulator_out.g2o", "graph file which will be written", true); arg.parseArgs(argc, argv); std::tr1::ranlux_base_01 generator; OptimizableGraph graph; World world(&graph); for (int i=0; i<nlandmarks; i++){ WorldObjectPointXY * landmark = new WorldObjectPointXY; double x = sampleUniform(-.5,.5, &generator)*worldSize; double y = sampleUniform(-.5,.5, &generator)*worldSize; landmark->vertex()->setEstimate(Vector2d(x,y)); world.addWorldObject(landmark); } cerr << "nSegments = " << nSegments << endl; for (int i=0; i<nSegments; i++){ WorldObjectSegment2D * segment = new WorldObjectSegment2D; int ix = sampleUniform(-segmentGridSize,segmentGridSize, &generator); int iy = sampleUniform(-segmentGridSize,segmentGridSize, &generator); int ith = sampleUniform(0,3, &generator); double th= (M_PI/2)*ith; th=atan2(sin(th),cos(th)); double xc = ix*(worldSize/segmentGridSize); double yc = iy*(worldSize/segmentGridSize); double l2 = sampleUniform(minSegmentLenght, maxSegmentLenght, &generator); double x1 = xc + cos(th)*l2; double y1 = yc + sin(th)*l2; double x2 = xc - cos(th)*l2; double y2 = yc - sin(th)*l2; segment->vertex()->setEstimateP1(Vector2d(x1,y1)); segment->vertex()->setEstimateP2(Vector2d(x2,y2)); world.addWorldObject(segment); } Robot2D robot(&world, "myRobot"); world.addRobot(&robot); stringstream ss; ss << "-ws" << worldSize; ss << "-nl" << nlandmarks; ss << "-steps" << simSteps; if (hasOdom) { SensorOdometry2D* odometrySensor=new SensorOdometry2D("odometry"); robot.addSensor(odometrySensor); Matrix3d odomInfo = odometrySensor->information(); odomInfo.setIdentity(); odomInfo*=100; odomInfo(2,2)=1000; odometrySensor->setInformation(odomInfo); ss << "-odom"; } if (hasPoseSensor) { SensorPose2D* poseSensor = new SensorPose2D("poseSensor"); robot.addSensor(poseSensor); Matrix3d poseInfo = poseSensor->information(); poseInfo.setIdentity(); poseInfo*=100; poseInfo(2,2)=1000; poseSensor->setInformation(poseInfo); ss << "-pose"; } if (hasPointSensor) { SensorPointXY* pointSensor = new SensorPointXY("pointSensor"); robot.addSensor(pointSensor); Matrix2d pointInfo = pointSensor->information(); pointInfo.setIdentity(); pointInfo*=100; pointSensor->setInformation(pointInfo); ss << "-pointXY"; } if (hasPointBearingSensor) { SensorPointXYBearing* bearingSensor = new SensorPointXYBearing("bearingSensor"); robot.addSensor(bearingSensor); bearingSensor->setInformation(bearingSensor->information()*1000); ss << "-pointBearing"; } if (hasSegmentSensor) { SensorSegment2D* segmentSensor = new SensorSegment2D("segmentSensorSensor"); segmentSensor->setMaxRange(3); segmentSensor->setMinRange(.1); robot.addSensor(segmentSensor); segmentSensor->setInformation(segmentSensor->information()*1000); SensorSegment2DLine* segmentSensorLine = new SensorSegment2DLine("segmentSensorSensorLine"); segmentSensorLine->setMaxRange(3); segmentSensorLine->setMinRange(.1); robot.addSensor(segmentSensorLine); Matrix2d m=segmentSensorLine->information(); m=m*1000; m(0,0)*=10; segmentSensorLine->setInformation(m); SensorSegment2DPointLine* segmentSensorPointLine = new SensorSegment2DPointLine("segmentSensorSensorPointLine"); segmentSensorPointLine->setMaxRange(3); segmentSensorPointLine->setMinRange(.1); robot.addSensor(segmentSensorPointLine); Matrix3d m3=segmentSensorPointLine->information(); m3=m3*1000; m3(3,3)*=10; segmentSensorPointLine->setInformation(m3); ss << "-segment2d"; } robot.move(SE2()); double pStraight=0.7; SE2 moveStraight; moveStraight.setTranslation(Vector2d(1., 0.)); double pLeft=0.15; SE2 moveLeft; moveLeft.setRotation(Rotation2Dd(M_PI/2)); //double pRight=0.15; SE2 moveRight; moveRight.setRotation(Rotation2Dd(-M_PI/2)); for (int i=0; i<simSteps; i++){ cerr << "m"; SE2 move; SE2 pose=robot.pose(); double dtheta=-100; Vector2d dt; if (pose.translation().x() < -.5*worldSize){ dtheta = 0; } if (pose.translation().x() > .5*worldSize){ dtheta = -M_PI; } if (pose.translation().y() < -.5*worldSize){ dtheta = M_PI/2; } if (pose.translation().y() > .5*worldSize){ dtheta = -M_PI/2; } if (dtheta< -M_PI) { // select a random move of the robot double sampled=sampleUniform(0.,1.,&generator); if (sampled<pStraight) move=moveStraight; else if (sampled<pStraight+pLeft) move=moveLeft; else move=moveRight; } else { double mTheta=dtheta-pose.rotation().angle(); move.setRotation(Rotation2Dd(mTheta)); if (move.rotation().angle()<std::numeric_limits<double>::epsilon()){ move.setTranslation(Vector2d(1., 0.)); } } robot.relativeMove(move); // do a sense cerr << "s"; robot.sense(); } //string fname=outputFilename + ss.str() + ".g2o"; ofstream testStream(outputFilename.c_str()); graph.save(testStream); return 0; }
int main(int argc, char** argv) { /************************************************************************ * Input Handling * ************************************************************************/ string configFile; string logFile; CommandArgs arg; int nscale; int mscale; // Optional input parameters. arg.param("nscale", nscale, 1, "image scaling for the normal extraction"); arg.param("mscale", mscale, 1, "image scaling for matching"); // Last parameter has to be the working directory. arg.paramLeftOver("config_file", configFile, "", "file where the configuration will be written", true); arg.paramLeftOver("log_file", logFile, "", "synchronized log file", true); arg.parseArgs(argc, argv); //Aligner* aligner; //DepthImageConverter* converter; cerr << "processing log file [" << logFile << "] with parameters read from [" << configFile << "]" << endl; cerr << "reading the configuration from file [" << configFile << "]" << endl; std::vector<Serializable*> alignerInstances = readConfig(argv[0], aligner, converter, configFile); cerr<< "Aligner: " << aligner << endl; cerr<< "Converter: " << converter << endl; if (logFile=="") { cerr << "logfile not supplied" << endl; } Deserializer des; des.setFilePath(logFile); std::vector<BaseSensorData*> sensorDatas; RobotConfiguration* conf = readLog(sensorDatas, des); cerr << "# frames: " << conf->frameMap().size() << endl; cerr << "# sensors: " << conf->sensorMap().size() << endl; cerr << "# sensorDatas: " << sensorDatas.size() << endl; TSCompare comp; std::sort(sensorDatas.begin(), sensorDatas.end(), comp); MapManager* manager = new MapManager; SensingFrameNodeMaker* sensingFrameMaker = new SensingFrameNodeMaker; sensingFrameMaker->init(manager, conf); ImuRelationAdder* imuAdder = new ImuRelationAdder(manager, conf); OdometryRelationAdder* odomAdder = new OdometryRelationAdder(manager, conf); TwoDepthImageAlignerNode* pairwiseAligner = new TwoDepthImageAlignerNode(manager, conf, converter, aligner, "/kinect/depth_registered/image_raw"); for (size_t i = 0; i<sensorDatas.size(); i++) { // see if you make a sensing frame with all the infos you find SensingFrameNode* s = sensingFrameMaker->processData(sensorDatas[i]); if (s) { // add a unary relation modeling the imu to the sensing frame imuAdder->processNode(s); // add a binary relation modeling the odometry between two sensing frames odomAdder->processNode(s); // add a pwn sensing data(if you manage) pairwiseAligner->processNode(s); } } cerr << "writing out things" << endl; Serializer ser; ser.setFilePath("out.log"); for (size_t i = 0; i< alignerInstances.size(); i++) { ser.writeObject(*alignerInstances[i]); } conf->serializeInternals(ser); ser.writeObject(*conf); ReferenceFrame* lastFrame = 0; for (size_t i = 0; i<sensorDatas.size(); i++) { if (lastFrame != sensorDatas[i]->robotReferenceFrame()) { lastFrame = sensorDatas[i]->robotReferenceFrame(); ser.writeObject(*lastFrame); } ser.writeObject(*sensorDatas[i]); } ser.writeObject(*manager); for (std::set<MapNode*>::iterator it = manager->nodes().begin(); it!=manager->nodes().end(); it++) ser.writeObject(**it); cerr << "writing out " << manager->relations().size() << " relations" << endl; for (std::set<MapNodeRelation*>::iterator it = manager->relations().begin(); it!=manager->relations().end(); it++) ser.writeObject(**it); // write the aligner stuff // write the robot configuration // write the sensor data // write the manager // write the objects in the manager // // write back the log // Serializer ser; // ser.setFilePath("sensing_frame_test.log"); // conf->serializeInternals(ser); // ser.writeObject(*conf); // previousReferenceFrame = 0; // for (size_t i = 0; i< sensorDatas.size(); i++){ // BaseSensorData* data = sensorDatas[i]; // if (previousReferenceFrame!=data->robotReferenceFrame()){ // ser.writeObject(*data->robotReferenceFrame()); // } // ser.writeObject(*data); // previousReferenceFrame = data->robotReferenceFrame(); // } // for(size_t i=0; i<newObjects.size(); i++){ // ser.writeObject(*newObjects[i]); // } }
int main(int argc, char **argv) { CommandArgs arg; double resolution; double maxScore; double kernelRadius; int minInliers; int windowLoopClosure; double inlierThreshold; int idRobot; int nRobots; std::string outputFilename; std::string odometryTopic, scanTopic, fixedFrame; arg.param("resolution", resolution, 0.025, "resolution of the matching grid"); arg.param("maxScore", maxScore, 0.15, "score of the matcher, the higher the less matches"); arg.param("kernelRadius", kernelRadius, 0.2, "radius of the convolution kernel"); arg.param("minInliers", minInliers, 7, "min inliers"); arg.param("windowLoopClosure", windowLoopClosure, 10, "sliding window for loop closures"); arg.param("inlierThreshold", inlierThreshold, 2., "inlier threshold"); arg.param("idRobot", idRobot, 0, "robot identifier" ); arg.param("nRobots", nRobots, 1, "number of robots" ); arg.param("odometryTopic", odometryTopic, "odom", "odometry ROS topic"); arg.param("scanTopic", scanTopic, "scan", "scan ROS topic"); arg.param("fixedFrame", fixedFrame, "odom", "fixed frame to visualize the graph with ROS Rviz"); arg.param("o", outputFilename, "", "file where to save output"); arg.parseArgs(argc, argv); ros::init(argc, argv, "srslam"); RosHandler rh(idRobot, nRobots, REAL_EXPERIMENT); rh.setOdomTopic(odometryTopic); rh.setScanTopic(scanTopic); rh.useOdom(true); rh.useLaser(true); rh.init(); rh.run(); //For estimation SE2 currEst = rh.getOdom(); std::cout << "My initial position is: " << currEst.translation().x() << " " << currEst.translation().y() << " " << currEst.rotation().angle() << std::endl; SE2 odomPosk_1 = currEst; std::cout << "My initial odometry is: " << odomPosk_1.translation().x() << " " << odomPosk_1.translation().y() << " " << odomPosk_1.rotation().angle() << std::endl; //Graph building GraphSLAM gslam; gslam.setIdRobot(idRobot); int baseId = 10000; gslam.setBaseId(baseId); gslam.init(resolution, kernelRadius, windowLoopClosure, maxScore, inlierThreshold, minInliers); RobotLaser* rlaser = rh.getLaser(); gslam.setInitialData(currEst, odomPosk_1, rlaser); GraphRosPublisher graphPublisher(gslam.graph(), fixedFrame); ros::Rate loop_rate(10); while (ros::ok()){ ros::spinOnce(); SE2 odomPosk = rh.getOdom(); //current odometry SE2 relodom = odomPosk_1.inverse() * odomPosk; currEst *= relodom; odomPosk_1 = odomPosk; if((distanceSE2(gslam.lastVertex()->estimate(), currEst) > 0.25) || (fabs(gslam.lastVertex()->estimate().rotation().angle()-currEst.rotation().angle()) > M_PI_4)){ //Add new data RobotLaser* laseri = rh.getLaser(); gslam.addDataSM(odomPosk, laseri); gslam.findConstraints(); struct timeval t_ini, t_fin; double secs; gettimeofday(&t_ini, NULL); gslam.optimize(5); gettimeofday(&t_fin, NULL); secs = timeval_diff(&t_fin, &t_ini); printf("Optimization took %.16g milliseconds\n", secs * 1000.0); currEst = gslam.lastVertex()->estimate(); char buf[100]; sprintf(buf, "robot-%i-%s", idRobot, outputFilename.c_str()); gslam.saveGraph(buf); //Publish graph to visualize it on Rviz graphPublisher.publishGraph(); } loop_rate.sleep(); } return 0; }
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; }
int main (int argc , char ** argv){ int maxIterations; bool verbose; bool robustKernel; double lambdaInit; CommandArgs arg; bool fixSensor; bool fixPlanes; bool fixFirstPose; bool fixTrajectory; bool planarMotion; bool listSolvers; string strSolver; cerr << "graph" << endl; arg.param("i", maxIterations, 5, "perform n iterations"); arg.param("v", verbose, false, "verbose output of the optimization process"); arg.param("solver", strSolver, "lm_var", "select one specific solver"); arg.param("lambdaInit", lambdaInit, 0, "user specified lambda init for levenberg"); arg.param("robustKernel", robustKernel, false, "use robust error functions"); arg.param("fixSensor", fixSensor, false, "fix the sensor position on the robot"); arg.param("fixTrajectory", fixTrajectory, false, "fix the trajectory"); arg.param("fixFirstPose", fixFirstPose, false, "fix the first robot pose"); arg.param("fixPlanes", fixPlanes, false, "fix the planes (do localization only)"); arg.param("planarMotion", planarMotion, false, "robot moves on a plane"); arg.param("listSolvers", listSolvers, false, "list the solvers"); arg.parseArgs(argc, argv); SparseOptimizer* g=new SparseOptimizer(); ParameterSE3Offset* odomOffset=new ParameterSE3Offset(); odomOffset->setId(0); g->addParameter(odomOffset); OptimizationAlgorithmFactory* solverFactory = OptimizationAlgorithmFactory::instance(); OptimizationAlgorithmProperty solverProperty; OptimizationAlgorithm* solver = solverFactory->construct(strSolver, solverProperty); g->setAlgorithm(solver); if (listSolvers){ solverFactory->listSolvers(cerr); return 0; } if (! g->solver()){ cerr << "Error allocating solver. Allocating \"" << strSolver << "\" failed!" << endl; cerr << "available solvers: " << endl; solverFactory->listSolvers(cerr); cerr << "--------------" << endl; return 0; } cerr << "sim" << endl; Simulator* sim = new Simulator(g); cerr << "robot" << endl; Robot* r=new Robot(g); cerr << "planeSensor" << endl; Matrix3d R=Matrix3d::Identity(); R << 0, 0, 1, -1, 0, 0, 0, -1, 0; Isometry3d sensorPose=Isometry3d::Identity(); sensorPose.matrix().block<3,3>(0,0) = R; sensorPose.translation()= Vector3d(.3 , 0.5 , 1.2); PlaneSensor* ps = new PlaneSensor(r, 0, sensorPose); ps->_nplane << 0.03, 0.03, 0.005; r->_sensors.push_back(ps); sim->_robots.push_back(r); cerr << "p1" << endl; Plane3D plane; PlaneItem* pi =new PlaneItem(g,1); plane.fromVector(Eigen::Vector4d(0.,0.,1.,5.)); static_cast<VertexPlane*>(pi->vertex())->setEstimate(plane); pi->vertex()->setFixed(fixPlanes); sim->_world.insert(pi); plane.fromVector(Eigen::Vector4d(1.,0.,0.,5.)); pi =new PlaneItem(g,2); static_cast<VertexPlane*>(pi->vertex())->setEstimate(plane); pi->vertex()->setFixed(fixPlanes); sim->_world.insert(pi); cerr << "p2" << endl; pi =new PlaneItem(g,3); plane.fromVector(Eigen::Vector4d(0.,1.,0.,5.)); static_cast<VertexPlane*>(pi->vertex())->setEstimate(plane); pi->vertex()->setFixed(fixPlanes); sim->_world.insert(pi); Quaterniond q, iq; if (planarMotion) { r->_planarMotion = true; r->_nmovecov << 0.01, 0.0025, 1e-9, 0.001, 0.001, 0.025; q = Quaterniond(AngleAxisd(0.2, Vector3d::UnitZ()).toRotationMatrix()); iq = Quaterniond(AngleAxisd(-0.2, Vector3d::UnitZ()).toRotationMatrix()); } else { r->_planarMotion = false; //r->_nmovecov << 0.1, 0.005, 1e-9, 0.05, 0.001, 0.001; r->_nmovecov << 0.1, 0.005, 1e-9, 0.001, 0.001, 0.05; q = Quaterniond((AngleAxisd(M_PI/10, Vector3d::UnitZ()) * AngleAxisd(0.1, Vector3d::UnitY())).toRotationMatrix()); iq = Quaterniond((AngleAxisd(-M_PI/10, Vector3d::UnitZ()) * AngleAxisd(0.1, Vector3d::UnitY())).toRotationMatrix()); } Isometry3d delta=Isometry3d::Identity(); sim->_lastVertexId=4; Isometry3d startPose=Isometry3d::Identity(); startPose.matrix().block<3,3>(0,0) = AngleAxisd(-0.75*M_PI, Vector3d::UnitZ()).toRotationMatrix(); sim->move(0,startPose); int k =20; int l = 2; double delta_t = 0.2; for (int j=0; j<l; j++) { Vector3d tr(1.,0.,0.); delta.matrix().block<3,3>(0,0) = q.toRotationMatrix(); if (j==(l-1)){ delta.matrix().block<3,3>(0,0) = Matrix3d::Identity(); } delta.translation()=tr*(delta_t*j); Isometry3d iDelta = delta.inverse(); for (int a=0; a<2; a++){ for (int i=0; i<k; i++){ cerr << "m"; if (a==0) sim->relativeMove(0,delta); else sim->relativeMove(0,iDelta); cerr << "s"; sim->sense(0); } } } for (int j=0; j<l; j++) { Vector3d tr(1.,0.,0.); delta.matrix().block<3,3>(0,0) = iq.toRotationMatrix(); if (j==l-1){ delta.matrix().block<3,3>(0,0) = Matrix3d::Identity(); } delta.translation()=tr*(delta_t*j); Isometry3d iDelta = delta.inverse(); for (int a=0; a<2; a++){ for (int i=0; i<k; i++){ cerr << "m"; if (a==0) sim->relativeMove(0,delta); else sim->relativeMove(0,iDelta); cerr << "s"; sim->sense(0); } } } ofstream os("test_gt.g2o"); g->save(os); if (fixSensor) { ps->_offsetVertex->setFixed(true); } else { Vector6d noffcov; noffcov << 0.1,0.1,0.1,0.5, 0.5, 0.5; ps->_offsetVertex->setEstimate(ps->_offsetVertex->estimate() * sample_noise_from_se3(noffcov)); ps->_offsetVertex->setFixed(false); } if (fixFirstPose){ OptimizableGraph::Vertex* gauge = g->vertex(4); if (gauge) gauge->setFixed(true); } // else { // // multiply all vertices of the robot by this standard quantity // Quaterniond q(AngleAxisd(1, Vector3d::UnitZ()).toRotationMatrix()); // Vector3d tr(1,0,0); // Isometry3d delta; // delta.matrix().block<3,3>(0,0)=q.toRotationMatrix(); // delta.translation()=tr; // for (size_t i=0; i< g->vertices().size(); i++){ // VertexSE3 *v = dynamic_cast<VertexSE3 *>(g->vertex(i)); // if (v && v->id()>0){ // v->setEstimate (v->estimate()*delta); // } // } // } ofstream osp("test_preopt.g2o"); g->save(osp); //g->setMethod(SparseOptimizer::LevenbergMarquardt); g->initializeOptimization(); g->setVerbose(verbose); g->optimize(maxIterations); if (! fixSensor ){ SparseBlockMatrix<MatrixXd> spinv; std::pair<int, int> indexParams; indexParams.first = ps->_offsetVertex->hessianIndex(); indexParams.second = ps->_offsetVertex->hessianIndex(); std::vector<std::pair <int, int> > blockIndices; blockIndices.push_back(indexParams); if (!g->computeMarginals(spinv, blockIndices)){ cerr << "error in computing the covariance" << endl; } else { MatrixXd m = *spinv.block(ps->_offsetVertex->hessianIndex(), ps->_offsetVertex->hessianIndex()); cerr << "Param covariance" << endl; cerr << m << endl; cerr << "OffsetVertex: " << endl; ps->_offsetVertex->write(cerr); cerr << endl; cerr << "rotationDeterminant: " << m.block<3,3>(0,0).determinant() << endl; cerr << "translationDeterminant: " << m.block<3,3>(3,3).determinant() << endl; cerr << endl; } } ofstream os1("test_postOpt.g2o"); g->save(os1); }
int main(int argc, char** argv) { CommandArgs arg; int nlandmarks; int simSteps; double worldSize; bool hasOdom; bool hasPoseSensor; bool hasPointSensor; bool hasPointBearingSensor; bool hasSegmentSensor; bool hasCompass; bool hasGPS; std::string outputFilename; arg.param("simSteps", simSteps, 100, "number of simulation steps"); arg.param("worldSize", worldSize, 25.0, "size of the world"); arg.param("hasOdom", hasOdom, false, "the robot has an odometry" ); arg.param("hasPointSensor", hasPointSensor, false, "the robot has a point sensor" ); arg.param("hasPointBearingSensor", hasPointBearingSensor, false, "the robot has a point bearing sensor" ); arg.param("hasPoseSensor", hasPoseSensor, false, "the robot has a pose sensor" ); arg.param("hasCompass", hasCompass, false, "the robot has a compass"); arg.param("hasGPS", hasGPS, false, "the robot has a GPS"); arg.param("hasSegmentSensor", hasSegmentSensor, false, "the robot has a segment sensor" ); arg.paramLeftOver("graph-output", outputFilename, "simulator_out.g2o", "graph file which will be written", true); arg.parseArgs(argc, argv); std::tr1::ranlux_base_01 generator; OptimizableGraph graph; World world(&graph); for (int i=0; i<nlandmarks; i++){ WorldObjectPointXY * landmark = new WorldObjectPointXY; double x = sampleUniform(-.5,.5, &generator)*worldSize; double y = sampleUniform(-.5,.5, &generator)*worldSize; landmark->vertex()->setEstimate(Vector2d(x,y)); world.addWorldObject(landmark); } Robot2D robot(&world, "myRobot"); world.addRobot(&robot); stringstream ss; ss << "-ws" << worldSize; ss << "-nl" << nlandmarks; ss << "-steps" << simSteps; if (hasOdom) { SensorOdometry2D* odometrySensor=new SensorOdometry2D("odometry"); robot.addSensor(odometrySensor); Matrix3d odomInfo = odometrySensor->information(); odomInfo.setIdentity(); odomInfo*=100; odomInfo(2,2)=1000; odometrySensor->setInformation(odomInfo); ss << "-odom"; } if (hasPoseSensor) { SensorPose2D* poseSensor = new SensorPose2D("poseSensor"); robot.addSensor(poseSensor); Matrix3d poseInfo = poseSensor->information(); poseInfo.setIdentity(); poseInfo*=100; poseInfo(2,2)=1000; poseSensor->setInformation(poseInfo); ss << "-pose"; } if (hasPointSensor) { SensorPointXY* pointSensor = new SensorPointXY("pointSensor"); robot.addSensor(pointSensor); Matrix2d pointInfo = pointSensor->information(); pointInfo.setIdentity(); pointInfo*=100; pointSensor->setInformation(pointInfo); ss << "-pointXY"; } if (hasPointBearingSensor) { SensorPointXYBearing* bearingSensor = new SensorPointXYBearing("bearingSensor"); robot.addSensor(bearingSensor); bearingSensor->setInformation(bearingSensor->information()*1000); ss << "-pointBearing"; } robot.move(SE2()); double pStraight=0.7; SE2 moveStraight; moveStraight.setTranslation(Vector2d(1., 0.)); double pLeft=0.15; SE2 moveLeft; moveLeft.setRotation(Rotation2Dd(M_PI/2)); //double pRight=0.15; SE2 moveRight; moveRight.setRotation(Rotation2Dd(-M_PI/2)); for (int i=0; i<simSteps; i++){ cerr << "m"; SE2 move; SE2 pose=robot.pose(); double dtheta=-100; Vector2d dt; if (pose.translation().x() < -.5*worldSize){ dtheta = 0; } if (pose.translation().x() > .5*worldSize){ dtheta = -M_PI; } if (pose.translation().y() < -.5*worldSize){ dtheta = M_PI/2; } if (pose.translation().y() > .5*worldSize){ dtheta = -M_PI/2; } if (dtheta< -M_PI) { // select a random move of the robot double sampled=sampleUniform(0.,1.,&generator); if (sampled<pStraight) move=moveStraight; else if (sampled<pStraight+pLeft) move=moveLeft; else move=moveRight; } else { double mTheta=dtheta-pose.rotation().angle(); move.setRotation(Rotation2Dd(mTheta)); if (move.rotation().angle()<std::numeric_limits<double>::epsilon()){ move.setTranslation(Vector2d(1., 0.)); } } robot.relativeMove(move); // do a sense cerr << "s"; robot.sense(); } //string fname=outputFilename + ss.str() + ".g2o"; ofstream testStream(outputFilename.c_str()); graph.save(testStream); }