Refactoring and Bugfix of NDT #4180

koide3 · 2020-06-12T15:43:43Z

@kunaltyagi
Sorry for being late to open this PR. I refactored the NDT code according to #4135 and took a look at the suspicious code mentioned at #3832.

Refactoring #4135 (review)

PCL_NO_COMPILE condition check is added to ndt.cpp
Unneeded codes are removed
I left the arguments of updateIntervalMT and trialValueSelectionMT as they are because these methods have different in/out argument combinations, and putting them in a struct would hide this information
h_ang_* are concatenated into one matrix for simplicity and Eigen's SSE optimization (it brings about 1% speedup)
Parentheses are removed from return statements
point_gradient is renamed to point_jacobian
Transformation vector p is renamed to transform
Typo fixed

All the above modifications don't change the behavior of NDT, and the registration result will be exactly the same as before refactoring.

Bugfix #3832 (comment)
(step_max - step_min) > 0 must be (step_max - step_min) < 0 because this code is intended to check the validity of the range. As @atom2003 pointed out, (step_max - step_min) > 0 is always true, and this prevents the step length calculation being performed. This bug affects the registration accuracy in particular when we set a small transformation_epsilon for fine registration.

After resolving this bug, the fitness score of the registration result of the unit test (test_ndt.cpp) is slightly improved (before: 5.05433e-05, after: 4.68033e-05).

I also confirmed the accuracy improvement on my own dataset. The following figures show sensor trajectories estimated by frame-by-frame NDT registration, and the one before the bugfix has a large drift.

After Bugfix

Before Bugfix

kunaltyagi

Adding notes of the notable non-variable-name-change

registration/include/pcl/registration/impl/ndt.hpp

registration/src/ndt.cpp

kunaltyagi · 2020-06-12T18:17:46Z

registration/include/pcl/registration/ndt.h

@@ -440,21 +438,16 @@ namespace pcl
        *
        * The precomputed angular derivatives for the jacobian of a transformation vector, Equation 6.19 [Magnusson 2009]. 
        */
-      Eigen::Vector3d j_ang_a_, j_ang_b_, j_ang_c_, j_ang_d_, j_ang_e_, j_ang_f_, j_ang_g_, j_ang_h_;
+      Eigen::Matrix<double, 8, 4> angular_jacobian_;


Note: 3D vectors changed to 4D vectors. ABI breakage here

kunaltyagi · 2020-06-12T18:31:49Z

Should we separate the bug fix (the > -> <) in order to release the fix in 1.11.1? Due to ABI and public interface deprecation, this patch is scheduled for at least 1.12.0. This is not needed and we can just merge this entire PR together.

koide3 · 2020-06-13T00:23:39Z

Should we separate the bug fix (the > -> <) in order to release the fix in 1.11.1?

I agree. It would be nice to merge the bug fix first because it brings significant accuracy improment. Should I open another PR for the bugfix?

registration/include/pcl/registration/ndt.h

Refactoring and Bugfix of NDT PointCloudLibrary/pcl#4180

registration/include/pcl/registration/impl/ndt.hpp

SergioRAgostinho

Thank you for the clean-up.
Edit: looks like we need a rebase though

koide3 · 2020-07-03T09:55:44Z

I rabased and there should not be conflicts now.
Thanks for reviewing.

koide3 · 2020-07-03T11:19:30Z

Sorry, it seems the commit was contaminated with a wrong branch. I'll revert it.

kunaltyagi

Welp. I reviewed some before looking at what I was reviewing 👀. You can use this in the other PR though 😆

test/registration/test_ndt_omp.cpp

filters/include/pcl/filters/impl/voxel_grid_covariance.hpp

koide3 · 2020-07-06T02:51:44Z

Sorry, I did something wrong and did reset the branch. I'm sorry if it increases the reviewing effort...

kunaltyagi · 2020-07-07T13:00:52Z

Is this ready for review?

koide3 · 2020-07-07T14:07:09Z

Yes, it's ready.

