[BUG] {Problem with 3D Reconstruction}

[lucianoperetti]

Describe the bug

I have a problem with rebuilding the pointcloud using the latest version of depthai_ros on the commit 0144d79. In the following video, you can see how the reconstruction changes place randomly when the camera is still:

PointcloudNewDriver.webm

I'm also attaching a second video recorded with our fork (made 8 months ago approx.) in which the reconstruction seems to be stable. You can see in the following video:

PontcloudOldDriver1.webm

Minimal Reproducible Example

We use the launch file:

<node_container pkg="rclcpp_components" exec="component_container_mt" name="pipeline" namespace="oak">

<composable_node pkg="depthai_ros_driver" plugin="depthai_ros_driver::Camera" name="camera" namespace="oak">
  <param from="$(find-pkg-share avisense_base)/params/oak.yaml"/>
  <extra_arg name="use_intra_process_comms" value="true"/>
</composable_node>

<composable_node pkg="image_proc" plugin="image_proc::RectifyNode" name="rgb_rectifier" namespace="oak">
  <remap from="image" to="camera/rgb/image_raw"/>
  <remap from="camera_info" to="camera/rgb/camera_info"/>
  <remap from="image_rect" to="camera/rgb/image_rect"/>
  <remap from="image_rect/compressed" to="camera/rgb/image_rect/compressed"/>
  <remap from="image_rect/compressedDepth" to="camera/rgb/image_rect/compressedDepth"/>
  <remap from="image_rect/theora" to="camera/rgb/image_rect/theora"/>
  <extra_arg name="use_intra_process_comms" value="true"/>
</composable_node>

</node_container>

We use the params file:

camera:
i_enable_imu: true
i_enable_ir: true
i_pipeline_type: RGBD
i_nn_type: none
i_laser_dot_brightness: 1000
i_floodlight_brightness: 700
i_ip: ''
i_mx_id: ''
i_usb_port_id: ''
i_usb_speed: SUPER_PLUS
i_pipeline_dump: false
i_calibration_dump: false
i_external_calibration_path: ''
imu:
i_acc_mode: linear
i_acc_freq: 50
i_acc_cov: 0.003
i_gyro_mode: calibrated
i_gyro_freq: 50
i_gyro_cov: 0.003
i_mag_mode: calibrated
i_mag_freq: 50
i_enable_rotation: true
i_message_type: IMU
i_rot_mode: default
i_rot_freq: 50
i_rot_cov: 0.003
i_sync_method: LINEAR_INTERPOLATE_ACCEL
i_rotation_vector_type: ROTATION_VECTOR
rgb:
i_board_socket_id: 0
i_fps: 10.0
i_width: 640
i_height: 360
i_resolution: 1080P
r_focus: 0
r_set_man_focus: false
i_isp_num: 1
left:
i_board_socket_id: 1
i_fps: 10.0
i_width: 1280
i_height: 720
i_resolution: 720P
i_publish_topic: true
right:
i_board_socket_id: 2
i_fps: 10.0
i_width: 1280
i_height: 720
i_resolution: 720P
i_publish_topic: true
stereo:
i_align_depth: true
i_board_socket_id: 0
i_publish_right_rect: true
i_left_socket_id: 1
i_right_socket_id: 2
i_depth_filter_size: 7
i_enable_distortion_correction: true
i_enable_speckle_filter: true
i_depth_preset: HIGH_ACCURACY
i_stereo_conf_threshold: 230
i_subpixel: true
i_width: 640
i_height: 360
i_lr_check: true
i_extended_disp: true
i_disparity_width: DISPARITY_96

Expected behavior

I hope it works like the old driver, which can be seen in the second video.

You can save it in depthai_ros_driver, either by calling /save_pipeline ROS service, or by setting parameter camera.i_pipeline_dump in ROS 2 or camera_i_pipeline_dump in ROS. Pipeline dump is saved to /tmp/pipeline.json.

Attach system log

• Ubuntu 22.04
• ROS2
• Humble

Additional context
We have more than 10 oak D pro cameras working well, but in the new version bought in February, we have problem to launch the system with our Fork because there is an inconpatibility between the boot system of the new cameras and the old version of depthai_ros. For these reason we need to update the driver to the new version but we have some problems with the new driver.

