Add Doppler ICP in tensor registration pipeline

[heethesh]

We would like to contribute the implementation of our Doppler ICP algorithm for point clouds captured by FMCW LiDARs. This PR adds support for Doppler ICP in the tensor registration pipeline within Open3D. This is the implementation of the following paper:

@INPROCEEDINGS{Hexsel-RSS-22, 
    AUTHOR    = {Bruno Hexsel AND Heethesh Vhavle AND Yi Chen}, 
    TITLE     = {{DICP: Doppler Iterative Closest Point Algorithm}}, 
    BOOKTITLE = {Proceedings of Robotics: Science and Systems}, 
    YEAR      = {2022}, 
    ADDRESS   = {New York City, NY, USA}, 
    MONTH     = {June}, 
    DOI       = {10.15607/RSS.2022.XVIII.015} 
}

Additions

• Added CPU and CUDA kernels for computing the DICP residuals and Jacobians.
• Added macro for dual robust function dispatch.
• I separated the ICP loop for the Doppler variant since this needs additional arguments such as the time period, calibration, and other DICP related parameters.
• Added TransformationToPose converter along with their kernels (based on the existing Eigen implementation of utility::TransformMatrix4dToVector6d).
• Added Python bindings for all the new Doppler ICP methods.

Updates to existing API

• Made matmul3x3_3x1 kernel available for __host__ in addition to __device__.
• Added num_iterations and converged field to RegistrationResult.
• Rename correspondences_ to correspondence_set to match the legacy Python parameters.

Checklist

• Add datasets.
• Write examples using the dataset.
• Write benchmarks using few samples from the dataset.
• Write tests using few samples from the dataset.

Review Guide

• It might be helpful to take a look at the legacy implementation first (easier to read), specifically this file.
• Here is a call graph of some important methods along with the filename where the implementation exists.

---

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.

[heethesh]

Still working on adding the sample dataset and Python/C++ examples. Note that supporting multi-scale Doppler ICP is not trivial as we cannot use voxel downsampling which averages out the Doppler velocities in the voxel. I would prefer having uniform downsample for tensor point clouds for my examples. Here is an issue #4849 I had created a while back and I see #5202 in progress.

[reyanshsolis]

Hi @heethesh, may we have a zoom call for discussion?
In my review, I found this can be merged with the existing ICP interface with some changes, and we can re-use most of its code.
Also, these will be some other required changes:

• We need you to add a unit test, a dataset, example, benchmark.
• Modify IO to use the existing XYZI reader.
• Use the existing ICP and Multi-ICP interface, and add additional params only in TransformationEstimationForDopplerICP as we do for other methods such as TransformationEstimationForColerdICP.
• The API is having a sensor to vehicle transform, to get results w.r.t. vehicle, which we don't need. Getting sensor frame to frame transform must be enough.
• Some other design simplifications changes are also there.

I can send you an invite, if you may share your email and preferred time (with time zone). I am currently in India (IST) [UTC+05:30].

Thanks and Regards,
Rishabh [rishabh (dot) 17iitkgp (at) gmail (dot) com]

[heethesh]

@reyanshsolis these are good suggestions, sent you an email. Let's work on simplifying it.

The API is having a sensor to vehicle transform, to get results w.r.t. vehicle, which we don't need. Getting sensor frame to frame transform must be enough.

Sure this could work, but note that FMCW sensors cannot measure any tangential velocity (or infer ego-angular velocity). We formulate the pose estimation in vehicle frame to infer this angular component to optimize the pose. Basically providing a vehicle-to-sensor transform will help improve the pose estimation where the sensor undergoes a rotation.

[reyanshsolis]

Hi @heethesh, just checking, any updates following our discussion? Feel free to ping me for discussion and collaboration.
Thanks.

[heethesh]

Hey @reyanshsolis, will try to get all the comments addressed this week, was caught up with some other stuff recently. Do we have any release deadlines coming up?

[reyanshsolis]

Hey @reyanshsolis, will try to get all the comments addressed this week, was caught up with some other stuff recently. Do we have any release deadlines coming up?

The release date is not finalized, you may continue working on this PR at your convenience, as this is not blocking the release. Feel free to ping me for discussions if required. Thanks.

[ssheorey]

Hi @heethesh can you provide some example data to show the results from this PR? Also, please update the PR to resolve conflicts. We are looking to merge this for the next release.

[heethesh]

@ssheorey yes I have some sample data sequences and examples. Sure I'll prioritize this PR again and address all the comments by end of next week.

[ssheorey]

