fix(pointcloud_preprocessor): fix distortion corrector unit test #7833

vividf · 2024-07-04T05:55:13Z

Description

@knzo25 gave some feedback on the distortion corrector unit test.
TIER4_INTERNAL_LINK

Original PR #7801
Due to the jitter of the timestamp, the threshold is a bit too strict.

6: The difference between data_ptr[i * 5] and expected_pointcloud[i][0] is 5.340576171875e-05, which exceeds 5e-5, where
6: data_ptr[i * 5] evaluates to 20.010353088378906,
6: expected_pointcloud[i][0] evaluates to 20.010299682617188, and
6: 5e-5 evaluates to 5.0000000000000002e-05.
6: [ FAILED ] DistortionCorrectorTest.TestUndistortPointCloud3dWithImuInLidarFrame (1036 ms)

More information is in the description of this PR #7879

Main changes from the previous unit test:

Provide a debugging parameter to print the Ground truth value.
Remove memcpy and use pointcloud iterator
Remove reinterpret_cast
Use the self-defined timestamp instead of node_->get_clock()->now() to prevent some jitter between points' stamp.
Add tests for pure linear motion and pure rotation motion.

The 2D algorithm in the distortion corrector uses tier4::cos/sin and quat.setvalue(0, 0, 0.5 * angle, 0.5 * angle), both of which can speed up the process. However, the values produced by these methods differ from those obtained using std::cos/sin, quat.setRPY(0, 0, angle) and exponential map. As a result, the 2D and 3D algorithms will not yield equivalent outcomes when comparing pure rotational motions.

Build & Test

colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select pointcloud_preprocessor

Test

colcon test --packages-select pointcloud_preprocessor --event-handlers console_cohesion+

How was this PR tested?

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

github-actions · 2024-07-04T05:55:32Z

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.

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

vividf · 2024-07-04T06:03:35Z

I just cleaned my whole repository and forgot to install pre-commit again.
I installed it again so the auto fix won't show again

sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp

knzo25

Left some minor comments and questions

knzo25 · 2024-07-04T10:18:02Z

@vividf
Ok, no. I remembered that I had asked for proper tests rather than comparing to a GT.
I.e., input pointcloud vs theoretical expected pointcloud (for simple cases like pure linear movement and pure rotational movement)

knzo25 · 2024-07-04T12:46:07Z

I can not make it into a code suggestion, but Tier IV -> TIER IV

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

knzo25

Still some comments that need addressing

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

…:vividf/autoware.universe into fix/distortion_correction_node_unit_test

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

knzo25

@vividf
If it was not clear, there are comments left unattended

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

knzo25

LGTM 🎉

knzo25 · 2024-07-09T11:49:27Z

@vividf
I will auto-merge this since I want to finish the point types ASAP (and at night, when there are less merges)

vividf · 2024-07-09T11:50:34Z

Sure, thanks for the reviews!

* fix: use pointcloud iterator instead of memcpy, remove reinterpret cast Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: set default debugging parameter to false Signed-off-by: vividf <yihsiang.fang@tier4.jp> * style(pre-commit): autofix * chore: run precommit Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: fix TIER IV name Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: changed public variables to protected and add getters Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: add tolerance Signed-off-by: vividf <yihsiang.fang@tier4.jp> * feat: add two tests for pure linear and rotation Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: change naming, fix tolerance value Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: add comment for quat Signed-off-by: vividf <yihsiang.fang@tier4.jp> * fix: remove node_->get_clock() and use self-defined timestamp Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: remove redundant parameters Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: fix spell error and add tests in cmake Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: move all clock->now() to self-defined time Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: change function names Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: remove irrelevant variable Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: fix variables naming Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: change boolen naming: generate_points Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: add assert to make sure ms is not larger than a second Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: add note Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: add unifore initialization and semantic meaning for magic number Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: change vector to Eigen Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: fix explanation Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: fix more eigen Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: fix more magic numbers Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: add unit Signed-off-by: vividf <yihsiang.fang@tier4.jp> * fix: use assert from gtest Signed-off-by: vividf <yihsiang.fang@tier4.jp> --------- Signed-off-by: vividf <yihsiang.fang@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kenzo Lobos Tsunekawa <kenzo.lobos@tier4.jp>

…owarefoundation#7833) * fix: use pointcloud iterator instead of memcpy, remove reinterpret cast Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: set default debugging parameter to false Signed-off-by: vividf <yihsiang.fang@tier4.jp> * style(pre-commit): autofix * chore: run precommit Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: fix TIER IV name Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: changed public variables to protected and add getters Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: add tolerance Signed-off-by: vividf <yihsiang.fang@tier4.jp> * feat: add two tests for pure linear and rotation Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: change naming, fix tolerance value Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: add comment for quat Signed-off-by: vividf <yihsiang.fang@tier4.jp> * fix: remove node_->get_clock() and use self-defined timestamp Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: remove redundant parameters Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: fix spell error and add tests in cmake Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: move all clock->now() to self-defined time Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: change function names Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: remove irrelevant variable Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: fix variables naming Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: change boolen naming: generate_points Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: add assert to make sure ms is not larger than a second Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: add note Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: add unifore initialization and semantic meaning for magic number Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: change vector to Eigen Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: fix explanation Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: fix more eigen Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: fix more magic numbers Signed-off-by: vividf <yihsiang.fang@tier4.jp> * chore: add unit Signed-off-by: vividf <yihsiang.fang@tier4.jp> * fix: use assert from gtest Signed-off-by: vividf <yihsiang.fang@tier4.jp> --------- Signed-off-by: vividf <yihsiang.fang@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kenzo Lobos Tsunekawa <kenzo.lobos@tier4.jp>

