Introduction

This repository implements a Distance Map based localization system for a 2D robot. The system uses a grid map, initial pose, and laser scan data to continuously localize the robot in its environment.

Localization ensures the robot aligns its internal representation of position and orientation with the real-world map, enabling navigation and task execution.

The project was developed for the Sapienza's Robot Programming Course, as written in the acknowledgement

Table of Contents

1. Installation and Usage
2. Library Structure
   • Modules: grid
   • Modules: dmap
   • Modules: laser_scanner
   • Modules: world_item
3. Acknowledgement

Installation and Usage

Once the repository is cloned, you can build the project with

catkin build

Then, on different terminals:

roscore

To launch the ROS core

rosrun project_localizer main_node

To launch the localizer

rosrun map_server map_server <path/to/map.yaml>

To publish the map

rviz

To launch the ROS visualizer

rosrun stage_ros stageros <path/to/map.world>

To launch the Stage simulation

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

To launch the tools used to move the robot in Stage ROS.

Remember to run source opt/ros/noetic/setup.bash each time you open a new terminal (or put it in ~/bashrc) and source <workspace_DMAP/devel/setup.bash> when you want to run the localizer package.

Library Structure

Inside the package /src/localizer, in src:

• main_node.cpp: the ROS main node that subscribes to topics and perform localization
• modules/dmap: distance map generation and localization
• modules/grid: grid map handling and transformation
• modules/draw: visualization helpers in OpenCV

catkin uses CMake to build the project. Above, a description of the most relevant class methods of the library.

modules/grid

in grid.h:

• Grid_ is a templated grid class that provides the underlying grid structure and methods for grid operations like:
  • resize(), adjusts the size of the cells vector to hold all the grid cells
  • element access operator
  • inside(), to check if a row-column pair (a cell) is inside the bounds
  • distance2(), squared distance between two cells
  • rowDerivative() and colDerivative() to compute derivatives
  • draw(): renders the grid onto an OpenCV Canvas.

in grid_map.h (and grid_map.cpp):

• GridMapping handles the transformations between the world coordinates and the grid coordinates
  • reset(), that resets the grid mapping with a new origin and resolution
  • world2grid(), that multiplies the world coordinates by the _w2g affine transformation matrix to compute the corresponding grid coordinates.
  • grid2world() does the opposite with the _g2w affine transformation matrix
• GridMap extends GridMapping adding a Grid Data and operations for loading a map, raycasting and querying the grid
  • scanRay(): simulates a raycast from a given origin in a specific direction to determine the distance to the nearest obstacle or return max_range is no obstacle is found. Iteratively:
    • computes the endpoint in the grid coordinates with world2grid(origin + direction * range)
    • if this endpoint is outside of the grid bounds, stop and return max_range, that is the maximum range to check
    • otherwise, if the ray hits an occupied cells (value < 127), the function returns the current range
    • otherwise, increments the range and repeat

modules/dmap

in dmap.h (and dmap.cpp):

• DMapCell: represents a single cell in a distance map. It has a pointer to another DMapCell, used to trace the nearest obstacle.
• DMap: a distance map that extends the generic grid structure Grid_<DMapCell> (note the templated instantiated with DMapCell). It is used to compute and store distances from obstacles within a 2D grid
  • clear() resets the distance map by setting the parent of every DMapCell to null
  • compute(): for a set of obstacles, calculates the shortest squared distances from each cell in the grid to the nearest obstacle using a breadth-first traversal approach. More precisely:
    • Input: a list of grid cells representing the obstacles and dmax2_: the maximum squared distance for which distances are computed
    • Then add all the obstacles to a frontier
    • Perform Breadth-First propagation, while the frontier is not empty:
      • Take the first (current) cell from the frontier and remove it, get its position and retrieve its parents
      • For each of the 8 neighbors of the current cell:
        • calculate the squared distance from the neighbor to its parent
        • calculate the squared distance from the neighbor to the current cell's parent
        • if the latter is smaller than the first update the neihbord's parent to the current cell's parent
        • add the neighbor to the frontier In this way the algorithm propagates the distances outward from the obstacles with this BFS approach. This ensures that each cell is assigned the shortest possible distance to any obstacle.

