This is a pseudo C++ package for doing mapping based on the occupancy grid
In robotics, mapping takes place after SLAM. In the SLAM, the robot pose and environment map is generated while in the mapping, the poses are taken filtered and assumed as known.
Going back to the graphical model of mapping with known poses, the goal is to implement a mapping algorithm and estimate the map given noisy measurements and assuming known poses.
The Mapping with Known Poses problem can be represented with P(m∣z1:t,x1:t) function. With this function, we can compute the posterior over the map given all the measurements up to time t and all the poses up to time t represented by the robot trajectory.
In estimating the map, we’ll exclude the controls u since the robot path is provided to us from SLAM. However, keep in mind that the robot controls will be included later in SLAM to estimate the robot’s trajectory.
It is obvious that maps have high dimensionality. Consequently, it will be too pricey in terms of computational memory to compute the posterior under this first approach.
A better approach to estimating the posterior map is to decompose this problem into many separate problems. In each of these problems, we will compute the posterior map mi at each instant. However, this approach still presents some drawbacks because we are computing the probability of each cell independently. Thus, we still need to find a different approach that addresses the dependencies between neighboring cells.
In the third approach we relate cells and overcome the need for huge computational memory in order to estimate the map with the product of marginals or factorization.
Inverse Measurement Model - P(x | z1:t): Estimating a posterior over the system state given the measurement.
The inverse measurement model is generally used when measurements are more complex than the system's state.
Because in the occupancy grid approach, the cells are either occupied or free (or unknown), we need a binary bias filter:
where the initial belief represents the initial state of the system before taking any sensor measurements into consideration.
In this pseudo implementation, a robot is equipped with eight sonar rangefinder sensors circulating in an environment to map it. This robot is provided with its exact poses at each timestamp.
the advatage of using the odds log probablity is due its numerical stability around zero and one as it can be seen:
The inverse sensor model takes the following inputs
and delivers if the cell is occupied, free, or unknown. Then, based on the pose data:
