feat(image_projection_based_fusion): add instance segmentation fusion

[StepTurtle]

Description

This PR adds a new image projection based fusion method which fuses instance segmentation results and point cloud.

Related links

Parent Issue:

• Implement RTMDet to Perception Pipeline #7235

How was this PR tested?

1) Rebase this PR onto autoware.universe main branch

2) Build the package and other dependencies of package

cd autoware
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-up-to image_projection_based_fusion

3) Update the topic parameters from instance_segmentation_pointcloud_fusion.launch.xml and run the launch file

ros2 launch image_projection_based_fusion instance_segmentation_pointcloud_fusion.launch.xml

It needs following topics:

• sensor_msgs/msg/CameraInfo (for each camera)
• sensor_msgs/msg/PointCloud2
• tier4_perception_msgs/msg/DetectedObjectsWithMask (for each camera) (It comes from tensorrt_rtmdet package)

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

[github-actions]

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

• You've checked our contribution guidelines.
• Your PR follows our pull request guidelines.
• All required CI checks pass before marking the PR ready for review.

[StepTurtle]

Currently there is a problem something like following: Video

The FusionNode base class was used for the Message Time Filter, but I couldn't manage to get the ROIs and lidar synchronized in any way. It usually only manages to fuse a single ROI and lidar.

I used NuScenes dataset and the timestamps looks like this:

There are following time gaps between sensors:

Two Lidar Message: ~50 ms
Lidar and Front Left Camera: ~7 ms
Lidar and Front Camera: ~15 ms
Lidar and Front Right Camera: ~22 ms
Lidar and Back Right Camera: ~31 ms
Lidar and Back Camera: ~40 ms
Lidar and Back Left Camera: ~49 ms

I used following parameters for FusionNode:

/**:
  ros__parameters:
    input_offset_ms: [0.0, 15.0, 25.0, 30.0, 40.0, 50.0, 0.0, 0.0]
    timeout_ms: 70.0
    match_threshold_ms: 50.0
    image_buffer_size: 15
    debug_mode: false
    filter_scope_min_x: -100.0
    filter_scope_min_y: -100.0
    filter_scope_min_z: -100.0
    filter_scope_max_x: 100.0
    filter_scope_max_y: 100.0
    filter_scope_max_z: 100.0

I guess the input_offset_ms is using to avoid time diff caused by 2D detection (YOLOX etc.). So I used for avoid the time diff caused by sensors. I also set RTMDet output times equal with image message times.

Do you have any idea about what did i do wrong?,

Sorry for the pings, feel free to ignore. I just ping the people suggested for review to ask question. I guess you are the maintainers of image_projection_based_fusion @badai-nguyen @storrrrrrrrm

[Shin-kyoto]

Thank you for your PR!
I have one question.
The visualization results are certainly out of sync, but I also think there is a delay in the visualization. Is it misaligned even without visualization delay?

And @tzhong518 , you have gained a lot of experience with offset settings during the development of pointpainting. Could you please provide some comments?

[StepTurtle]

Thanks for your interest @Shin-kyoto

The visualization results are certainly out of sync, but I also think there is a delay in the visualization. Is it misaligned even without visualization delay?

I forgot to say. I used -r 0.2 parameter when I played rosbag. I guess It caused the delay you saw.

And yes it is misaligned. The callback is kindda slow right now because of the number of the instance segmentation images. I have to optimize the callback. Do you think it cause the sync problem of ROIs and lidar. If yes, I also would like you to take a look at https://github.com/orgs/autowarefoundation/discussions/5047 so I can update mag type and optimize the fusion function somehow.

[Shin-kyoto]

Thanks for your interest @Shin-kyoto

The visualization results are certainly out of sync, but I also think there is a delay in the visualization. Is it misaligned even without visualization delay?

I forgot to say. I used -r 0.2 parameter when I played rosbag. I guess It caused the delay you saw.

And yes it is misaligned. The callback is kindda slow right now because of the number of the instance segmentation images. I have to optimize the callback. Do you think it cause the sync problem of ROIs and lidar. If yes, I also would like you to take a look at https://github.com/orgs/autowarefoundation/discussions/5047 so I can update mag type and optimize the fusion function somehow.

Thank you for your comments.
First, it is important to check whether synchronization occurs without the impact of visualization delays or not. Can you consider the following options?

• Play the rosbag at a very slow speed.
• Use input_offset_ms and save log the timestamps of the images from the ROIs topics and the LiDAR topic. Then, use the LiDAR point cloud and the segmentation images created from the ROIs topics at the corresponding timestamps in the nuScenes dataset for visualization.

[StepTurtle]

@Shin-kyoto

Hi again, I guess I could not understand your idea totally. Here is the timestamps of rois and lidar from subCallback:

times.csv

I didn't fully understand the rest of what you said. Please can you explain in different way? When visualization, do you want me to use the original dataset along with the ROIs I generated, or should I only use the lidar and camera data from the dataset and is the main thing we expect to see the lidar and camera data fitting together properly?

Also I just realize I put a wrong video. The past video includes the data from our test vehicle. The outputs and issues are same but I updated the video link anyway. You can see new video from this link (includes nuScenes dataset): Link

[Shin-kyoto]

Please can you explain in different way?

OK.

First, it is important to check whether synchronization occurs without the impact of visualization delays or not. So, please play the rosbag at a very slow speed.

Second, by visualizing your dataset, please check which timestamped point cloud data and which timestamped images should be assigned. Then, verify which timestamped point cloud data and which timestamped images are actually being assigned when running Autoware.

[Shin-kyoto]

@tzhong518 , you have gained a lot of experience with offset settings during the development of pointpainting. Could you please provide some comments?

[StepTurtle]

OK @Shin-kyoto, these are timestamps from Autoware and Dataset:

These are the timestamps from dataset and synced timestamps should be like that: synced_should_be.csv

These are the timestamps from Autoware: from_autoware.csv

Like you mentioned, I compared the timestamps from Autoware callback and dataset, and it look like it is not as it should be and often takes previous and next timestamps.

I think we have to tune parameters somehow, right?

[Shin-kyoto]

@StepTurtle

First of all, the timestamp of lidar topic in from_autoware.csv (e.g. 1.53315160354759E+018) is NOT in synced_should_be.csv. Please check your pointcloud publisher.

[StepTurtle]

@StepTurtle

First of all, the timestamp of lidar topic in from_autoware.csv (e.g. 1.53315160354759E+018) is NOT in synced_should_be.csv. Please check your pointcloud publisher.

It because, point cloud hz is bigger than camera hz and I did not put the pointclouds does not match camera data. There is actually a pointcloud frame with 1.53315160354759E+018 timestamp in dataset but it not saved to csv file because it shouldn't match with any camera.

[Shin-kyoto]

@StepTurtle
First of all, the timestamp of lidar topic in from_autoware.csv (e.g. 1.53315160354759E+018) is NOT in synced_should_be.csv. Please check your pointcloud publisher.

It because, point cloud hz is bigger than camera hz and I did not put the pointclouds does not match camera data. There is actually a pointcloud frame with 1.53315160354759E+018 timestamp in dataset but it not saved to csv file because it shouldn't match with any camera.

@StepTurtle

Please publish the point cloud at 10Hz for easy debugging. Please thin out the point cloud included in the dataset at 20Hz to 10Hz.

The timestamps of the image included in synced_should_be.csv do not match the timestamps of the image included in from_autoware.csv. Why?
For example, 1533151604047405 in synced_should_be.csv is not included in from_autoware.csv.

[StepTurtle]

Hi again, @Shin-kyoto

Please publish the point cloud at 10Hz for easy debugging. Please thin out the point cloud included in the dataset at 20Hz to 10Hz.

The data I used is nuScenes dataset which converted to ROS 2 bag file with open source tool, because of that I did not set the point cloud hz before but you are right it will be better to use 10 hz lidar. So let me try to update the tool, but it could be take a little bit time, because I have to do some changes in converter tool to reduce the lidar hz.

The timestamps of the image included in synced_should_be.csv do not match the timestamps of the image included in from_autoware.csv. Why? For example, 1533151604047405 in synced_should_be.csv is not included in from_autoware.csv.

About this one, actually this stamp is in both csv file. The resolution of the number is one digit short in from_autoware.csv, and the number is rounded by one digit. In this case, it's caused by the converter tool as well, which rounds the number by one digit when saving the message. However, I don't think this will be an issue because the rounded part is the same for all camera messages, and if I'm not mistaken, it corresponds to half a millisecond in time.

I created the diagram below to symbolize the data in the csv files, and the timestamps form a similar pattern.

The part I don't understand is that when the FusionNode::subCallback function is called, the most recent point cloud is used, and there's no chance to use the cameras that come after the point cloud. Additionally, in this case, I used negative offset values to match the Lidar and ROIs ([-15.0, -22.0, -7.0, -40.0, -31.0, -45.0, 0.0, 0.0], the camera order is different here, but these numbers are equal to the numbers shown in the diagram), because within the callback, the offset is added to the point cloud's timestamp to perform the calculation. Do you think it would be correct to use the latest point cloud and the ROI messages that came before it?

Additionally, I haven't seen the fuseOnSingleImage function being executed within the FusionNode::roiCallback function. This situation is likely due to the fact that all ROIs have already arrived by the time the point cloud arrives, and therefore processing is done within FusionNode::subCallback.

By the way, I've been taking up a bit too much of your time lately, and I'm sorry for that.