in dmap_localizer.h (and dmap_localizer.cpp):

• DMapLocalizer: provides the actual localization capabilities
  • setMap(): initializes the localizer with a pre-computed grid mapping and distance map, or build a distance map from a list of obstacle positions or from an occupancy grid map.
  • localize(): refine the robot's pose (Isometry2f X) using a set of laser scan endpoints (measurements) as inputs. This optimization process uses the Iterative Closest Point (ICP) principle. Here's the pseudocode:
    • Input: list of sensors measurements
    • For each iteration:
      • compute the Jacobian of the grid-to-world transformation $J_{gm}$
      • For each measurements:
        • convert the point to the grid coordinate frame $p_{grid}$
        • query the distance map for the error
        • compute the squared error and apply robust kernel $\lambda$ to down-weight outliers
          • if the error is $e = 0$ it means that the measurement point lies exactly on an obstacle
          • if the error is $e &gt; 0$ the measurement point lies at a distance $e$ from the actual nearest obstacle
        • compute gradients of the distance map $J_{dmap}$ at point $p_{grid}$
        • compute the full jacobian $$J = J_{dmap} \cdot J_{gm} \cdot J_{icp}$$
        • update Hessian and gradient $H = H + \lambda \cdot J^T \cdot J$, $b = b + \lambda \cdot J^T \cdot e$
        • accumulate error $\chi^2 = \chi^2 + e^2$
      • Add damping to the Hessian: $H += I \cdot d$
      • Solve for pose correction: $\Delta x = - H^{-1} \cdot b$ Note that $\Delta x$ contains three components, $\Delta x$, $\Delta y$, $\Delta \theta$
      • Apply the pose update: create a transformation matrix $dX$ that has a rotation matrix $R(\theta)$ and a translation $(\Delta x, \Delta y)$
      • Finally, update the robot's pose: $X = X \cdot dX$

modules/laser_scanner

in laser_scan.h (and laser_scan.cpp):

• LaserScan: representing the laser scan input to the algorithm
  • draw() visualizes the laser scan by drawing lines representing each laser beam on the Canvas

in laser_scanner.h (and laser_scanner.cpp):

• LaserScanner: it simulates a laser scanner in a virtual world and provides functionality for scanning the environment
  • tick: advances the simulation time by time_interval. If enough time has elapsed, triggers a new scan
  • getScan(): simulates a laser scan computing the distance each beam travels before hitting an obstacle
    • first it calculates the global position and orientation of the laser scanner in the world with globalPose() from WorldItem
    • for each laser beam $i$:
      • calculate the beam angle as $$\text{beam angle} = \text{angle min} + \text{angle increment } \cdot i$$ where angle increment is $$\frac{\text{angle max} - \text{angle min}}{\text{number of laser beams}}$$
      • determine the beam direction $(cos(\text{beam angle}), \ \ sin(\text{beam angle}))$
      • convert the beam direction in the global frame by rotating it with the scanner's rotation
      • calculate distances with scanRay()
      • now ranges[i] contains the distance to an obstacle, or max_range if no obstacles are found

modules/world_item

in world_item.h (and world_item.cpp)

• WorldItem: represents an object in the grid-based world.
  • globalPose() calculates the item's global pose by chaining transformation from its parent
  • isAncestor() just check if the curent item is ancestor of another item
  • gridMap() retrieves the GridMap
  • tick() updates the state of the item and its children. Actually, this tick implementation just perform recursive updates, but the actual logic of the update must be specified in the derived class (like UnicyclePlatform)
• World: represent the overall world. It extends WorldItem. You can draw the entire grid map with its draw().

Acknowledgement

• The project was developed for the Robot Programming course of the Sapienza Artificial Intelligence and Robotics Master's degree.