feat(dummy_perception_publisher): publish realistic dummy pointcloud using raymarchig

[HiroIshida]

Related Issue(required)

• This PR fixes Difficult to raytrace #413
  In principle, the new algorithm geometrically simulate the ray from the vehicle (see the explanation below) so the problem mentioned the issue will not occur.

• This PR fixes ray tracing in dummy perception publisher is too heavy to publish objects in 10Hz #460
  Experiments varying the pedestrians around the ego: (x: number of pediestrian, y: elapsed time in the callback)

CLICK BELOW for the detail of the experiment

detail

The code for plotting

import matplotlib.pyplot as plt
pre = [0.011, 0.028, 0.119, 0.369] 
post = [0.027, 0.0325, 0.0335, 0.045, 0.0569]
plt.plot([3*2**N for N in range(len(pre))], pre, 'ro-', label='previous')
plt.plot([3*2**N for N in range(len(post))], post, 'bo-', label='after PR')
plt.legend()
plt.grid()
plt.show()

Similar to the condition mentioned in #460, I set leaf size to 0.02 and enable raytracing by HiroIshida@87ad586
Before this PR:
3 pedestrians (later just peds.).

[dummy_perception_publisher_node-32] [INFO] [1651098705.458081354] [ishida_debug]: elapsed in cb: 0.0111047
[dummy_perception_publisher_node-32] [INFO] [1651098705.558121352] [ishida_debug]: elapsed in cb: 0.0111413
[dummy_perception_publisher_node-32] [INFO] [1651098705.658110000] [ishida_debug]: elapsed in cb: 0.0111253

6 peds.

[dummy_perception_publisher_node-32] [INFO] [1651098755.475391737] [ishida_debug]: elapsed in cb: 0.0284033
[dummy_perception_publisher_node-32] [INFO] [1651098755.580160119] [ishida_debug]: elapsed in cb: 0.0329564
[dummy_perception_publisher_node-32] [INFO] [1651098755.677777064] [ishida_debug]: elapsed in cb: 0.0307617

12 peds.

[dummy_perception_publisher_node-32] [INFO] [1651098785.779767720] [ishida_debug]: elapsed in cb: 0.119255
[dummy_perception_publisher_node-32] [INFO] [1651098785.919841666] [ishida_debug]: elapsed in cb: 0.139978
[dummy_perception_publisher_node-32] [INFO] [1651098786.048075382] [ishida_debug]: elapsed in cb: 0.128161

24 peds

[dummy_perception_publisher_node-32] [INFO] [1651098830.236009168] [ishida_debug]: elapsed in cb: 0.369423
[dummy_perception_publisher_node-32] [INFO] [1651098830.588221277] [ishida_debug]: elapsed in cb: 0.352129
[dummy_perception_publisher_node-32] [INFO] [1651098830.940979519] [ishida_debug]: elapsed in cb: 0.352684

After this PR:
3 peds.

[dummy_perception_publisher_node-32] [INFO] [1651098380.290698191] [ishida_debug]: elapsed in cb: 0.0271508
[dummy_perception_publisher_node-32] [INFO] [1651098380.389969137] [ishida_debug]: elapsed in cb: 0.0264274
[dummy_perception_publisher_node-32] [INFO] [1651098380.489191225] [ishida_debug]: elapsed in cb: 0.025653

6 peds

[dummy_perception_publisher_node-32] [INFO] [1651098399.996082245] [ishida_debug]: elapsed in cb: 0.0325398
[dummy_perception_publisher_node-32] [INFO] [1651098400.091935234] [ishida_debug]: elapsed in cb: 0.0283962
[dummy_perception_publisher_node-32] [INFO] [1651098400.192519080] [ishida_debug]: elapsed in cb: 0.0289406

12 peds

[dummy_perception_publisher_node-32] [INFO] [1651098446.797084462] [ishida_debug]: elapsed in cb: 0.033536
[dummy_perception_publisher_node-32] [INFO] [1651098446.904084056] [ishida_debug]: elapsed in cb: 0.0405116
[dummy_perception_publisher_node-32] [INFO] [1651098447.000447143] [ishida_debug]: elapsed in cb: 0.0368933

24 peds