[Serafadam]

Hi, thanks for the report, would it be possible for you to share a rosbag with topics available? Also, what is the CPU/RAM usage when the driver is running?

[lucianoperetti]

Hi, thanks for the report, would it be possible for you to share a rosbag with topics available? Also, what is the CPU/RAM usage when the driver is running?

This rosbag contain the main topic from de camera. You can find the pointcloud in the topic /environment_points . If you want to see on rviz, you need to change the qos of /environment_points to best_effort.

rosbag link:
https://drive.google.com/drive/folders/1WGnM5Xltjr3b60gvSPU53p0B2VB1XDTg?usp=drive_link

On this picture, you can see the CPU/RAM usage:

[MariuszSzczepanikSpyrosoft]

@Serafadam do you have some update if there is some way to workaround mentioned issue? Maybe we could disable some functionalities on the OAK camera to avoid issues? If you have an idea how to process I would appreciate your sharing it with us.

[Serafadam]

Hi @MariuszSzczepanikSpyrosoft @lucianoperetti, I investigated a bit and unfortunately I wasn't able to replicate the issue, although I might've found a clue - stereo/camera_info topic seems to change randomly and it shouldn't as it is set once at the startup and is being taken from CameraInfoManager, which theoretically could make it possible to change that message in runtime, although this shouldn't happen randomly. I would check if anything calls for updates to calibration or if there is some other publisher that hijacked this topic.

[MariuszSzczepanikSpyrosoft]

@Serafadam currently from our software side: we didn't publish on topic /stereo/camera_info, only reading using the subscription mechanism.

Could you tell me more about: "I would check if anything calls for updates to calibration" and how this could be checked? some dedicated topics messages?

Second question: after upgrading OAK drivers to the newest ones do we need to recalibrate the camera/IMU if old drivers work correctly on the same hardwares?

[Serafadam]

@MariuszSzczepanikSpyrosoft

Could you tell me more about: "I would check if anything calls for updates to calibration" and how this could be checked? some dedicated topics messages?

I would check with rqt_graph how the node graph looks and whether there is another node publishing on the camera_info topic. CameraInfoManager has a service for changing the calibration data, I don't think that on successful calibration it will print logs so you could try build it from source and add some debug prints. Additionally you could build driver from source and print the info message before publishing.

Second question: after upgrading OAK drivers to the newest ones do we need to recalibrate the camera/IMU if old drivers work correctly on the same hardwares?

The calibration values are the same, with one of the future updates we will add IMU extrinsics but that shouldn't affect the driver much.

[MariuszSzczepanikSpyrosoft]

Hi @Serafadam
Thank you for your recommendations regarding the rqt_graph and source modifications. We've checked our side and haven't found any publications to the camera_info topic, it appears these might be internal to Luxonis. Could you check if there's been any improvement in the new drivers that would allow us to utilize the latest cameras effectively?

Appreciate your support in resolving this.

[Serafadam]

Hi, could you checkout this branch and see what is the output? Additionally it would be good to compare it with what appears on topic itself

[MateoGiova]

Hi @Serafadam.
I tested your branch, but the function where you added the logger never executes. I added a similar logger in the camera configuration function to discard a logger error, but there I do see the message. Basically I added the message below the confirmation that the camera is ready:

RCLCPP_INFO(this->get_logger(), “Camera ready!”);
RCLCPP_INFO(rclcpp::get_logger(“oak”), “info width 1: ”);

However, I never see the message coming from the file
depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp, so I can assume that function is not executed, since I don't see the message, even adding a log outside the if.

[Serafadam]

Hi @MateoGiova, you might need to subscribe to the stereo topic (should be enough to display it using rviz or rqt) for those logs to appear

[MateoGiova]

Hi @Serafadam I subscribe to the stereo topic (and the pointcloud topic) by ros2 topic echo ... and with rviz but the logs never appear.

[Serafadam]

Hi @MateoGiova I have updated the branch to publish the information while using IPC and changed the launch files a bit to enable IPC in them, although I haven't been able to reproduce the issue where two publishers are spawned for /stereo/camera_info, I would suggest starting from minimal launch file and seeing if that behavior can be reproduced with it