Help optimizing RTAB-Map for outdoor agricultural mapping with RealSense D455

[VanjaLjubobratovic]

Hello, I'm working on a project which aims to use RealSense D455 and RTAB-Map in ROS2 in order to create an areal view map of agricultural terrains. The idea is that those systems would already be in place on an autonomous plant sprayer, used for something else, and they would create the map on the side so you can monitor the plants.

I have already developed and set up everything needed for that, but the results I'm getting are not really satisfying so I was wondering if anyone could point me in the right direction.

The main issue I'm facing is point cloud spray which you can see in the next couple of images:

From a different angle the same area looks fairly decent

But obviously when I rotate everything so I can get an areal view, the results aren't really usable and you can clearly see all the "smearing" I'm struggling with.

My camera launch parameters:

#!/bin/bash

resolution="848x480x60"

LC_NUMERIC="en_US.UTF-8"; ros2 launch realsense2_camera rs_launch.py \
rgb_camera.profile:=$resolution \
depth_module.profile:=$resolution \
enable_gyro:=true \
enable_accel:=true \
unite_imu_method:=2 \
gyro_fps:=400 \
accel_fps:=200 \
enable_infra1:=false \
enable_infra2:=false \
enable_sync:=false \
align_depth.enable:=true \
pointcloud.enable:=false \
initial_reset:=true \
depth_module.enable_auto_exposure:=true \
decimation_filter.enable:=false \
temporal_filter.enable:=true \
spatial_filter.enable:=true \
json_file_path:=/home/vanja/RTAB-Map-Orthophoto/configs/MediumDensity.json

RTAB-Map launch parameters:

LC_NUMERIC="en_US.UTF-8"; ros2 launch rtabmap_launch rtabmap.launch.py \
rtabmap_args:="--delete_db_on_start" \
rviz:=true \
rtabmap_viz:=false \
frame_id:=camera_link \
rgb_topic:=/camera/color/image_raw \
depth_topic:=/camera/aligned_depth_to_color/image_raw \
camera_info_topic:=/camera/color/camera_info \
approx_sync:=false \
wait_imu_to_init:=true \
queue_size:=300 \
cfg:=/home/vanja/RTAB-Map-Orthophoto/configs/rtab_config.ini\

RTAB-Map config file: rtab_config.txt

I would appreciate any suggestions on this. Also, please feel free to ask me about any additional information like databases, config files, launch files and similar. Thank you!

[matlabbe]

not really satisfying

That looks pretty good to me! ;)

The "spray" effect is caused by the realsense's disparity approach, it will interpolate depth on the edges of objects, so that there are interpolated disparity between the foreground and background. This can be removed by adjusting the depth confidence on realsense driver. I think you can feed a preset to realsense2 ros driver to change how depth is computed, which could remove the interpolated disparity around foreground objects.

[VanjaLjubobratovic]

Ah, ok. So it's definitely the camera's parameters I should be playing with. I'll try that and report on the results. Thanks!

[VanjaLjubobratovic]

@matlabbe Hi, I think I found a solution I find satisfying and I will just repost what I wrote on RealSense GitHub.

So after some experimentation I have managed to find a solution which I would call satisfactory.

In terms of camera settings:

• I've set temporal filter off as per documentation it can increase point cloud spray if the camera is moving
• I'm using the optimal profile which is 848x480x60 as per the documentation
• I'm using some of the camera presets. High Accuracy if there will be no filtering done on the cloud afterwards, and if you are filtering as I will be, maybe medium density can work great also, because it wont remove as much good points from your clouds.

In terms of camera operation:

• I'm recording at as big of an angle to the ground as I can since most of the errors are along the X axis, if the X axis points towards the ground, when I rotate the cloud to get an areal view, a lot of the spray will be covered up by good pixels

In terms of filtering:

• I have my custom node which is used to filter the point clouds produced by RTAB-Map and I'm using distance thresholding (0.5m to 4.0m). Already implemented by a RTAB-Map function for decoding messages on /rtabmap/mapData topic.
• My custom filter which calculates the median distance and standard deviation of all the points from the camera in the XY plane and removes those which don't fit into the specified interval. The effect is that point cloud edges are "trimmed". This removes a lot of outlier clusters where some outliers are being clustered together in the middle of empty space.
• PCL Statistical Outlier Removal filter which is specifically made to remove point cloud spray. It is used last because it is ineffective at removing clusters of outliers. Since I'm concerned with real time applications I was forced to make a multi threaded implementation which is decent for my needs.

The results of filtering on the point cloud RECORDED WITH ORIGINAL CAMERA SETTINGS: BEFORE

AFTER

When all of the steps above are actually combined, the result should be quite good. Here's my code if someone's interested in implementation details

I hope someone finds this useful.

[matlabbe]

Great! thx for sharing!