[dummy_perception_publisher_node-32] [INFO] [1651098491.308734791] [ishida_debug]: elapsed in cb: 0.0451902
[dummy_perception_publisher_node-32] [INFO] [1651098491.409088687] [ishida_debug]: elapsed in cb: 0.0455172
[dummy_perception_publisher_node-32] [INFO] [1651098491.511595902] [ishida_debug]: elapsed in cb: 0.0480176

48 pedes

[dummy_perception_publisher_node-32] [INFO] [1651098579.820494309] [ishida_debug]: elapsed in cb: 0.0569535
[dummy_perception_publisher_node-32] [INFO] [1651098579.924211786] [ishida_debug]: elapsed in cb: 0.0606381
[dummy_perception_publisher_node-32] [INFO] [1651098580.021794651] [ishida_debug]: elapsed in cb: 0.0582257

TODO list after first review

• strategy pattern
• ray tracing for multiple object at once
• move deafult ray tracing method to ObjectCentricPointcloudGeneratir

Description(required)

The original PR is https://github.com/tier4/autoware.iv/pull/1809

This PR solves
#460 because previous method is two-stage, but the new method directly generate the pointcloud by sphere tracing.
#413 because the new method is free from parameter tuning like in the previous method (leaf size)

Originally, the dummy point cloud was generated by two-steps: 1) generate point around the edges of square of objects. 2) then descritize [-pi, pi] by cells indicated by intervals between blue lines in the following figure. Then among points falls into each cell, pick the one which has smallest distance from the vehicle. For example, the points annotated with "selected" is picked for cell 3. As you can see, the point cloud generation process does not well model the real generation.

The problem of this method becomes prominent in the following two cases:

1. if interval between each red point is set to large, even if angle discretization is precise enough, the resulting pointcloud will be too sparse.

2. if the interval between red points are too small, resulting pointcloud can be exist on back side of the object, which is physically strange.

On the other hand, the newer implementation simulate the point cloud generation process by lidar by ray-casting. Simply we simulate the equally interval-ed rays marching (blue lines), and collision points (indicated as red points in the figure below) of each rays will be finally used as the pointcloud.

