Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(lidar_centerpoint): priotizing voxel processing by space #1672

Conversation

technolojin
Copy link

@technolojin technolojin commented Nov 29, 2024

Description

@TomohitoAndo
こちら、centerpointのmax voxelを超える出力があった際の挙動を調整するPRです。

  • before: 左前側がdropする
  • after: 左右後ろのcornerからdropする
    基本的には今お台場でやっているvoxel sizeの調整がメインで万一漏れた際のセーフティとしてこのPRが活きてくるイメージでいます by Yoshi Ri

Current voxel processing is done by order of X, Y sequentially.
IF the number of filled voxel exceed the ML model limit, the detectable range is limited.

The priority map is set by a scalar field as following.

    // absolute rectangular hyperbola to focus on crosssection
    // multiply sigmoid function to prioritize the front area
    const float score_a = abs((pos_x - 15.0) * pos_y) * (1 / (1 + exp(pos_x * 0.3)) + 0.75);
    // ellipse area focus on (10,0) and (150,0)
    const float score_b = sqrt((pos_x - 150) * (pos_x - 150) + pos_y * pos_y) +
                          sqrt((pos_x - 10) * (pos_x - 10) + pos_y * pos_y) - 140;
    // total score with weight
    // add noise to extend detection area in low-score area
    const float score =
      sqrt(score_a * 0.1 + score_b * 2.5) + normal_distribution(generator) * score_b * 0.01;

Figure_2

The following visualization is to show filled voxel distribution. Configurations are as follwing.

  • input high-density lidar point cloud
  • score_threshold: 0.01
  • max_voxel_size: 40000
  • point_cloud_range: [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0]
    Then lower score object will be shown (due to the lower score_threshold), indirectly visualize where the voxel is filled.

Screenshot from 2024-11-29 18-19-04

Related links

How was this PR tested?

Processing time

Screenshot from 2024-12-05 17-34-06

left: Before this PR
right: After this PR

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

This commit adds the `priority_map` array to the `preprocess_kernel.hpp` and `centerpoint_trt.hpp` files. The `priority_map` is a new CUDA array that is allocated and initialized in the `PreprocessKernelTest::SetUp()` function. The `priority_map` is used in the `generateBaseFeatures_launch()` function to generate base features for the voxel grid. The `priority_map` is also used in the `centerpoint_trt.hpp` file for initializing and updating the `priority_map_` member variable.

Signed-off-by: Taekjin LEE <[email protected]>
…tPriorityMap()

This commit modifies the `CenterPointTRT::initPriorityMap()` function to initialize and assign a priority score map. The priority score map is created using the `priority_map` vector, which contains pairs of scores and indices. The map is sorted in descending order based on the scores and then assigned to the `priority_map_` member variable. This change improves the efficiency of the priority map initialization process.

feat: initialize and assign priority score map in CenterPointTRT::initPriorityMap()
Signed-off-by: Taekjin LEE <[email protected]>
This commit refactors the `CenterPointTRT::initPriorityMap()` function to optimize its performance. The changes include:
- Simplifying the calculation of `pos_x` and `pos_y`
- Introducing two new variables, `score_a` and `score_b`, to calculate the total score
- Adjusting the weights of `score_a` and `score_b` to improve the accuracy of the priority map

These optimizations enhance the efficiency and accuracy of the priority map initialization process.

Signed-off-by: Taekjin LEE <[email protected]>
Signed-off-by: Taekjin LEE <[email protected]>

refactor: optimize CenterPointTRT::initPriorityMap()

refactor: optimize CenterPointTRT::initPriorityMap()
@technolojin technolojin force-pushed the feat/centerpoint/voxel-priority-map branch from 4c7e93f to 7e8a3f8 Compare December 5, 2024 05:14
@technolojin technolojin marked this pull request as ready for review December 6, 2024 03:59
Signed-off-by: Taekjin LEE <[email protected]>
Copy link

sonarcloud bot commented Dec 6, 2024

Copy link

@YoshiRi YoshiRi left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

v4.0.1 is a temporary version for testing in many ODDs and I think this PR will prevent some edge cases in the test driving. LGTM for merge this into this version.

@technolojin
Copy link
Author

This PR will be replaced by #1688

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants