Depth compression in rtabmap_msgs/SensorData

classic Classic list List threaded Threaded
3 messages Options
ana
Reply | Threaded
Open this post in threaded view
|

Depth compression in rtabmap_msgs/SensorData

ana
Hello,

I am using RTABMap SLAM with RGBD images and would like to further process the RGB and depth images associated with RTABMap key frames in real time.

Here is the command I used to launch RTABMap:
```
ros2 launch rtabmap_launch rtabmap.launch.py    
rtabmap_args:="--delete_db_on_start"    
rgb_topic:=/zed/rgb/image_rect_color    
depth_topic:=/zed/depth/depth_registered    
camera_info_topic:=/zed/depth/camera_info    
frame_id:=zed_camera_link    
imu_topic:=/zed/imu/data
```    

To find the RGB and depth images associated with RTABMap key frames in real time, I am using the `get_map_data` service to get the rtabmap_msgs/MapData message and the rtabmap_msgs/SensorData message within it. The raw images `left` and `right` are both empty. The compressed RGB image `left_compressed` looks fine, and I am able to successfully uncompress it in Python using OpenCV. But the `right_compressed` image seems to be in an RGBA format. It is the same resolution as my depth image, but with four channels ranging from 0-255, instead of one channel ranging from 0-10000. I am having trouble uncompressing this into a useful format.

If I create my map first and export my database using this command:
`rtabmap-export --images_id --poses_camera --poses_format 11 <path-to-database>.db`

Then the depth images look great. Here is the format:
rtabmap_export depth Image Header:
Format: PNG
Mode: I;16
Size: (640, 360)

But when I use the get_map_data service as described, or even the get_node_data service, this is the format:
ros get_node_data, get_map_data service right_compressed Image header:
Format: PNG
Mode: RGBA
Size: (640, 360)

How would you recommend uncompressing the depth images from the map?
ana
Reply | Threaded
Open this post in threaded view
|

Re: Depth compression in rtabmap_msgs/SensorData

ana
In case anyone else has this issue in the future:

The depth images can be uncompressed by combining all four 8 bit channels as a single 32 bit float and then converting this 32 bit float to a 16 bit uint by removing NaNs and any values greater than 2^16.
Reply | Threaded
Open this post in threaded view
|

Re: Depth compression in rtabmap_msgs/SensorData

matlabbe
Administrator
If depth images are 32 bits float, they are compressed with opencv as RGBA images. See c++ code to see how it is handled:
https://github.com/introlab/rtabmap/blob/623d056436946c35beb12199831f178a84eaad73/corelib/src/Compression.cpp#L140-L147

After decoding into RGBA, we recast the whole data structure from 8UC4 format to float 1 channel.