Here, we used sphere-marching (or raymarching) method described here. The sphere marching method assumes that signed distance field can be computed for each object. In planning simulator, dummy objects (vehicle and pedestrian) are considered as a square, so it can easily be [computed analytically].(https://computergraphics.stackexchange.com/questions/161/what-is-ray-marching-is-sphere-tracing-the-same-thing/163)

From these difference of pointcloud generation process, I named the old method as createObjectPointCloudObjectCentric and the new method as createObjectPointCloudVehicleCentric. One can select from two methods, by using object_centric_pointcloud: bool parameter.

demo with old method

old_demo_apr7.mp4

demo with new method

Pointcloud is physically faithful.

new_demo_apr7.mp4

Review Procedure(required)

Related PR(optional)

https://github.com/tier4/autoware.iv/pull/1809

Pre-Review Checklist for the PR Author

PR Author should check the checkboxes below when creating the PR.

• Read commit-guidelines
• Assign PR to reviewer

If you are adding new package following items are required:

• Documentation with description of the package is available
• A sample launch file and parameter file are available if the package contains executable nodes

Checklist for the PR Reviewer

Reviewers should check the checkboxes below before approval.

• Commits are properly organized and messages are according to the guideline
• PR title describes the changes

Post-Review Checklist for the PR Author

PR Author should check the checkboxes below before merging.

• All open points are addressed and tracked via issues or tickets

CI Checks

• Build and test for PR / build-and-test-pr: Required to pass before the merge.
• Build and test for PR / clang-tidy-pr: NOT required to pass before the merge. It is up to the reviewer(s). Found false positives? See the [guidelines][clang-tidy-guidelines].
• Check spelling: NOT required to pass before the merge. It is up to the reviewer(s). See here if you want to add some words to the spell check dictionary.

[codecov]

Codecov Report

Merging #527 (09a120a) into main (b2f90a0) will increase coverage by 0.02%.
The diff coverage is 14.39%.

@@           Coverage Diff            @@
##            main    #527      +/-   ##
========================================
+ Coverage   9.64%   9.66%   +0.02%     
========================================
  Files        907     911       +4     
  Lines      56508   56765     +257     
  Branches    6693    6726      +33     
========================================
+ Hits        5451    5488      +37     
- Misses     46566   46763     +197     
- Partials    4491    4514      +23     

Flag	Coverage Δ		*Carryforward flag
differential	5.36% <15.94%> (?)
total	9.64% <0.00%> (ø)		Carriedforward from db00da5

*This pull request uses carry forward flags. Click here to find out more.

Impacted Files	Coverage Δ
...lisher/include/dummy_perception_publisher/node.hpp	0.00% <0.00%> (ø)
simulator/dummy_perception_publisher/src/node.cpp	0.00% <0.00%> (ø)
...my_perception_publisher/src/pointcloud_creator.cpp	0.00% <0.00%> (ø)
...blisher/test/src/test_signed_distance_function.cpp	33.33% <33.33%> (ø)
..._perception_publisher/signed_distance_function.hpp	42.85% <42.85%> (ø)
...ception_publisher/src/signed_distance_function.cpp	100.00% <100.00%> (ø)
...ption_rviz_plugin/src/tools/interactive_object.hpp	0.00% <0.00%> (ø)

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update b2f90a0...09a120a. Read the comment docs.

[yukkysaito]

I'll review today

[yukkysaito]

@HiroIshida
Thank you very much.
I think it is better to remove createObjectPointcloudObjectCentric and use only createObjectPointcloudVehicleCentric.
Or it would be better to create an interface class for createObjectPointcloud to be able to switch methods.
reference links:

• strategy pattern
  • https://qiita.com/knknkn1162/items/9a65ca65b558f31121f0
• example of switching the Kalman filter model depending on the class
  • https://github.com/autowarefoundation/autoware.universe/blob/main/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp

[yukkysaito]

By the way, it generates a point by ray tracing between objects after ray tracing each object, but is it possible to do it simultaneously with sphere-marching, including between objects at the same time?

[HiroIshida]

@yukkysaito
Thank you for your review.
I understand. I would chose leaving the old class method in case.

By the way, it generates a point by ray tracing between objects after ray tracing each object, but is it possible to do it simultaneously with sphere-marching, including between objects at the same time?

sure, your suggestion will make the algorithm lot faster. I will certainly implement it.

[HiroIshida]

@yukkysaito

I have two questions.

1. What is the purpose of DetectedObjectsWithFeature/Feature/cluster message compute in this node? Originally, this cluster is copied from the pointcloud generated by object-centric ray tracing,

autoware.universe/simulator/dummy_perception_publisher/src/node.cpp

Line 149 in 939a5db

pcl::toROSMsg(*pointcloud_ptr, feature_object.feature.cluster);

,

so cluster is uniformly surrounding the object. But if I modify the code as this PR, generated pointcloud will no longer be uniformly surrounding the object. Is it intended behavior?

2. On the random noise on the point cloud
   Currently, the random noise on the pointcloud is following the dummy_perceptionpublisher_msg::Object::initial_state::pose_covaraince of the subscribed message. To my understanding, object message is more like a recognition (e.g. deep point cloud network) rather than low-level perception, so it seems bit strange that recognition's uncertainty affects point cloud generation. I think it is more straightforward to define a low level perception error level and randomize the pointcloud based on that. Based on the this point, should we modify this in the future PR? (I think this is out of scope of this PR, by the way)

Note: Actually, the second question is related to the first question. I suppose that dummy pointcloud published by this node should be randomized based on low-level perception uncertainty, but on the other hand, the cluster in DetecedObjectWithFeature should be randomized based on recognitoin-level uncertainty.

[yukkysaito]

What is the purpose of DetectedObjectsWithFeature/Feature/cluster message compute in this node? Originally, this cluster is copied from the pointcloud generated by object-centric ray tracing,

This is to perform shape fitting so that part of Perception can also be tested.

I think it should be put in after ray traced.

[yukkysaito]

On the random noise on the point cloud
Currently, the random noise on the pointcloud is following the dummy_perceptionpublisher_msg::Object::initial_state::pose_covaraince of the subscribed message. To my understanding, object message is more like a recognition (e.g. deep point cloud network) rather than low-level perception, so it seems bit strange that recognition's uncertainty affects point cloud generation. I think it is more straightforward to define a low level perception error level and randomize the pointcloud based on that. Based on the this point, should we modify this in the future PR? (I think this is out of scope of this PR, by the way)

You are right. The point cloud and object-level noise should be separated.
This can be outside the scope of this PR.

[HiroIshida]

Thank you for your reply. I understand. I will probably make a new PR about the object-level noise lidar-level noise separation.

[taikitanaka3]

@HiroIshida
Thank you for your nice contribution. Can you link issues if this PR resolves one of issues below ?

• ray tracing in dummy perception publisher is too heavy to publish objects in 10Hz #460
• Difficult to raytrace #413

[yukkysaito]

@HiroIshida
TODO? and comment

• Implement sphere-marching, including between objects. feat(dummy_perception_publisher): publish realistic dummy pointcloud using raymarchig #527 (comment)
• I would like to change the following naming. Does vehicle in VehicleCentricPointCloudCreator mean ego?
  • ObjectCentricPointCloudCreator
  • VehicleCentricPointCloudCreator
• I would like to set ray trace between objects and VehicleCentricPointCloudCreator to true by default.

[HiroIshida]

@taikitanaka3 Thank you for the links. I think this PR fixes the mentioned two issues. So I mention that in the PR description. However, let me check more before finalizing this PR.

[HiroIshida]

@yukkysaito Thanks for comments. I am fixing following your TODO list.

I would like to change the following naming. Does vehicle in VehicleCentricPointCloudCreator mean ego?

Sorry, I couldn't get the request of this line. Are you request to change from VehicleCentricPointCloudCreator to EgoCentricPointCloudCreator ?

[HiroIshida]

Now, this PR is ready to review

[yukkysaito]

@HiroIshida
If I merge this PR, it doesn't work until I place an obstacle in the planning simulator.

[HiroIshida]

Thanks @yukkysaito
If I understand correctly, the problem is that /perception/obstacle_segmentation/pointcloud was not published when no dummy objects are placed. And, even when no objects are places, empty pointcloud must be published.

If my understanding above is correct, this bug is fixed by this commit 7b519c6

[yukkysaito]

@HiroIshida Thank you! Can you resolve conflict and others(CI) to merge?

[HiroIshida]

@yukkysaito Thanks for the bug report. I found it's a bug due to me. I fixed this bug in db00da5

The reason for the bug is that, when object is put far away, objects_ is empty, thus selected_indices in the code is also empty. So, in such case merged_pointcloud_ptr is not initialized correctly.

When merged_pointcloud_ptr is not properly initialized, the converted ros message looks like

header:
  stamp:
    sec: 1651089592
    nanosec: 43981564
  frame_id: base_link
height: 0
width: 0
fields: []
is_bigendian: false
point_step: 0
row_step: 0
data: []
is_dense: false

which lacks fields attribute thus the error message you gave occured. While if initialized correctly the pointcloud message looks like (after the patch db00da5)

header:
  stamp:
    sec: 1651089459
    nanosec: 998696708
  frame_id: base_link
height: 1
width: 0
fields:
- name: x
  offset: 0
  datatype: 7
  count: 1
- name: y
  offset: 4
  datatype: 7
  count: 1
- name: z
  offset: 8
  datatype: 7
  count: 1
is_bigendian: false
point_step: 16
row_step: 0
data: []
is_dense: true

[HiroIshida]

@taikitanaka3
I experimentally checked that this PR fixes the issues you mentioned. So, I added that in the PR description. So, please check that when you have time

[HiroIshida]

Now this PR is again, ready fpr review

[yukkysaito]

@HiroIshida Thank you! I'll review today

[taikitanaka3]

LGTM
working fine with odaiba map

[taikitanaka3]

@HiroIshida
can you write this change to release note?

[HiroIshida]

@taikitanaka3 OK, I will add it

[shmpwk]

@HiroIshida
I have a question.

The function create_pointcloud returns pointcloud.
What is the difference of that pointcloud and the argument merged_pointcloud which is overwritten in the function?
Are they same except type?

https://github.com/HiroIshida/autoware.universe/blob/09a120a60e29f6190d07d9d4929ee5c2abdca10b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp#L73-L76

[HiroIshida]

@shmpwk
pointclouds is a vector of pointcloud of each vehicle. (vehicle-wise pointcloud is required to create dynamic object message)
On the other hand, merged pointcloud is the concatenation of pointclouds to a single point cloud.

It is more straightforward to return only pointclouds, but I decided to overwrite merged_pointcloud also in the function for computational efficiency for ObjectCentricPointCloudCreator. However, after getting question from you, I start to feel the computational speed improvement is slight for the code complexity. So, I will make a refactor PR so that it only return pointclouds.