void lrfExample(ImageWriter& iw)
{
iw.setLabel("lrf-example");
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
WorldModel2D wm;
// wm.addEntity(createBox(geo::Vec2(), geo::Vec2()));
wm.addEntity(createBox(7.63, 4.09, true), fromXYADegrees(0.085, 0.065, 0));
wm.addEntity(createBox(0.691667, 1.45833), fromXYADegrees(-0.529166, 0.404165, 0));
wm.addEntity(createBox(0.45833, 0.6), fromXYADegrees(-1.69583, 0.375, 0));
wm.addEntity(createBox(0.95833, 0.516667), fromXYADegrees(-1.72917, -0.616667, 0));
wm.addEntity(createBox(0.96666, 0.50833), fromXYADegrees(-1.75, 1.37084, 0));
wm.addEntity(createBox(1.6, 0.30834), fromXYADegrees(-0.45, -1.8375, 0));
wm.addEntity(createBox(0.65, 0.95), fromXYADegrees(1.6, 0.4, 0));
wm.addEntity(createBox(0.55, 1.28333), fromXYADegrees(3.54167, 0.558333, 0));
wm.addEntity(createCircle(0.1), fromXYA(0.358333, 1.81667, 0));
wm.addEntity(createCircle(0.1), fromXYA(-1.73333, -1.78333, 0));
wm.addEntity(createCircle(0.1), fromXYA(3.18333, 1.68333, 0));
Model2D model;
Contour2D& c = model.addContour();
c.addPoint(-3.73333 + 3.3, -1 + 1.6);
c.addPoint(-3.15 + 3.3, -1 + 1.6);
c.addPoint(-3.15 + 3.3, -1.46 + 1.6);
c.addPoint(-2.36 + 3.3, -1.46 + 1.6);
c.addPoint(-2.36 + 3.3, -1.97 + 1.6);
c.addPoint(-3.73333 + 3.3, -1.97 + 1.6);
wm.addEntity(model, fromXYADegrees(-3.3, -1.6, 0));
// wm.addEntity(createBox(geo::Vec2(0, -4), geo::Vec2(8, 0), true), fromXYA(-4, 2, 0)); // 0
// wm.addEntity(createBox(1.6, 0.8), fromXYA(2, -1, 0)); // 1 table
// wm.addEntity(createCircle(0.1), fromXYA(2.6, -0.8, 0)); // 2 small object
// wm.addEntity(createCircle(0.5), fromXYA(-3.2, -1.2, 0)); // 3
// wm.addEntity(createCircle(0.3), fromXYA(-2.2, -1.4, 0)); // 4
// wm.addEntity(createCircle(0.3), fromXYA(3.3, 1.0, 0)); // 5
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
geo::LaserRangeFinder lrf;
lrf.setNumBeams(50);
lrf.setAngleLimits(-1.0, 1.0);
lrf.setRangeLimits(0, 10);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Draw world model without extra object
Canvas canvas = iw.nextCanvas();
drawImage(canvas, iw.image_path() + "/livingroom.jpg", 0.9);
iw.setLabel("livingroom");
iw.process(canvas);
iw.setLabel("lrf-example");
iw.process(canvas);
// canvas = iw.nextCanvas();
// drawWorld(canvas, wm);
// iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Render world model (LRF) without extra object (virtual data)
geo::Transform2 lrf_pose = fromXYADegrees(0.533333, -1.24167, 145);
drawLRFPose(canvas, lrf_pose, Color(0, 150, 0, 2));
iw.process(canvas);
std::vector<double> ranges_virtual = renderLRF(lrf, lrf_pose, wm);
drawRanges(canvas, lrf, lrf_pose, ranges_virtual, Color(255, 0, 0, 3), Color(50, 50, 50));
drawLRFPose(canvas, lrf_pose, Color(0, 150, 0, 2));
iw.process(canvas);
drawWorld(canvas, wm);
drawRanges(canvas, lrf, lrf_pose, ranges_virtual, Color(255, 0, 0, 3), Color(50, 50, 50));
drawLRFPose(canvas, lrf_pose, Color(0, 150, 0, 2));
iw.process(canvas);
canvas = iw.nextCanvas();
drawWorld(canvas, wm);
drawRanges(canvas, lrf, lrf_pose, ranges_virtual, Color(255, 0, 0, 3), Color(50, 50, 50));
drawLRFPose(canvas, lrf_pose, Color(0, 150, 0, 2));
iw.process(canvas);
}
void relativeSection(ImageWriter& iw)
{
iw.setLabel("relative");
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
WorldModel2D wm;
// wm.addEntity(createBox(geo::Vec2(), geo::Vec2()));
// wm.addEntity(createBox(7.63, 4.09, true), fromXYADegrees(0.085, 0.065, 0));
wm.addEntity(createBox(geo::Vec2(0, -4.09), geo::Vec2(7.63, 0), true), fromXYADegrees(-3.75, 2.1, 0));
int idx_couch = wm.entities.size();
wm.addEntity(createBox(0.691667, 1.45833), fromXYADegrees(-0.529166, 0.404165, 0));
wm.addEntity(createBox(0.45833, 0.6), fromXYADegrees(-1.69583, 0.375, 0));
wm.addEntity(createBox(0.95833, 0.516667), fromXYADegrees(-1.72917, -0.616667, 0));
wm.addEntity(createBox(0.96666, 0.50833), fromXYADegrees(-1.75, 1.37084, 0));
wm.addEntity(createBox(1.6, 0.30834), fromXYADegrees(-0.45, -1.8375, 0));
int idx_table = wm.entities.size();
wm.addEntity(createBox(0.65, 0.95), fromXYADegrees(1.6, 0.4, 0));
int idx_cabinet = wm.entities.size();
wm.addEntity(createBox(0.55, 1.28333), fromXYADegrees(3.54167, 0.558333, 0));
wm.addEntity(createCircle(0.1), fromXYA(0.358333, 1.81667, 0));
wm.addEntity(createCircle(0.1), fromXYA(-1.73333, -1.78333, 0));
int idx_plant = wm.entities.size();
wm.addEntity(createCircle(0.1), fromXYA(3.18333, 1.68333, 0));
Model2D model;
Contour2D& c = model.addContour();
c.addPoint(-3.73333 + 3.3, -1 + 1.6);
c.addPoint(-3.15 + 3.3, -1 + 1.6);
c.addPoint(-3.15 + 3.3, -1.46 + 1.6);
c.addPoint(-2.36 + 3.3, -1.46 + 1.6);
c.addPoint(-2.36 + 3.3, -1.97 + 1.6);
c.addPoint(-3.73333 + 3.3, -1.97 + 1.6);
wm.addEntity(model, fromXYADegrees(-3.3, -1.6, 0));
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
Canvas canvas = iw.nextCanvas();
drawImage(canvas, iw.image_path() + "/livingroom.jpg", 0.9);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
drawWorld(canvas, wm);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
int idx_lrf = wm.entities.size();
wm.addEntity(createLRFPose(), fromXYADegrees(0.533333, -1.24167, 145), Color(0, 150, 0, 2));
drawWorld(canvas, wm);
iw.process(canvas);
int idx_target = wm.entities.size();
wm.addEntity(createTarget(), fromXYADegrees(-0.533333, -0.725, 0), Color(255, 0, 0, 2));
drawWorld(canvas, wm);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Moved couch
canvas = iw.nextCanvas();
drawImage(canvas, iw.image_path() + "/livingroom2.jpg", 0.9);
drawWorld(canvas, wm);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// canvas = iw.nextCanvas();
drawWorldModelAbsolute(canvas, wm, wm.entities[0].pose.t);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
canvas = iw.nextCanvas();
drawImage(canvas, iw.image_path() + "/livingroom2.jpg", 0.9);
drawWorld(canvas, wm);
iw.process(canvas);
std::vector<Link> links;
links.push_back(Link(0, idx_target));
links.push_back(Link(0, idx_lrf));
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
canvas = iw.nextCanvas();
drawImage(canvas, iw.image_path() + "/livingroom2.jpg", 0.9);
drawWorld(canvas, wm);
iw.process(canvas);
links.clear();
links.push_back(Link(idx_couch, idx_target));
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
links.push_back(Link(idx_lrf, idx_couch));
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
WorldModel2D wm2;
wm2.entities.push_back(wm.entities[idx_lrf]);
wm2.entities.push_back(wm.entities[idx_couch]);
wm2.entities.push_back(wm.entities[idx_target]);
links.clear();
links.push_back(Link(0, 1));
links.push_back(Link(1, 2));
canvas = iw.nextCanvas();
drawWorldModelSceneGraph(canvas, wm2, links);
iw.process(canvas);
canvas = iw.nextCanvas();
drawImage(canvas, iw.image_path() + "/livingroom2.jpg", 0.9);
wm.entities[idx_target].pose.t.y += 0.7;
wm.entities[idx_couch].pose.t.y += 0.7;
links.clear();
links.push_back(Link(idx_couch, idx_target));
links.push_back(Link(idx_lrf, idx_couch));
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
links.push_back(Link(idx_couch, 2));
links.push_back(Link(2, 3));
links.push_back(Link(2, 4));
links.push_back(Link(idx_lrf, idx_table));
links.push_back(Link(idx_table, idx_cabinet));
links.push_back(Link(idx_cabinet, idx_plant));
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Soccer field
iw.setLabel("relative-soccer");
//Click: [ -2.0125 -0.0125 ]
//Click: [ 0.8375 -1.1 ]
//Click: [ 2.075 0.025 ]
//Click: [ 2 1.7125 ] ball
wm.entities.clear();
wm.addEntity(createSoccerFieldModel(), geo::Transform2::identity(), Color(255, 255, 255, 2));
wm.addEntity(createCircle(0.2), fromXYA(2, 1.7125, 0), Color(255, 220, 0, 2));
wm.addEntity(createTurtleModel(), fromXYADegrees(0.8375, -1.1 , 60), Color(0, 0, 0, 2));
// wm.addEntity(createTurtleModel(), fromXYADegrees(-1.1, -0.15, -90), Color(0, 0, 0, 2));
// wm.addEntity(createTurtleModel(), fromXYADegrees(2.275, 0.025, 105), Color(0, 0, 0, 2));
canvas = iw.nextCanvas();
drawSoccerField(canvas);
drawWorld(canvas, wm);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
drawWorldModelAbsolute(canvas, wm, geo::Vec2(-4, 2.67));
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
links.clear();
links.push_back(Link(2, 1));
canvas = iw.nextCanvas();
drawSoccerField(canvas);
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
wm.addEntity(createTurtleModel(), fromXYADegrees(-1.5, -0.15, -10), Color(0, 0, 0, 2));
canvas = iw.nextCanvas();
drawSoccerField(canvas);
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
links.push_back(Link(3, 2));
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
drawArrow(canvas, wm.entities[3].pose.t, wm.entities[1].pose.t, Color(150, 150, 150, 2), true);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
canvas = iw.nextCanvas();
drawSoccerField(canvas);
geo::Vec2 ball_pos_rel = wm.entities[2].pose.inverse() * wm.entities[1].pose.t;
geo::Transform2 offset = geo::Transform2::identity();
offset.setRotation(-0.3);
wm.entities[2].pose = wm.entities[2].pose * offset;
wm.entities[1].pose.t = wm.entities[2].pose * ball_pos_rel;
drawWorldModelSceneGraph(canvas, wm, links);
drawArrow(canvas, wm.entities[3].pose.t, wm.entities[1].pose.t, Color(150, 150, 150, 2), true);
iw.process(canvas);
// Turn back
wm.entities[2].pose = wm.entities[2].pose * offset.inverse();
wm.entities[1].pose.t = wm.entities[2].pose * ball_pos_rel;
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
canvas = iw.nextCanvas();
drawSoccerField(canvas);
drawWorld(canvas, wm);
iw.process(canvas);
// wm.addEntity(createTurtleModel(), fromXYADegrees(2.275, 0.025, 105), Color(0, 0, 0, 2));
wm.addEntity(createTurtleModel(), fromXYADegrees(0.375, 0.65, -105), Color(0, 0, 0, 2));
drawWorld(canvas, wm);
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
links.clear();
links.push_back(Link(2, 1));
links.push_back(Link(2, 4));
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
// links.push_back(Link(4, 1));
// drawWorldModelSceneGraph(canvas, wm, links);
// iw.process(canvas);
links.push_back(Link(3, 2));
links.push_back(Link(3, 4));
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
cv::Mat temp = canvas.image.clone();
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
canvas = iw.nextCanvas();
canvas.image = temp.clone();
drawTriangle(canvas, wm.entities[1].pose.t, wm.entities[2].pose.t, wm.entities[4].pose.t, Color(0, 255, 255, 2));
iw.process(canvas);
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Replace 3th turtle by field feature
wm.entities[4] = Entity2D(Model2D(), fromXYA(0, 0.7, 0), Color());
canvas = iw.nextCanvas();
drawSoccerField(canvas);
drawWorldModelSceneGraph(canvas, wm, links);
iw.process(canvas);
drawTriangle(canvas, wm.entities[1].pose.t, wm.entities[2].pose.t, wm.entities[4].pose.t, Color(0, 255, 255, 2));
iw.process(canvas);
}