THanks, @heethesh
Another task would be to add the dataset examples to the new Open3D datasets API. @yxlao can you help @heethesh with this?

[ssheorey]

Hi @heethesh @yxlao how is this PR coming along?

[heethesh]

I've made some progress addressing @reyanshsolis comments, will finish the rest and update the PR over the weekend.

[heethesh]

@yxlao @ssheorey https://drive.google.com/file/d/1kwYA0WnX3cT_UEb9eknCbGn10Pmo0jhy/view?usp=share_link here's a 10s sequence with 100 XYZD files. Please let me know how to integrate this dataset with the tests, benchmarks, and examples.

[heethesh]

@reyanshsolis I've addressed your comments regarding the refactor and IO format for XYZD, please take a look when you can, thanks!

[ssheorey]

Hi @heethesh can you check the CI errors, please?

I've uploaded the example data here: https://github.com/isl-org/open3d_downloads/releases/download/doppler-icp-data/carla-town05-curved-walls.zip. To add this to the Open3D dataset API, follow these steps:

• Add a new dataset class with the above download URL and the SHA256 hash to cpp/open3d/data/Dataset.h and it's implementation in cpp/open3d/data/Dataset/___.cpp. This is derived from the base dataset class that handles downloads and archive extraction.
• This class will just make available the paths to the dataset files to user code. See an example here: cpp/open3d/data/dataset/DemoICPPointClouds.cpp
• Add python bindings to cpp/pybind/data/dataset.cpp.
• Add an example code for using the example dataset as a python or cpp (or both) to examples/{cpp,python}. Good examples for this are examples/cpp/OfflineSLAM.cpp and examples/python/pipelines/rgbd_odometry.py.

If you need help, just let us know. We can add some of this dataset API code to this PR as well. (Please enable
"let maintainers push to this branch" for that.)

[ssheorey]

Hi @heethesh we are planning to release v0.17 very soon. Please update the PR when you have a chance.

[heethesh]

@ssheorey sure, I got the dataset integrated. Will quickly finish the benchmarks, examples, and a simple test.

[heethesh]

@ssheorey examples, benchmarks, unit tests and dataset integration all done now. Please let me know if you have any comments, thanks! I'll shortly add another example for Python as well.

[heethesh]

@ssheorey I see this error when importing Open3D in Python (locally as well as on the CI). Any clue how to debug this?

