1. A method for determining a three-dimensional (3D) pose of a 3D object in an environment, comprising: extracting features from an image acquired of the environment by a camera; matching the features to a 3D model of the environment to determine correspondences; transforming a camera reference frame of the image and a world reference frame of the environment to a corresponding intermediate camera reference frame and a corresponding world reference frame using the correspondences; applying geometrical constraints to the intermediate camera reference frame and the intermediate world reference frame to obtain a constrained intermediate world reference frame and a constrained world reference frame; and determining the 3D pose from parameters of the constrained intermediate world reference frame and the constrained world reference frame, wherein the steps are performed in a processor.