Warning
This page is located in archive. Go to the latest version of this course pages. Go the latest version of this page.

t1d-map - Map building

The main task is to implement a function to fuse the laser scan data into the occupancy grid map. You will learn the Bayesian update of the grid map and the practical limitations of its usage. Further, you will experience a dataset with sensory data to speed up your workflow.

Files Mapper.py - File to be implemented
bresenham.py - Implementation of the Bresenham's line algorithm
utils.py - Helpers for evaluation, drawing, and saving figures
data - Scenarios and reference solutions in pkl files
t1d-map.py - Evaluation script
t1d-map-navsim.py - Evaluation script for simulation
Resources Introduction to CoppeliaSim/V-REP and Open-Loop Robot Locomotion Control
Exteroceptive sensing and Reactive-based Obstacle Avoidance
redcp.zip,
t1d-map.zip - Initial files and evaluation script
Implement and revise
  • Mapper.fuse_laser_scan() in Mapper.py,
  • Mapper.update_free() in Mapper.py,
  • Map.update_occupied() in Mapper.py.
  • You can use ControllerReactive from t1b-react or the provided ControllerReactive.py.

Assignment

In Mapper.py, implement the fuse_laser_scan function to fuse new data into the occupancy grid map as the robot traverses the environment. The laser scan measurements shall be fused to the grid using the Bayesian update described in Map Building. The input parameters of the function are:

  1. grid_map - OccupancyGrid - grid map representation;
  2. laser_scan - LaserScan - contains the current laser scan data perceived by the robot.;
  3. odometry - Odometry - pose.position.x, pose.position.y and pose.orientation.quaternion encode the current robot absolute position in the environment.

The function returns:

  1. the OccupancyGrid with the grid map fused with the new laser scan data; when the laser scan or the odometry data is invalid, the function returns a copy of the grid map on the output.

The fuse_laser_scan function in the Mapper.py class has the following prescription.

    def fuse_laser_scan(self, grid_map: OccupancyGrid, laser_scan: LaserScan, odometry: Odometry) -> OccupancyGrid:
        """ Method to fuse the laser scan data sampled by the robot with a given 
            odometry into the probabilistic occupancy grid map
        Args:
            grid_map: OccupancyGrid - grid map to fuse the laser scan to
            laser_scan: LaserScan - laser scan perceived by the robot
            odometry: Odometry - perceived odometry of the robot
        Returns:
            grid_map_update: OccupancyGrid - grid map updated with the laser scan data
        """


Approach

The recommended approach for the occupancy map building using the Bayesian approach is to follow Map Building. The OccupancyGrid represents a probabilistic representation of the word. In particular, the variable “OccupancyGrid.data” holds the probabilities of individual states to be occupied. The metadata further parameterizes the map in the OccupancyGrid:

  • resolution - the size of the single cell in the real (simulated) world;
  • width - width of the map;
  • height - height of the map;
  • origin - the real-world pose of the cell (0,0) in the map;
  • data - the actual probabilistic map data (in row-major order).

Hence the approach to the fusion the laser scan data into the occupancy grid map can be as follows.

  • Project the laser scan points to $x,y$ plane with respect to (w.r.t.) the robot heading.

$\qquad\bullet\,$ Detailed description and guidance for the step (click to view)

  • Compensate for the robot odometry
    • Compensate the robot's heading by rotating the scanned points based on the current odometry.
    • Compensate for the robot's position by offsetting the scanned points to the robot's coordinates.

$\qquad\bullet\,$ Detailed description and guidance for the step (click to view)

  • Transfer the points from the world coordinates to the map coordinates (compensate for the map offset and resolution).

$\qquad\bullet\,$ Detailed description and guidance for the step (click to view)

  • Raytrace individual scanned points to give you coordinates of the cells that occupancy probability should be updated

$\qquad\bullet\,$ Detailed description and guidance for the step (click to view)

  • Update the occupancy grid using the Bayesian update and the simplified laser scan sensor model with $\epsilon = 0$, i.e., update all the points $d$ lying between the position of the robot (given by its odometry) and each of the reflection points: $(d \in [0,r))$ and update their probability (being free) and only the reflection point $(d = r)$ being occupied

$\qquad\bullet\,$ Detailed description and guidance for the step (click to view)

The probability values shall never reach 0 or 1!!!

The probability values shall never reach 0 or 1!!! The effect of letting it happen is the inability of the map to dynamically add/delete obstacles, as demonstrated in the following example.

Correct behavior Wrong behavior
In the evaluation script t1d-map, the grid map is predefined with the size $100\times100$ points and correct map origin of $(-5,-5, 0)^T$ and orientation $(0,0,0,1)$, which is prepared exactly for seamless finishing of the t1d-map assignment. However, consider generalizing your solution to allow for a dynamically growing map.

Example behavior


Evaluation

The evaluation focus on the ability of the robot to do the correct probabilistic update. Two evaluation scripts are provided, t1d-map.py using recorded dataset data/scan_dataset.pkl and data/grid_map.pkl. The second script, t1d-map-navsim.py, utilizes ControllerReactive from t1b-react for navigation of the robot in the simulation environment.

The code can be evaluated using the following script (also attached as t1d-map.py).

    dataset = pickle.load( open( "data/scan_dataset.pkl", "rb" ) )
    reference = pickle.load( open( "data/gridmap.pkl", "rb" ) )
    print('Loaded dataset and reference')
 
    scan_n = 0
    for scan, odometry in dataset: #process the dataset
        sys.stdout.write('\rFusing scan %3d' % scan_n)
        scan_n += 1
        gridmap = mapper.fuse_laser_scan(gridmap, scan, odometry) #get the current laser scan and odometry and fuse them to the map
    print('\nAll scan fused compare the map with reference')
    fig = plot_map(gridmap, 'Map - Created map')
    image_file = RESULT_DIR + '/map-fused.png'
    save_image(fig, 'Save created map to ' + image_file, image_file)

Furthermore, the evaluation using simulation t1d-map-navsim.py is as follows.

    robot = hexapod.HexapodRobot(Controller())
    mapper = Mapper()
 
    robot.turn_on() #turn on the robot 
    robot.start_navigation() #start navigation thread
    goals = [ #assign goal for navigation
        Pose(Vector3(3.5, 3.5, 0), Quaternion(1, 0, 0, 0)), 
        Pose(Vector3(0.0, 0.0, 0), Quaternion(1, 0, 0, 0)), 
    ]
 
    plt.ion() #prepare the online plot
    fig, ax = plt.subplots()
 
    gridmap = OccupancyGrid() #prepare the gridmap
    gridmap.resolution = 0.1
 
    gridmap.width = 100
    gridmap.height = 100
    gridmap.origin = Pose(Vector3(-5.0, -5.0, 0.0), Quaternion(1, 0, 0, 0))
    gridmap.data = 0.5*np.ones((gridmap.height*gridmap.width))
 
    for goal in goals: #go from goal to goal
        robot.goto_reactive(goal)
        while robot.navigation_goal is not None:
            plt.cla()
            #get the current laser scan and odometry and fuse them to the map
            gridmap = mapper.fuse_laser_scan(gridmap, robot.laser_scan_, robot.odometry_)
            redraw_map(ax, gridmap) #plot the map
    robot.stop_navigation()
    robot.turn_off()

The expected output is the visually correct environment map, even for dynamic events.

courses/crl-courses/redcp/tasks/t1d-map.txt · Last modified: 2022/11/25 16:02 by faiglj