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