introlab / rtabmap

RTAB-Map library and standalone application
https://introlab.github.io/rtabmap
Other
2.83k stars 787 forks source link

Map coordinates on rtabmap/cloud_map to rtabmap/map ? #1339

Open illusionaryshelter opened 2 months ago

illusionaryshelter commented 2 months ago

Hi, I am a newbie in rtabmap,I read some point cloud from rtabmap/cloud_map, then reconstructed the plane using PCL's RANSAC (using only the raw data from cloud_map) and looked up some colors in it (e.g. the coordinates of the red objects), and now I would like to project these coordinates into a 2D map published by rtabmap/map. How could I do that? I learned that the type of rtabmap/map is nav_msgs/OccupancyGrid, is it the same coordinate system as rtabmap/cloud_map?(for x,y coordinates) Can I use the 2D coordinates from rtabmap/cloud_map reconstruction directly as 2D coordinates on rtabmap/map, or do I need to do some coordinate conversion? Thanks so much!

matlabbe commented 2 months ago

They are both in same origin. The coordinate (x,y) of each point in rtabmap/cloud_map would correspond to a cell in the occupancy grid. To know which cell (u,v) corresponds to point (x,y), you can use the info metadata contained in the nav_msgs/OccupancyGrid topic, For example, if the metadata says:

resolution = 0.05
origin = (-5, -7, 0) (0,0,0,1)

then

u = (x - origin.x) / resolution = (x - -5) / 0.05
v = (y - origin.y) / resolution = (y - -7) / 0.05

(in general the rotation is Identity (unit quaternion), so I didn't add it here for simplification)

illusionaryshelter commented 2 months ago

They are both in same origin. The coordinate (x,y) of each point in rtabmap/cloud_map would correspond to a cell in the occupancy grid. To know which cell (u,v) corresponds to point (x,y), you can use the info metadata contained in the nav_msgs/OccupancyGrid topic, For example, if the metadata says:

resolution = 0.05
origin = (-5, -7, 0) (0,0,0,1)

then

u = (x - origin.x) / resolution = (x - -5) / 0.05
v = (y - origin.y) / resolution = (y - -7) / 0.05

(in general the rotation is Identity (unit quaternion), so I didn't add it here for simplification)

@matlabbe Thank you for your response, that's what I am looking for! One last question, the point cloud I get from cloud_map is very sparse, I read in some places that I need to modify the cloud_decimation as well as the cloud_voxel_size to make it denser, however I didn't find them in the ros2 version, the ones I'm modifying now are the Grid/DepthDecimation (modifying to 1) and Grid/CellSize (modifying to 0.01) and finally I started rtabmap via rtabmap.launch.py , is this the correct behavior?

matlabbe commented 2 months ago

You don't need to necessary use rtabmap.launch.py to make it work. But yeah, those parameters (in particular, Grid/CellSize) will make the cloud_map denser. This could affect the resolution of the occupancy grid generated, which will be then 1 cm cell size. If you want to keep the occupancy grid at 5 cm cell, you could consider using map_assembler node (note that I just ported it on ros2 if you are on ros2). Here is an example with turtlebot4 demo where by default the occupancy grid is created from the laser scan in 2D. With the map_assembler node, we can regenerate the cloud_map in 3D using the depth camera instead. Note that with Grid/DepthDecimation=1 (default 4) a lot of CPU is required to generate/update the cloud.

ros2 run rtabmap_util map_assembler --ros-args \
   -p use_sim_time:=true \
   -p Grid/CellSize:="'0.01'" \
   -r mapData:=/mapData \
   -r __ns:=/map_assembler \
   -p rtabmap:="/rtabmap" \
   -p Grid/Sensor:="'1'" \
   -p Grid/DepthDecimation:="'1'" \
   -p regenerate_local_grids:=true

Screenshot from 2024-09-11 23-43-44