Data structure to store a map while doing it with SLAM

Question

I'm studying robotics at the university and I have to implement on my own SLAM algorithm. To do it I will use ROS Melodic, Gazebo 9.1.0 and C++.

I have a doubt about what data structure I have to use to store the map (and what I'm going to store it, but this is another story).

I have thought to represent the map as a 2D grid and robot's start location is (0,0). But I don't know where exactly is the robot on the world that I have to map. It could be at the top left corner, at the middle of the world, or in any other unknonw location inside the world.

Each cell of the grid will be 1x1 meters. I will use a laser to know where are the obstacles. Using current robot's location, I will set to 1 on all the cells that represent an obstacle. For example, it laser detects an obstacle at 2 meters in front of the robot, I will set to 1 the cell at (0,2).

Using a vector, or a 2D matrix, here is a problem, because, vector and matrices indices start at 0, and there could be more room behind the robot to map. And that room will have an obstacle at (-1,-3).

On this data structure, I will need to store the cells that have an obstacle and the cells that I know they are free.

Which kind of data structure will I have to use?

UPDATE:

The process to store the map will be the following:

1. Robot starts at (0,0) cell. It will detect the obstacles and store them in the map.
2. Robot moves to (1,0) cell. And again, detect and store the obstacles in the map.
3. Continue moving to free cells and storing the obstacles it founds.

the robot will detect the obstacles that are in front of him and to the sides, but never behind it.

My problem comes when the robot detects an obstacle on a negative cell (like (0,-1). I don't know how to store that obstacle if I have previously stored only the obstacle on "positive" cells. So, maybe the "offset", it is not a solution here (or maybe I'm wrong).

---

Originally posted by Elric on ROS Answers with karma: 71 on 2019-08-27

Post score: 0

Answer 1

I have a doubt about what data structure I have to use to store the map

Why not use the same data structure as the nav_msgs/OccupancyGrid ?

This structure has a field data, which is a 2D-Matrix and another field, nav_msgs/MapMetadata which is used to store the origin, width, height and resolution of your map.

But I don't know where exactly is the robot on the world that I have to map

Since you are doing the mapping of the envirronment you are free to choose any origin for your robot, as long as you keep track of all your obstacles relatively from your origin.

Using a vector, or a 2D matrix, here is a problem, because, vector and matrices indices start at 0, and there could be more room behind the robot to map.

True, the first index is always 0, but it's your choice to assign the coordinates to the indexes, meanning that index 0 isn't necessarily the coordinate (0;0).

All that being said, you can decide when you start your mapping to set the current position of the robot to be the coordinate (0;0), for the example I choose to set the map origin to the bottom right corner of the map. I would calculate the origin coordinates according to your laser range. I would set the origin, width and height to ensure that everything the robot can "see" at its start position could be stored in your map, so I would choose something like

origin.x = -(laser_range + 1) 
origin.y = -(laser_range + 1)
height = 2*(laser_range + 1)
width = 2*(laser_range + 1)

The origin are negatives to be at the bottom right and all the values are incremented by 1 to be greater than the laser range. From that, all your cells are the indexes of your matrix. From that and knowing your resolution is 1 meter, if your laser has a range of 4 meters you get a matrix of size 100. Your first index being the origin of your map.

Now every index can be calculate from any coordinates :

index  = floor((x - origin.x)/resolution) + floor((y - origin.y)/resolution)*width

The last *width is because you have a 2D-Matrix, if you browse your map from bottom to top and right to left, one increment of y is equal to an increment of width in your vector.

With your exemple if you see an obstacle two meters in front of the robot, you know that your are two meters in front of (0;0) meaning (2;0). That represent the index :

index = floor((2-(-5))/1) + floor((0-(-5))/1)*10
index = floor(7) + floor(5)*10
index = 57

If your robot has moved don't forget to add the translation in the calculation :

index  = floor((x + robot.x - origin.x)/resolution) + floor((y + robot.y - origin.y)/resolution)*width

Now if you detect obstacles further than your map limits you just have to recalculate a new origin and increase the size of your map. All your previous indexes can be recalucated using conversion from the actual coordinates of the robot and then another conversion to with the new data of your map.

---

Originally posted by Delb with karma: 3907 on 2019-08-27

This answer was ACCEPTED on the original site

Post score: 3