Made rejectors work with ICP

[sdmiller]

Making correspondence rejectors actually work with ICP. As discussed in issue #324, this required adding PCLPointCloud2 setters to CorrespondenceRejector, with flags for whether or not they required source/target normals/points. As this requires a copy, it's a bit wasteful -- but short of a much larger refactoring job, it seems like the best choice to me. At least now the desired behavior is as expected -- let me know if I missed any Rejectors which use source/target clouds.

Note 1: CorrespondenceRejectorFeature does not fit this paradigm, so the user still needs to set this externally. The good news is any transform-invariant feature should be fine with this.
Note 2: Propagating this to JointICP as we speak
Note 3: the same trick can be done with CorrespondenceEstimation, so normals are always propagated (to those which require normals).

[sdmiller]

Notes 2 and 3 are also done. This required adding the same logic to CorrespondenceEstimation (requiresSourceNormals (), requiresTargetNormals ()) and propagating it to ICP and JointICP.

While I was at it, I also added a "clone ()" method to CorrespondenceEstimation types. This made it so, e.g., if JointICP was used to align 10 pairs of clouds, you wouldn't need to call "addCorrespondenceEstimator" 10 times.

test_registration should reflect all of these changes -- both ICP and JointICP are now called with multiple rejectors and custom estimators, some of which require normals.

[sdmiller]

Note: the Travis error is on an unrelated component. I'm pretty confident this works and strictly fixes bugs, though of course someone else should review to verify :)

[Liming-Cheng]

Hi @sdmiller , I found that the size of correspondence_rejectors_ is 0 when it was initialized in registration.h, so the correspondence rejection will not be done in default. I am confused about it. The "iterative_closest_point.cpp" provide an example of the usage, and I think it's more fine.

Also, I found the var "inlier_threshold_" in "correspondence_rejetion_sample_consensus.h" was uncorrelated with "inlier_threshold_" in "registration.h", so I wonder how the ransac rejection run.

Finally, the function "determineCorrespondeces()" find corrrespondences according to the max_dis_sqr, is that really right?

Can you help me about that? Thanks very much!

[sdmiller]

Hi @Liming-Cheng!

I'm afraid it has been many years since I've touched this codebase, and I no longer know enough to confidently speak to this. It's highly possible that there was a bug in my code due to a redefined inherited variable -- or, if we're being exceedingly generous to me, that at one point this made sense but no longer does :).

I believe this Pull Request was just gluing together a few pieces from the registration module, not implementing any of the logic that actually makes it work. I'm unsure who would be the best point of contact for that.