A method for a six degree of freedom position and orientation determination of a three dimensional known shape in a scenery, by taking a range image by means of a range imaging camera and taking a visual picture by means of a digital camera. The range imaging camera comprises a range image module having a sensor array with a first number of pixels, wherein for each of the first pixels range information from the sensor to a point of the scenery is determined, resulting in a 3D cluster of points. The digital camera comprises an image sensor having a second number of pixels, resulting in a 2D picture. The relation of the first and the second pixels fields of view is known. The method comprises a geometrically fitting of a stored 3D digital representation of the known shape in a virtual space to match with the reproduction of the known object in the 2D picture and the 3D cluster of points and determining the six degree of freedom position and orientation of the known shape in the scenery according to the virtual match.