Nacho/robust kernels

[nachovizzo]

Robust Kernels for Registration in Open3D

• Results
  • Noisy Input Data
  • Vanilla ICP
  • Vanilla ICP + higher distance threshold
  • Robust ICP using TukeyLoss
• Kernels Implemented so far
  • RobustKernel (base class)
  • L2
  • L1
  • Huber
  • Cauchy
  • German-McClure
  • Tukey
• Documentation
• More info

This PR brings the power of statistical robust kernels to aid the registration pipelines built-in Open3D. For the time being, it only changes the PointToPlane ICP algorithm but could be easily adapted for other optimization problems.

Results

The full explanation on this it's hosted on the tutorial robust_kernels. The results are shown below:

Noisy Input Data

Vanilla ICP

Vanilla ICP + higher distance threshold

Robust ICP using TukeyLoss

As we show, using a robust kernel(o robust loss function) in the PointToPlane optimization, leads to much better results in the context of noisy data.

Kernels Implemented so far

RobustKernel (base class)

Adding a new robust kernel is as easy as inheriting from open3d::pipelines::registration::RobustKernel and implemented the virtual Weight(double residual) method.

L2

Is the Vanilla ICP loss function, by default, this is the selected kernel(thus, no real robust optimization). Although this guarantee backward compatibility will all the existing code that relies on Open3D registration ICP pipelines.

L1

Huber

Cauchy

German-McClure

Tukey

Documentation

I spent extra time and effort trying to make the documentation the most extensible possible. Please check the tutorial and docs generated by this PR for more information.

More info

For bibliography, you could check out https://arxiv.org/pdf/1810.01474.pdf or https://arxiv.org/pdf/2004.14938.pdf (this adaptive kernel I plane to add soon)

---

This change is 

[update-docs]

Thanks for submitting this pull request! The maintainers of this repository would appreciate if you could update the CHANGELOG.md based on your changes.

[theNded]

This PR looks great, thanks for the contribution! Several initial considerations:

1. We are pushing the Tensor branch, would you consider developing a version for Tensors? i.e., instead of taking a double as an argument, we may take a flattened 1D Tensor as input.
2. [Optional] would it be possible that you benchmark the performance for different kernels on some small datasets (like merely the point clouds in test_data)?
   Thanks again!

[nachovizzo]

@theNded Thanks for the feedback.

1.) Yes I can totally do it, although I more in favor of atomic PR, so I would probably happen in the next PR
2.) I will try to add a small benchmark. Even so, I don't think this adds any overhead in the optimization problem.

I will clean up what I've done, add some experiments on the tutorials showing the use case of robust kernels, and move forward. If you have any other concerns respect to the design It would be awesome to know before I open the cleaned version of these changes.

Thanks!

[theNded]

@nachovizzo
1 totally makes sense for me. For 2 I was only want to make sure that the registration can converge around to the correct result, the runtime would not be critical.

Thanks and looking forward to the PR!

[nachovizzo]

@theNded this PR is ready to review. I did not add any sort of benchmark right now because I ran out of time. Even so, the API is backward compatible, so, it will not change any running code. In the future, we should add benchmarks and also unit tests that assert that robust kernels work with noisy data.

I hope this contribution is helpful for all the Open3D users!

[theNded]

Reviewed 5 of 11 files at r1, 1 of 2 files at r2, 7 of 7 files at r3, 1 of 1 files at r4.
Reviewable status: all files reviewed, 4 unresolved discussions (waiting on @nachovizzo and @yxlao)

---

cpp/open3d/pipelines/registration/RobustKernel.cpp, line 39 at r4 (raw file):