kunaltyagi

Based on what you've said, you performed some benchmarking? Could you please share those results?

registration/include/pcl/registration/impl/ndt.hpp

koide3 · 2020-07-08T11:11:18Z

I found a bug in computeDerivatives() that was injected during the refactoring. I fixed it, and now the registration result is consistent with the NDT in master branch.

kunaltyagi

Looks good otherwise. After these and CI passes, rebase and merge as needed. My bandwidth is a bit low nowadays

registration/include/pcl/registration/impl/ndt.hpp

koide3 · 2020-07-09T05:23:36Z

In computeHessian(), I found a bug same as the one in computeDerivatives(). After fixing it, I confirmed that the score, jacobian, and hessian each iteration step are exactly the same as before refactoring.

CMakeLists.txt

kunaltyagi

Just a few more places where we can put const in

registration/include/pcl/registration/impl/ndt.hpp

koide3 · 2020-07-12T04:43:36Z

I replaced duplicated transformation vector-matrix conversion codes with convertTransform (that had existed but not been used).

kunaltyagi

Thanks for the update with const

registration/include/pcl/registration/ndt.h

registration/include/pcl/registration/impl/ndt.hpp

kunaltyagi · 2020-07-14T06:17:35Z

Needs conflict resolution and green CI, but a go from my side

koide3 · 2020-07-14T09:20:17Z

I did rebase and squash to resolve the conflict

kunaltyagi · 2020-08-14T14:51:02Z

Work on ABI breaks can now commence

kunaltyagi reviewed Jun 12, 2020

View reviewed changes

kunaltyagi added module: registration needs: code review Specify why not closed/merged yet labels Jun 12, 2020

kunaltyagi added the needs: feedback Specify why not closed/merged yet label Jun 12, 2020

kunaltyagi reviewed Jun 13, 2020

View reviewed changes

registration/include/pcl/registration/ndt.h Show resolved Hide resolved

registration/include/pcl/registration/ndt.h Show resolved Hide resolved

kunaltyagi added this to the pcl-1.12.0 milestone Jun 13, 2020

kunaltyagi removed the needs: feedback Specify why not closed/merged yet label Jun 13, 2020

koide3 mentioned this pull request Jun 13, 2020

[registration] Fix bug in NDT step interval convergence criteria #4181

Merged

kunaltyagi added the changelog: ABI break Meta-information for changelog generation label Jun 13, 2020

rsasaki0109 pushed a commit to rsasaki0109/ndt_omp_ros2 that referenced this pull request Jun 26, 2020

Bug fix about line search in the NDT

d137de1

Refactoring and Bugfix of NDT PointCloudLibrary/pcl#4180

SergioRAgostinho reviewed Jul 1, 2020

View reviewed changes

kunaltyagi added needs: more work Specify why not closed/merged yet and removed needs: code review Specify why not closed/merged yet labels Jul 2, 2020

SergioRAgostinho reviewed Jul 3, 2020

View reviewed changes

registration/include/pcl/registration/impl/ndt.hpp Outdated Show resolved Hide resolved

SergioRAgostinho approved these changes Jul 3, 2020

View reviewed changes

kunaltyagi self-requested a review July 3, 2020 09:39

kunaltyagi reviewed Jul 3, 2020

View reviewed changes

koide3 force-pushed the ndt-refactoring branch 2 times, most recently from 09863c3 to 88c5b89 Compare July 6, 2020 02:46

kunaltyagi mentioned this pull request Jul 7, 2020

Potentially upstream the improvements to PCL? koide3/ndt_omp#19

Open

koide3 mentioned this pull request Jul 7, 2020

[filters] Refatoring VoxelGridCovariance to make it multi-thread safe (and more) #4251

Merged

kunaltyagi reviewed Jul 7, 2020

View reviewed changes

kunaltyagi reviewed Jul 8, 2020

View reviewed changes

registration/include/pcl/registration/impl/ndt.hpp Outdated Show resolved Hide resolved

