New feature: Manual Loop Closure via Correspondence Points

[matlabbe]

Original thread: http://official-rtab-map-forum.206.s1.nabble.com/Suggestion-Manual-Loop-Closure-via-Correspondence-Points-td11336.html

Hi,

Would it be possible to add an option to manually create loop closures using correspondence points, followed by a fine alignment step in the database editor? Since I use a handheld scanner, my scans often end up at odd angles, and it's quite cumbersome to manually figure out the correct transformation between views.

I think it would be really helpful to have either:

• A gizmo to move and rotate one of the two point clouds, or
• The ability to select three or more corresponding points in each view to perform a rough alignment

This would make the process of adding manual loop closures much easier in my workflow.

Thanks!

The idea would be to do a tool similar to https://www.cloudcompare.org/doc/wiki/index.php/Align inside the DatabaseViewer for the "Add" button in Constraints view. Currently, when we do "Add", the first guess is estimated from the pose of each node selected in the the optimized graph. If the loop closure fails, the UI asks if we want to manually enter the parameters (X,Y,Z,roll,pitch,yaw) of the expected transform, which can be hard to guess correctly. Adding the option to pick 3 points on each point cloud would make this step easier, with a following refinement.

Implementation:

• In CloudCompare, they show the clouds in same viewport, with "show" button to hide/show the source/target point clouds, so we can pick points on one cloud or the other. We can apply right away and see the result without closing the manual alignment tool, so that we can do corrections if needed before exiting it. We could have the "Refine" button already there in a similar UI to do ICP after the first rough alignment.
• VTK can do the picking, see how measurements is done by the default interactor in CloudViewer object
• IDs of picked points can be shown (maybe with a point or sphere)
• The rough alignment can be done by pcl::registration::TransformationEstimationSVD or rtabmap::util3d::transformFromXYZCorrespondencesSVD function.