namespace registration {

double L2Loss::Weight(double /*residual*/) const { return 1.0; }

Overall I'm thinking about making Weights inline member functions, since they are mostly one-liners.

---

cpp/open3d/pipelines/registration/RobustKernel.cpp, line 46 at r4 (raw file):

double HuberLoss::Weight(double residual) const {
    const double e = std::abs(residual);

return k_ / std::max(e, k_);

---

cpp/open3d/pipelines/registration/RobustKernel.cpp, line 54 at r4 (raw file):

double CauchyLoss::Weight(double residual) const {
    return 1.0 / (1 + std::pow(residual / k_, 2.0));

Would you consider writing a short inline function/Macro square(x) for these weights? std::pow is slower than x*x. It is a micro optimization though, you may ignore it.

---

cpp/open3d/pipelines/registration/RobustKernel.cpp, line 63 at r4 (raw file):

double TukeyLoss::Weight(double residual) const {
    const double e = std::abs(residual);
    if (e > k_) {

return std::pow((1.0 - std::pow(std::min(1, e / k_), 2.0)), 2.0);

[theNded]

Thanks for the great work! I added some suggestions for minor changes.
Several potential next steps can be

• Replacing the ad-hoc German McClure loss (https://github.com/intel-isl/Open3D/blob/master/cpp/open3d/pipelines/registration/FastGlobalRegistration.cpp#L268) in Fast Global Registration (http://vladlen.info/papers/fast-global-registration.pdf) with the new RobustKernel
• Benchmark performance of the offline reconstruction system's performance with some empirical loss setups.
  I can help with these tasks.

[nachovizzo]

Reviewable status: all files reviewed, 4 unresolved discussions (waiting on @theNded and @yxlao)

---

cpp/open3d/pipelines/registration/RobustKernel.cpp, line 39 at r4 (raw file):

Previously, theNded (Wei Dong) wrote…

Overall I'm thinking about making Weights inline member functions, since they are mostly one-liners.

I totally agree. Even so, the fact that C++ does not have proper support for polymorphism without virtual disable the compiler to inline these methods. I spent a few hours, there are 3/4 cppcon talks trying to address this problem. Most of them end up writing a header-only library that it's usually only supported in c++17. With this I want to say, I didn't find any "elegant" solution for this. I didn't want to use pointers, nor smartpointers, I don't see why we need to allocate one RobustKernel object in the heap... the problem is that it's the only way to make this polymorphic. For the time being, I didn't detect any real overhead.

Here are 2 examples to investigate further, even the trivial L2Loss::Weight function can not be inlined by the compiler(due to memory allocations in the heap and so on...)

• Polymorphic example: https://godbolt.org/z/1cahrd
• Fixed kernel example: https://godbolt.org/z/5Eofr7, observe how in this case the loss function is trivially inlied by the compiler and further optimized. I can add inline to the declarations, but it makes no real difference.

PS: I''m open to suggestions if you find a better way to implement this strategy pattern

---

cpp/open3d/pipelines/registration/RobustKernel.cpp, line 46 at r4 (raw file):

Previously, theNded (Wei Dong) wrote…

return k_ / std::max(e, k_);

Great catch!

---

cpp/open3d/pipelines/registration/RobustKernel.cpp, line 54 at r4 (raw file):

Previously, theNded (Wei Dong) wrote…

Would you consider writing a short inline function/Macro square(x) for these weights? std::pow is slower than x*x. It is a micro optimization though, you may ignore it.

Done

---

cpp/open3d/pipelines/registration/RobustKernel.cpp, line 63 at r4 (raw file):

Previously, theNded (Wei Dong) wrote…

return std::pow((1.0 - std::pow(std::min(1, e / k_), 2.0)), 2.0);

Got it, much better

[nachovizzo]

Thanks for the great work! I added some suggestions for minor changes.
Several potential next steps can be

• Replacing the ad-hoc German McClure loss (https://github.com/intel-isl/Open3D/blob/master/cpp/open3d/pipelines/registration/FastGlobalRegistration.cpp#L268) in Fast Global Registration (http://vladlen.info/papers/fast-global-registration.pdf) with the new RobustKernel
• Benchmark performance of the offline reconstruction system's performance with some empirical loss setups.
  I can help with these tasks.

Should we attempt to address this or in other PR?

[nachovizzo]

Reviewable status: 13 of 14 files reviewed, 4 unresolved discussions (waiting on @theNded and @yxlao)

---

cpp/open3d/pipelines/registration/RobustKernel.cpp, line 39 at r4 (raw file):

Previously, nachovizzo (Ignacio Vizzo) wrote…

I totally agree. Even so, the fact that C++ does not have proper support for polymorphism without virtual disable the compiler to inline these methods. I spent a few hours, there are 3/4 cppcon talks trying to address this problem. Most of them end up writing a header-only library that it's usually only supported in c++17. With this I want to say, I didn't find any "elegant" solution for this. I didn't want to use pointers, nor smartpointers, I don't see why we need to allocate one RobustKernel object in the heap... the problem is that it's the only way to make this polymorphic. For the time being, I didn't detect any real overhead.

Here are 2 examples to investigate further, even the trivial L2Loss::Weight function can not be inlined by the compiler(due to memory allocations in the heap and so on...)

• Polymorphic example: https://godbolt.org/z/1cahrd
• Fixed kernel example: https://godbolt.org/z/5Eofr7, observe how in this case the loss function is trivially inlied by the compiler and further optimized. I can add inline to the declarations, but it makes no real difference.

PS: I''m open to suggestions if you find a better way to implement this strategy pattern

Done.

---

cpp/open3d/pipelines/registration/RobustKernel.cpp, line 46 at r4 (raw file):

Previously, nachovizzo (Ignacio Vizzo) wrote…

Great catch!

Done.

---

cpp/open3d/pipelines/registration/RobustKernel.cpp, line 63 at r4 (raw file):

Previously, nachovizzo (Ignacio Vizzo) wrote…

Got it, much better

Done.

[theNded]

Thanks for the great work! I added some suggestions for minor changes.
Several potential next steps can be

• Replacing the ad-hoc German McClure loss (https://github.com/intel-isl/Open3D/blob/master/cpp/open3d/pipelines/registration/FastGlobalRegistration.cpp#L268) in Fast Global Registration (http://vladlen.info/papers/fast-global-registration.pdf) with the new RobustKernel
• Benchmark performance of the offline reconstruction system's performance with some empirical loss setups.
  I can help with these tasks.

Should we attempt to address this or in other PR?

Yeah of course, "next steps" here definitely mean other PRs :-)

[theNded]

Reviewed 1 of 1 files at r5.
Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @yxlao)

[germanros1987]

Here is an overview of what got changed by this pull request:

Clones added
============
- cpp/open3d/pipelines/registration/TransformationEstimation.h  2
         

See the complete overview on Codacy