System and method for hybrid simultaneous localization and mapping of 2D and 3D data acquired by sensors from a 3D scene

A method for registering data, comprising steps of:

• acquiring the data from a scene by a sensor at different viewpoints;
• extracting, from the data, three-dimensional (3D) points and 3D lines, and descriptors associated with the 3D points and the 3D lines;
• selecting a first set of primitives represented in a first coordinate system of the sensor, wherein the first set of primitives includes at least three 3D points;
• selecting a second set of primitives represented in a second coordinate system, wherein the second set of primitives includes any combination of 3D points and 3D lines to obtain at least three primitives;
• and
• registering, using the first set of primitives and the second set of primitives, the 3D points with each other, and the 3D points with the 3D lines to obtain registered primitives, wherein the registered primitives are used in a simultaneous localization and mapping (SLAM) system, wherein the primitives include 3D planes, wherein registering is performed via random sample consensus (RANSAC), which is initialized with triplets of correspondences in a following order of 3 plane-to-plane correspondences, 2 plane-to-plane and 1 point-to-point correspondences, 1 point-to-plane and 2 point-to-point correspondences, 3 point-to-point correspondences, and 3 point-to-point correspondences, and wherein the steps are performed in a processor.