vividf added 2 commits July 4, 2024 14:46

fix: use pointcloud iterator instead of memcpy, remove reinterpret cast

eb49316

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

chore: set default debugging parameter to false

ada48f6

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

vividf requested review from amc-nu, badai-nguyen, drwnz, knzo25, kyoichi-sugahara, meliketanrikulu, miursh, YoshiRi and yukkysaito as code owners July 4, 2024 05:55

vividf self-assigned this Jul 4, 2024

github-actions bot added the component:sensing Data acquisition from sensors, drivers, preprocessing. (auto-assigned) label Jul 4, 2024

vividf changed the title ~~Fix/distortion correction node unit test~~ fix(pointcloud_preprocessor): fix distortion corrector unit test Jul 4, 2024

vividf added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Jul 4, 2024

pre-commit-ci bot and others added 2 commits July 4, 2024 05:58

style(pre-commit): autofix

6a7d840

chore: run precommit

0c94a14

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

knzo25 reviewed Jul 4, 2024

View reviewed changes

sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp Outdated Show resolved Hide resolved

knzo25 reviewed Jul 4, 2024

View reviewed changes

sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp Outdated Show resolved Hide resolved

knzo25 reviewed Jul 4, 2024

View reviewed changes

sensing/pointcloud_preprocessor/test/test_distortion_corrector_node.cpp Show resolved Hide resolved

knzo25 requested changes Jul 4, 2024

View reviewed changes

vividf added 5 commits July 5, 2024 10:12

chore: fix TIER IV name

5703e96

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

chore: changed public variables to protected and add getters

376cc38

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

chore: add tolerance

1e0d400

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

feat: add two tests for pure linear and rotation

bb5a7e0

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

chore: change naming, fix tolerance value

ff86ebd

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

vividf and others added 9 commits July 9, 2024 11:19

chore: fix variables naming

816d187

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

chore: change boolen naming: generate_points

e99062e

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

chore: add assert to make sure ms is not larger than a second

78196ce

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

chore: add note

a7c2a8b

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

chore: add unifore initialization and semantic meaning for magic number

17d98f7

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

chore: change vector to Eigen

b0f46b7

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

chore: fix explanation

2629b2a

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

chore: fix more eigen

459a10e

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

Merge branch 'main' into fix/distortion_correction_node_unit_test

5a5fe59

vividf requested a review from knzo25 July 9, 2024 05:59

knzo25 requested changes Jul 9, 2024

View reviewed changes

vividf added 2 commits July 9, 2024 17:08

chore: fix more magic numbers

80694c2

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

Merge branch 'fix/distortion_correction_node_unit_test' of github.com…

ec3a1fb

…:vividf/autoware.universe into fix/distortion_correction_node_unit_test

vividf requested a review from knzo25 July 9, 2024 08:15

chore: add unit

8583607

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

knzo25 requested changes Jul 9, 2024

View reviewed changes

fix: use assert from gtest

1564c8a

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

vividf requested a review from knzo25 July 9, 2024 10:38

vividf and others added 2 commits July 9, 2024 19:38

Merge branch 'main' into fix/distortion_correction_node_unit_test

58a7cc0

Merge branch 'main' into fix/distortion_correction_node_unit_test

419dea1

knzo25 approved these changes Jul 9, 2024

View reviewed changes

knzo25 enabled auto-merge (squash) July 9, 2024 11:48

knzo25 merged commit 9b20ced into autowarefoundation:main Jul 9, 2024
22 of 23 checks passed

vividf deleted the fix/distortion_correction_node_unit_test branch July 17, 2024 02:26

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

fix(pointcloud_preprocessor): fix distortion corrector unit test #7833

fix(pointcloud_preprocessor): fix distortion corrector unit test #7833

vividf commented Jul 4, 2024 •

edited

Loading

github-actions bot commented Jul 4, 2024 •

edited

Loading

vividf commented Jul 4, 2024 •

edited

Loading

knzo25 left a comment

knzo25 commented Jul 4, 2024

knzo25 commented Jul 4, 2024

knzo25 left a comment

knzo25 left a comment

knzo25 left a comment

knzo25 commented Jul 9, 2024

vividf commented Jul 9, 2024

fix(pointcloud_preprocessor): fix distortion corrector unit test #7833

fix(pointcloud_preprocessor): fix distortion corrector unit test #7833

Conversation

vividf commented Jul 4, 2024 • edited Loading

Description

Build & Test

Related links

How was this PR tested?

Notes for reviewers

Interface changes

Effects on system behavior

github-actions bot commented Jul 4, 2024 • edited Loading

vividf commented Jul 4, 2024 • edited Loading

knzo25 left a comment

Choose a reason for hiding this comment

knzo25 commented Jul 4, 2024

knzo25 commented Jul 4, 2024

knzo25 left a comment

Choose a reason for hiding this comment

knzo25 left a comment

Choose a reason for hiding this comment

knzo25 left a comment

Choose a reason for hiding this comment

knzo25 commented Jul 9, 2024

vividf commented Jul 9, 2024

vividf commented Jul 4, 2024 •

edited

Loading

github-actions bot commented Jul 4, 2024 •

edited

Loading

vividf commented Jul 4, 2024 •

edited

Loading