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