void JacobianWorkspace::updateSize(const OptimizableGraph& graph) { for (OptimizableGraph::EdgeSet::iterator it = graph.edges().begin(); it != graph.edges().end(); ++it) { const OptimizableGraph::Edge* e = static_cast<const OptimizableGraph::Edge*>(*it); updateSize(e); } }
bool saveGnuplot(const std::string& gnudump, const OptimizableGraph& optimizer) { HyperGraph::VertexSet vset; for (HyperGraph::VertexIDMap::const_iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){ vset.insert(it->second); } return saveGnuplot(gnudump, vset, optimizer.edges()); }
bool dumpEdges(std::ostream& os, const OptimizableGraph& optimizer) { // seek for an action whose name is writeGnuplot in the library HyperGraphElementAction* saveGnuplot = HyperGraphActionLibrary::instance()->actionByName("writeGnuplot"); if (! saveGnuplot ){ cerr << __PRETTY_FUNCTION__ << ": no action \"writeGnuplot\" registered" << endl; return false; } WriteGnuplotAction::Parameters params; params.os = &os; // writing all edges os << "set terminal x11 noraise" << endl; os << "set size ratio -1" << endl; os << "plot \"-\" w l" << endl; for (HyperGraph::EdgeSet::const_iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) { OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it); (*saveGnuplot)(e, ¶ms); } os << "e" << endl; return true; }
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) { string inputFilename; string outputFilename; // command line parsing CommandArgs commandLineArguments; commandLineArguments.paramLeftOver("gm2dl-input", inputFilename, "", "gm2dl file which will be processed"); commandLineArguments.paramLeftOver("gm2dl-output", outputFilename, "", "name of the output file"); commandLineArguments.parseArgs(argc, argv); OptimizableGraph inputGraph; bool loadStatus = inputGraph.load(inputFilename.c_str()); if (! loadStatus) { cerr << "Error while loading input data" << endl; return 1; } OptimizableGraph outputGraph; // process all the vertices first double fx = -1; double baseline = -1; bool firstCam = true; for (OptimizableGraph::VertexIDMap::const_iterator it = inputGraph.vertices().begin(); it != inputGraph.vertices().end(); ++it) { if (dynamic_cast<VertexCam*>(it->second)) { VertexCam* v = static_cast<VertexCam*>(it->second); if (firstCam) { firstCam = false; g2o::ParameterCamera* camParams = new g2o::ParameterCamera; camParams->setId(0); const SBACam& c = v->estimate(); baseline = c.baseline; fx = c.Kcam(0,0); camParams->setKcam(c.Kcam(0,0), c.Kcam(1,1), c.Kcam(0,2), c.Kcam(1,2)); outputGraph.addParameter(camParams); } VertexSE3* ov = new VertexSE3; ov->setId(v->id()); Eigen::Isometry3d p; p = v->estimate().rotation(); p.translation() = v->estimate().translation(); ov->setEstimate(p); if (! outputGraph.addVertex(ov)) { assert(0 && "Failure adding camera vertex"); } } else if (dynamic_cast<VertexSBAPointXYZ*>(it->second)) { VertexSBAPointXYZ* v = static_cast<VertexSBAPointXYZ*>(it->second); VertexPointXYZ* ov = new VertexPointXYZ; ov->setId(v->id()); ov->setEstimate(v->estimate()); if (! outputGraph.addVertex(ov)) { assert(0 && "Failure adding camera vertex"); } } } for (OptimizableGraph::EdgeSet::const_iterator it = inputGraph.edges().begin(); it != inputGraph.edges().end(); ++it) { if (dynamic_cast<EdgeProjectP2SC*>(*it)) { EdgeProjectP2SC* e = static_cast<EdgeProjectP2SC*>(*it); EdgeSE3PointXYZDisparity* oe = new EdgeSE3PointXYZDisparity; oe->vertices()[0] = outputGraph.vertex(e->vertices()[1]->id()); oe->vertices()[1] = outputGraph.vertex(e->vertices()[0]->id()); double kx = e->measurement().x(); double ky = e->measurement().y(); double disparity = kx - e->measurement()(2); oe->setMeasurement(Eigen::Vector3d(kx, ky, disparity / (fx * baseline))); oe->setInformation(e->information()); // TODO convert information matrix oe->setParameterId(0,0); if (! outputGraph.addEdge(oe)) { assert(0 && "error adding edge"); } } } cout << "Vertices in/out:\t" << inputGraph.vertices().size() << " " << outputGraph.vertices().size() << endl; cout << "Edges in/out:\t" << inputGraph.edges().size() << " " << outputGraph.edges().size() << endl; cout << "Writing output ... " << flush; outputGraph.save(outputFilename.c_str()); cout << "done." << endl; 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) { 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); }