Nacho/generalized icp

[nachovizzo]

Generalized ICP

This PR brings the power of gicp to Open3D.

Experiments

KITTI benchmark, Sequence 00

• AVG runtime multicore: 14fps
• Other open-source implementations like fast_gicp achieve no more than 7/8 fps on the same computer.

Sequence 00

Method	Implementation	avg. translational error	avg. rotational error
gicp	Open3D	1.16	0.561
gicp	KAIST/fast-gicp	1.26	0.595
PointToPoint ICP	Open3D	3.18	1.24
PointToPlane ICP	Open3D	5.74	1.74

I also did some other experiments on different sequences that I will omit to
save space on this PR description.

Footnote

I'd like to create a more precise tutorial. The problem is I ran out of time and I can't contribute any more to this implementation(although I wish I had more time!).

I'm more than happy to review and comment if anyone from the Open3D community or wants to extend this Pull Request.

From my point of view, some stuff that needs to be addressed in future changes are:

• Add propper unit-tests (the transformation used in PointCloud.Transform test is not even in homogenous coordinates)
• Implement the 4 DISABLED unit tests, related to GeneralizedICP and EstimateCovariances
• Extend the PointCloud examples showing the use case of the new data member pcd.covariances_
• Improve the tutorial on generalized ICP. So far, it's now a toy example but not even the underlying math of the method is explained in the tutorial.

I'm sorry but this is the best I can do with my available free time.

Old PR

I have created a deprecated PR: #2521 but there was some discussion in there

---

This change is 

[review-notebook-app]

Check out this pull request on 

See visual diffs & provide feedback on Jupyter Notebooks.

---

Powered by ReviewNB

[theNded]

Reviewed 20 of 22 files at r1, 2 of 2 files at r2.
Reviewable status: all files reviewed, 2 unresolved discussions (waiting on @nachovizzo)

---

cpp/open3d/geometry/PointCloud.h, line 99 at r2 (raw file):

    /// Returns 'true' if the point cloud contains per-point covariance matrix.
    bool HasCovariances() const {
        return !points_.empty() && covariances_.size() == points_.size();

One potential change could be inheriting PointCloud in a specific context in the pipelines: https://github.com/intel-isl/Open3D/blob/master/cpp/open3d/pipelines/registration/ColoredICP.cpp#L45
The same applies to other extensions.
The current implementation looks fine though, as it couples with normal estimation.

---

cpp/open3d/pipelines/registration/GeneralizedICP.cpp, line 134 at r2 (raw file):

                const Eigen::Matrix3d &Ct = target.covariances_[corres[i][1]];
                const Eigen::Vector3d d = vs - vt;
                const Eigen::Matrix3d M = Ct + Cs;

You have mentioned that Ct + T @ Cs @ T.T was not implemented. Is it still the case? If so, please add a TODO line and leave it as a future work.

[nachovizzo]

Reviewable status: all files reviewed, 2 unresolved discussions (waiting on @theNded)

---

cpp/open3d/geometry/PointCloud.h, line 99 at r2 (raw file):

Previously, theNded (Wei Dong) wrote…

One potential change could be inheriting PointCloud in a specific context in the pipelines: https://github.com/intel-isl/Open3D/blob/master/cpp/open3d/pipelines/registration/ColoredICP.cpp#L45
The same applies to other extensions.
The current implementation looks fine though, as it couples with normal estimation.

That was the original implementation on PR #2521. But without changing the whole registration pipeline to use Geometry3D instead of PointCloud it's sort of "impossible" to efficiently implement G-ICP. The main reasons could be (just for the records)

• You need to have access to the transformation on each iteration, this adds an extra member to all TransformationPointToX classes. Not a big deal, but more noise in there.
• If we don't change this line https://github.com/intel-isl/Open3D/blob/784cb5ef4b6ba68f39c00aca4e887d13d8be1c96/cpp/open3d/pipelines/registration/Registration.cpp#L160 then, if we pass a PointCloudforGeneralizedICP there, we lose the non PointCloud members when we perform the copy.
• Because of the last point, we need to re-compute the covariances matrices at each ICP iteration that it's how I was doing it in the past, but it's slower.

Overall I still feel "unsafe" of adding a 4th private member to the PointCloud class, but this might also bring more flexibility for different applications.

---

cpp/open3d/pipelines/registration/GeneralizedICP.cpp, line 134 at r2 (raw file):

Previously, theNded (Wei Dong) wrote…

You have mentioned that Ct + T @ Cs @ T.T was not implemented. Is it still the case? If so, please add a TODO line and leave it as a future work.

It used to be like that, but since now the covariance is a private member of the PointCloud when you hit this line https://github.com/intel-isl/Open3D/blob/784cb5ef4b6ba68f39c00aca4e887d13d8be1c96/cpp/open3d/pipelines/registration/Registration.cpp#L162 then the Cs matrix will also get transformed here: https://github.com/nachovizzo/Open3D/blob/nacho/generalized_icp/cpp/open3d/geometry/Geometry3D.cpp#L164. Which is a nice outcome of the "new" implementation. That also might explain why when re-running the experiments there was a small improvement on the G-ICP approach.

PR #2521

Method	Implementation	avg. translational error	avg. rotational error
gicp	Open3D	1.18	0.568

PR #3181

Method	Implementation	avg. translational error	avg. rotational error
gicp	Open3D	1.16	0.561

Is not a big deal, but, it's better :)

[theNded]

Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @nachovizzo)

[nachovizzo]

Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @nachovizzo)

@theNded Should I do something? It looks good to me so far.

[theNded]

Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @nachovizzo)

@theNded Should I do something? It looks good to me so far.

Looks fine for me -- I will take some time to test the entire reconstruction system before merging just in case. Thanks!

[nachovizzo]

@theNded @ssheorey shall we merge this sometime? I didn't manage to find some time to improve the notebook example. One option would be to delete the notebook and move it to a ToDO list

[theNded]

@nachovizzo Sorry I forgot about this... Let me fix it today.