registration/include/pcl/registration/impl/ndt.hpp Outdated Show resolved Hide resolved

kunaltyagi reviewed Jul 9, 2020

View reviewed changes

CMakeLists.txt Outdated Show resolved Hide resolved

koide3 force-pushed the ndt-refactoring branch from aa8d665 to 14442a0 Compare July 9, 2020 10:13

kunaltyagi reviewed Jul 9, 2020

View reviewed changes

kunaltyagi reviewed Jul 12, 2020

View reviewed changes

registration/include/pcl/registration/ndt.h Show resolved Hide resolved

SergioRAgostinho reviewed Jul 12, 2020

View reviewed changes

registration/include/pcl/registration/impl/ndt.hpp Outdated Show resolved Hide resolved

registration/include/pcl/registration/impl/ndt.hpp Outdated Show resolved Hide resolved

registration/include/pcl/registration/impl/ndt.hpp Outdated Show resolved Hide resolved

kunaltyagi approved these changes Jul 14, 2020

View reviewed changes

Refactor and Bugfix of NDT

88524c1

koide3 force-pushed the ndt-refactoring branch from ab1dd8f to 88524c1 Compare July 14, 2020 09:14

SergioRAgostinho approved these changes Jul 14, 2020

View reviewed changes

SergioRAgostinho removed the needs: more work Specify why not closed/merged yet label Jul 14, 2020

koide3 mentioned this pull request Jul 17, 2020

[registration] Add OMP based Multi-threading to NDT #4277

Open

kunaltyagi merged commit fcbc655 into PointCloudLibrary:master Aug 14, 2020

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Refactoring and Bugfix of NDT #4180

Refactoring and Bugfix of NDT #4180

koide3 commented Jun 12, 2020

kunaltyagi left a comment

kunaltyagi Jun 12, 2020 •

edited

Loading

kunaltyagi commented Jun 12, 2020

koide3 commented Jun 13, 2020

SergioRAgostinho left a comment •

edited

Loading

koide3 commented Jul 3, 2020

koide3 commented Jul 3, 2020

kunaltyagi left a comment

koide3 commented Jul 6, 2020

kunaltyagi commented Jul 7, 2020

koide3 commented Jul 7, 2020

kunaltyagi left a comment

koide3 commented Jul 8, 2020

kunaltyagi left a comment

koide3 commented Jul 9, 2020

kunaltyagi left a comment

koide3 commented Jul 12, 2020

kunaltyagi left a comment

kunaltyagi commented Jul 14, 2020

koide3 commented Jul 14, 2020

kunaltyagi commented Aug 14, 2020

Refactoring and Bugfix of NDT #4180

Refactoring and Bugfix of NDT #4180

Conversation

koide3 commented Jun 12, 2020

kunaltyagi left a comment

Choose a reason for hiding this comment

kunaltyagi Jun 12, 2020 • edited Loading

Choose a reason for hiding this comment

kunaltyagi commented Jun 12, 2020

koide3 commented Jun 13, 2020

SergioRAgostinho left a comment • edited Loading

Choose a reason for hiding this comment

koide3 commented Jul 3, 2020

koide3 commented Jul 3, 2020

kunaltyagi left a comment

Choose a reason for hiding this comment

koide3 commented Jul 6, 2020

kunaltyagi commented Jul 7, 2020

koide3 commented Jul 7, 2020

kunaltyagi left a comment

Choose a reason for hiding this comment

koide3 commented Jul 8, 2020

kunaltyagi left a comment

Choose a reason for hiding this comment

koide3 commented Jul 9, 2020

kunaltyagi left a comment

Choose a reason for hiding this comment

koide3 commented Jul 12, 2020

kunaltyagi left a comment

Choose a reason for hiding this comment

kunaltyagi commented Jul 14, 2020

koide3 commented Jul 14, 2020

kunaltyagi commented Aug 14, 2020

kunaltyagi Jun 12, 2020 •

edited

Loading

SergioRAgostinho left a comment •

edited

Loading