1. A method for registering data, wherein the data have three dimensions, comprising the steps of: selecting a first set of primitives from the data in a first coordinate system, wherein the first set of primitives includes at least one plane, at least one point, and a third primitive that is either a point or a plane; selecting a second set of primitives from the data in a second coordinate system, wherein the second set of primitives includes at least one plane, at least one point, and a third primitive corresponding to the third primitive in the first set of primitives; determining a six degrees of freedom rigid body transformation [R, t] between the first coordinate system and the second coordinate system minimizing an error metric formed by a combination of a point-to-point error metric and a plane-to-plane error metric, wherein the point-to-point error metric includes a distance between the point in the first coordinate system and the point in the second coordinate system transformed with the six degrees of freedom rigid body transformation, and wherein the plane-to-plane error metric includes a distance between the plane in the first coordinate system and the plane in the second coordinate system transformed with the six degrees of freedom rigid body transformation; and registering, in the first and second set of primitives, planes with each other and points with each other to obtain registered primitives including registered points and registered planes, such that coordinates of the registered points are matched with each other, and normals of the registered planes and distances between the registered planes and an origin of the first or the second coordinate system are matched with each other, using the six degrees of freedom rigid body transformation wherein R in the six degrees of freedom rigid body transformation is a rotation matrix and t is a translation vector, wherein the registering is used in a simultaneous localization and mapping system wherein steps of the method are performed by a processor wherein the data has been acquired as a depth map acquired by a sensor.