A system of selecting a keyframe for iterative closest point (ICP) includes a reference frame selector that generates a reference frame according to a current frame and a current keyframe; an ICP loop unit that performs ICP on the reference frame and the current frame, thereby generating a pose of the current frame; and a keyframe update unit that generates a new keyframe according to an offset condition between the pose of the current frame and a pose of the reference frame.