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 point; extracting at least a second set of planar features from the second set of 3D data points; 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.