A method for estimating a camera motion and for determining a three-dimensional model of an environment is provided that includes the steps of: providing intrinsic parameters of a camera; providing a set of reference two-dimensional imaged points captured by the camera at a first camera pose and reference depth samples; determining a three-dimensional model of the environment; providing a set of current two-dimensional imaged points captured by the camera at a second camera pose and current depth samples associated to the set of current two-dimensional imaged points and determining a current three-dimensional model; estimating a camera motion between the first camera pose and the second camera pose; determining a similarity measure between the three-dimensional model and the current three-dimensional model, and if it is determined that the similarity measure meets a first condition, updating the three-dimensional model of the environment and adding the set of current two-dimensional imaged points to the set of reference two-dimensional imaged points.