1. A method of controlling an actuator based on a set of three-dimensional (3D) data points, the method comprising: obtaining a first set of 3D data points for a scene and a second set of 3D data points for a scene with a sensor; extracting at least a first set of planar features from the first set of 3D data points by determining a respective normal vector for each of the first set of 3D data points, wherein the respective normal vector for each of the first set of 3D data points is an estimation of a respective vector normal to the surface at that data point; extracting at least a second set of planar features from the second set of 3D data points by determining a respective normal vector for each of the second set of 3D data points, wherein the respective normal vector for each of the second set of 3D data points is an estimation of a respective vector normal to the surface at that data point; determining a motion between the first set of 3D data points and the second set of 3D data points based on a rotation and a translation from the at least a first set to the at least a second set; and controlling at least one actuator based on the motion.