from open3d.cpu.pybind import (core, camera, data, geometry, io, pipelines,
ImportError: SystemError: <built-in method join of str object at 0x7fc2a79bd670> returned a result with an error set

[heethesh]

Resolved, I had a typo for one of the parameters in docstring::ClassMethodDocInject for pybind.

[heethesh]

I'm done adding the Python example too, @reyanshsolis @yxlao this PR is ready for review now.

[ssheorey]

Reviewed 1 of 5 files at r5, 5 of 23 files at r7, 1 of 3 files at r8, 1 of 4 files at r10.
Reviewable status: 8 of 34 files reviewed, 4 unresolved discussions (waiting on @heethesh, @reyanshsolis, @theNded, and @yxlao)

---

cpp/tests/io/CMakeLists.txt line 25 at r11 (raw file):

    file_format/FileSTL.cpp
    file_format/FileXYZ.cpp
    file_format/FileXYZD.cpp

This file is unused now. Remove.

---

cpp/tests/io/file_format/FileXYZD.cpp line 7 at r11 (raw file):

// SPDX-License-Identifier: MIT
// ----------------------------------------------------------------------------

Remove this file.

---

examples/python/pipelines/doppler_icp_registration.py line 26 at r11 (raw file):

import numpy as np
import open3d as o3d
import transformations as tf

[nit] pyquaternion is already an Open3D dependency. Can we replace transformations with pyquarternion?

---

examples/python/pipelines/doppler_icp_registration.py line 161 at r11 (raw file):

    # Setup robust kernels.
    o3d_reg = o3d.t.pipelines.registration

This works now: import o3d.t.pipelines.registration as o3d_reg

[ssheorey]

Hi @heethesh I ran the examples using the example data and normal point to plane ICP seems to work better than Doppler ICP. Can you comment on how we should evaluate the method?

(o3d-310) L:~/Documents/Open3D/Code/Open3D/examples/python/pipelines(Y:heethesh/tensor-doppler-icp)$ python doppler_icp_registration.py -s 1 -t 5
Using external Open3D-ML in /Users/ssheorey/Documents/Open3D/Code/Open3D-ML/
Estimated pose from Point-to-Plane ICP [16 iterations]:
[[ 1.      0.      0.0005 -2.2506]
 [-0.      1.      0.0002  0.0001]
 [-0.0005 -0.0002  1.      0.008 ]
 [ 0.      0.      0.      1.    ]]

Estimated pose from Doppler ICP [23 iterations]:
[[ 1.     -0.0033 -0.001  -0.5608]
 [ 0.0033  1.      0.0017 -0.0246]
 [ 0.001  -0.0018  1.     -0.0142]
 [ 0.      0.      0.      1.    ]]

Ground truth pose:
Loaded 100 poses from ground_truth_poses.txt (9.90 secs)
[[ 1.      0.      0.     -2.2471]
 [ 0.      1.      0.      0.0001]
 [ 0.      0.      1.      0.    ]
 [ 0.      0.      0.      1.    ]]

[heethesh]

@ssheorey thanks for the comments, I'll address them.

Regarding the results, I think the period is not set correctly for the algorithm. Thanks for catching that, I missed testing non-sequential scans. Line 173 updated:

    estimator_dicp = o3d_reg.TransformationEstimationForDopplerICP(
        period=period * (args.target - args.source),

This sequence (CARLA Town05 Curved Walls, image right) might not highlight the benefits of DICP in the best manner since the curved region of the walls provides sufficient geometric constraints here. However, DICP should still converge faster (fewer iterations compared to Point-to-Plane ICP).

I have another tunnel sequence (CARLA Town04 Straight Walls, image left) that we could possibly replace the sample dataset with that will show benefits of DICP even for stride-1 sequential scans (the X axis geometry is under-constrained here and Doppler velocities are required to sufficiently constraint the registration).

I'll share this new dataset with you shortly and update the example scripts. I was also thinking 100 scans was a lot compared to the other datasets, I could reduce it to 20 scans in the new dataset.

Here are the results now with the period fixed and with a much larger stride (also note the # iterations):

python doppler_icp_registration.py -s 1 -t 10
Estimated pose from Point-to-Plane ICP [200 iterations]:
[[ 1.     -0.0014  0.0004 -0.1604]
 [ 0.0014  1.      0.0002  0.0021]
 [-0.0004 -0.0002  1.      0.0036]
 [ 0.      0.      0.      1.    ]]

Estimated pose from Doppler ICP [8 iterations]:
[[ 1.     -0.     -0.     -5.0544]
 [ 0.      1.     -0.0002  0.0006]
 [ 0.      0.0002  1.     -0.0021]
 [ 0.      0.      0.      1.    ]]

Ground truth pose:
Loaded 100 poses from ground_truth_poses.txt (9.90 secs)
[[ 1.      0.      0.     -5.0559]
 [ 0.      1.      0.      0.0003]
 [ 0.      0.      1.      0.    ]
 [ 0.      0.      0.      1.    ]]

and the inverse:

python doppler_icp_registration.py -s 10 -t 1
Estimated pose from Point-to-Plane ICP [5 iterations]:
[[ 1.      0.0015  0.      0.0056]
 [-0.0015  1.     -0.0004 -0.0016]
 [-0.      0.0004  1.      0.0005]
 [ 0.      0.      0.      1.    ]]

Estimated pose from Doppler ICP [7 iterations]:
[[ 1.      0.     -0.0001  5.0562]
 [-0.      1.     -0.0002 -0.0001]
 [ 0.0001  0.0002  1.     -0.0043]
 [ 0.      0.      0.      1.    ]]

Ground truth pose:
Loaded 100 poses from ground_truth_poses.txt (9.90 secs)
[[ 1.      0.      0.      5.0559]
 [ 0.      1.      0.     -0.0003]
 [ 0.      0.      1.     -0.    ]
 [ 0.      0.      0.      1.    ]]

[ssheorey]

Hi @heethesh I propose just adding the fix for non-sequential scans for now and merging it for v0.17. Please make a new PR for the other changes (different dataset) that we can add after v0.17. Could you push an update today?

[heethesh]

sure ill do that now

[ssheorey]

Hi @heethesh sorry, looks like we missed the window, so we'll have to merge this PR for the next release. Let's go ahead and update the dataset as you mentioned. Also, the period needs to be updated for the C++ example as well.
Be sure to add some images (or better yet a video clip). We will use it for the release notes / release video.

[heethesh]

Hi @ssheorey, I have some bandwidth to work on this PR this week. Where do you want me to upload the videos and images? Do I need to create markdown style docs in some place?

[ssheorey]

Hi @heethesh thanks for your hard work on this feature. Merging this PR now - please create a new PR to update the examples / data.