Method and apparatus for solving position and orientation from correlated point features in images

TECHNICAL FIELD

The present invention relates to the field of computer vision and epipolar geometry, more specifically to a method and apparatus for solving the position and orientation from correlated point features in images using a method that solves for both rotation and translation simultaneously.

The present invention relates to a method for determining the position and attitude of at least one camera by calculating and extracting the estimated rotation and translation values from an estimated fundamental matrix based on information from at least a first and second 2D image.

The present invention further relates to an apparatus for determining the position and attitude of at least one camera by calculating and extracting the estimated rotation and translation values from an estimated fundamental matrix based on information from at least a first and second 2D image, wherein said apparatus comprises a memory and a processor.

BACKGROUND ART

In a number of applications it is desirable to be able to determine the absolute or relative position of objects in images. Alternatively, it may be desirable to determine the absolute or relative position of the camera based on points with known locations in the images. For instance when taking pictures of the same object from two different locations the rotation and translation of the camera can be calculated using different methods.

A camera can be said to have a transfer function mapping a point in 3D space to an image point in the camera image according to where x ₀ and y ₀ are the coordinates of the point in the first image, L(f) is the distortion generated by the lens of the camera, K represents the intrinsic parameters of the camera and X, Y and Z are the coordinates of the point in 3D space. An image of the same object taken from a different position and rotation is calculated by where x and y are the coordinates of the point in the second image, R describes the rotation of the camera and t describes the translation of the camera. If the same camera is used for both images L(f) and K are the same, and ^ and t are desired unknowns.

Today, there exist various examples of solving position and orientation from correlated point features in images. Usually, these methods solve rotation and translation in two different separate steps using for instance the iterative methods LevenBerg-Marquard or Newton-Raphson. Simply setting up equations and using these methods provide unstable and uncertain solutions not necessarily converging fastly or to a correct solution.

The problem is thus to find an improved method and apparatus to rapidly obtain correct and stable convergence in solving for rotation and translation for movements of a camera in order to find the actual position and attitude for the camera. SUMMARY

The present invention is defined by the appended independent claims. Various examples of the invention are set forth by the appended dependent claims as well as by the following description and the accompanying drawings.

With the above description in mind, then, an aspect of the present invention is to provide position and orientation from correlated point features in images using a method that solves for both rotation and translation simultaneously which seeks to mitigate, alleviate, or eliminate one or more of the above- identified deficiencies in the art and disadvantages singly or in any combination.

The object of the present invention is to provide a more rapid and stable convergence in solving the rotation and translation values for the movements of a camera in order to find the actual position and attitude of the camera. This object is achieved by the features of claim 1 wherein, a method determines the position and attitude of at least one camera by calculating and extracting the estimated rotation and translation values from an estimated fundamental matrix based on information from at least a first and second 2D image, characterized in that said method comprising the steps of:

- generating four equations for the estimated rotation and translation values from the estimated fundamental matrix using single value decomposition (SVD);

- testing said four equations for the estimated rotation and translation values with a number of known points deriving one initial-equation which describes an initial estimate of the rotation and translation values; defining an equation system of the equations and substituting variables by changing heading, pitch and roll variables (h, p, r) and position variables (x, y, z) with other parameters, namely: changing h, p, r with a 3x3 matrix and changing x, y, z with ln(x), ln(y) and ln(z); using a parameterization for each 3D point containing the distance D in the first camera trough each matched point; calculating the equation system with k+12 unknown variables, where k equals the number of 3D points and where the 12 unknown variables come from the h, p, r 3x3 matrix yielding 9 unknown variables and 3 unknown variables x, y, z from the translation by solving the numerical equation system with the input values from the initial estimated rotation and translation using LevenBerg-Marquard or any similar process, thereby obtaining a final estimate of the rotation and translation values. The input values from the initial estimated rotation and translation are estimated from the estimated fundamental matrix.

Said object is further achieved by the features of claim 3 wherein, an apparatus determines the position and attitude of at least one camera by calculating and extracting the estimated rotation and translation values from an estimated fundamental matrix based on information from at least a first and second 2D image, wherein said apparatus comprising: a memory; and a processor, characterized in that said memory is encoded with instructions that, when executed, cause the processor to receive input from at least two 2D views wherein the apparatus is capable of: generating four equations for the estimated rotation and translation values from the estimated fundamental matrix using single value decomposition (SVD); testing said four equations for the estimated rotation and translation values with a number of known points deriving one initial-equation which describes an initial estimate of the rotation and translation values;

- defining an equation system of the equations and substituting variables by changing heading, pitch and roll variables (h, p, r) and position variables (x, y, z) with other parameters, namely: changing h, p, r with a 3x3 matrix and changing x, y, z with ln(x), ln(y) and ln(z); using a parameterization for each 3D point containing the distance d in the first camera trough each matched point;

- calculation of the rotation and translation values are performed by calculating the equation system with k+12 unknown variables, where k equals the number of 3D points and where the 12 unknown variables come from the h, p, r 3x3 matrix yielding 9 unknown variables and 3 unknown variables x, y, z from the translation by solving the numerical equation system with the input values from the intitial estimated rotation and translation using LevenBerg-Marquard or any similar process and thereby obtaining a final estimate of the rotation and translation values;

- output the position and attitude for the at least one camera.

According to a further advantageous aspect of the invention, the heading, pitch and roll variables are further determined by adding the following criteria's to the equations: each column shall have the magnitude = 1 yielding three equations; the columns in the matrix are orthogonal yielding three equations; the scale for a specific distance D given a known point is constant yielding one equation. Said object is further achieved by the features of claim 5 wherein, a computer-readable medium has computer executable instructions for performing the method of claim 1 . Any of the advantageous features of the present invention above may be combined in any suitable way. A number of advantages are provided by means of the present invention, for example:

- rapid convergence for rotation and translation for movements of a camera are obtained;

- well defined and stabile convergence to correct value for rotation and translation for movements of a camera when the fundamental matrix

F, from the beginning, has a an accurate estimate are obtained;

- simultaneous calulation for rotation an translation is allowed;

BRIEF DESCRIPTION OF THE DRAWINGS

The present invention will now be described in detail with reference to the figures, wherein:

Figure 1 schematically shows a pictorial representation of the epipolar geometry.

Figure 2 schematically shows a pictorial representation of the epipolar geometry and rotation and translation.

Figure 3 schematically shows a pictorial representation of three translation axes and the rotation around them.

Figure 4 schematically shows a pictorial representation of a flow chart for the process of determining the fundamental matrix according to the present invention.

Figure 5 schematically shows a pictorial representation of the geometrical projection error. Figure 6 schematically shows a pictorial representation of a flow chart for the process of determining the position and attitude of at least one camera.

DETAILED DESCRIPTION

Examples of the present invention relate, in general, to the field of epipolar geometry, in particularly, to determine the position and attitude of at least one camera by calculating and extracting the estimated rotation and translation values from an estimated fundamental matrix based on information from at least a first and second 2D image. Examples of the present invention will be described more fully hereinafter with reference to the accompanying drawings, in which examples of the invention are shown. This invention may, however, be embodied in many different forms and should not be construed as limited to the examples set forth herein. Rather, these examples are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the invention to those skilled in the art. Like reference signs refer to like elements throughout.

All the figures 1 to 6 are schematically illustrated. Epipolar Geometry

Figure 1 shows an example of two cameras taking a picture of the same scene from different points of views. The epipolar geometry then describes the relation between the two resulting views. The epipolar geometry is the intrinsic projective geometry between two views, or images. It is independent of scene structure, and only depends on a camera's internal parameters and relative pose. Epipolar geometry is a fundamental constraint used when images of a static scene are to be registered. When two cameras view a 3D scene from two different positions, there are a number of geometric relations between the 3D points and their projections onto the 2D images that lead to constraints between the image points. This generalized constraint, called epipolar constraint, can be used to estimate relative motion between two images. The epipolar constraint can be written as

x' ^T Fx = 0 [Equation 1] where, x and x' are the homogeneous coordinates of two corresponding points in the two images, and F is the fundamental matrix. The homogeneous coordinates x and x' are vectors. The homogeneous coordinate x' in equation 1 is the transpose of the homogeneous coordinate x' .

Epipolar geometry is the geometry of computer stereo vision. Computer stereo vision is the extraction of 3D information from digital images, such as obtained for example by a camera or a video camera. The camera may be a CCD camera and the images may constitute a video sequence. By comparing information about a scene from several camera perspectives, 3D information may be extracted by examination of the relative perspectives. As previously mentioned, when two cameras view a 3D scene from two different positions, there are a number of geometric relations between the 3D points and their projections onto the 2D images that lead to constraints between the image points. These relations are derived based on the assumption that the camera or cameras can be approximated by the pinhole camera model. The pinhole camera model describes the mathematical and geometric relationship between a 3D point and its 2D corresponding projection onto the image plane

In the epipolar geometry, the relation between two images provided from different cameras may be explained with a correspondence of a point to a line, rather than a correspondence of a point to a point. It could also be the same camera taking images from d ifferent views. Figure 1 shows the epipolar geometry, that for a point in one image, there is a corresponding point in the other image where the two points are the projections of the same physical point in 3D space, the original 3D space point. These two points are normally called correspondence points or corresponding points. The original 3D space point is a point positioned on an item which is imaged by the camera. A plane made by a point in 3D space X, and a first camera 0_, left camera, and a second camera CR, right camera, is called an epipolar plane EP. The intersection line of the epipolar plane EP and the image plane are called an epipolar line. Figure 1 shows, a left image plane IP _L and a left epipolar line EL _L and also a right image plane IPR and a right epipolar line EL _R . The intersection point made by the image plane and the line linking the two cameras O _. and CR is called epipole. Referring to figure 1 , the points E _L and E _R are the epipoles. All epipolar lines intersect at the epipole regardless of where the 3D space point X is located. An epipolar plane EP intersects the left and right image planes IPR, IP _l in the left and right epipolar lines EL _L , EL _R and defines the correspondence between the lines. Figure 1 show, a distance D relating to the distance from the first camera through the image point.

For a point in one image, the corresponding point in the second image is constrained to lie on the epipolar line. This means that for each point which is observed in one image the same point must be observed in the other image on a known epipolar line. This provides the epipolar constraint relation which corresponding image points must satisfy and it means that it is possible to test if two points really correspond to the same 3D point in space. Epipolar constraints can also be described by the essential matrix E or th e fundamental matrix F. The essential matrix is a 3x3-matrix and has rank 2. The essential matrix has 5 degrees of freedom, 3 for rotation and 2 for translation. Both the fundamental matrix and the essential matrix can be used for establishing constraints between matching image points. However, the essential matrix can only be used in relation to calibrated cameras since the intrinsic parameters, i.e. the internal camera parameters, must be known when using the essential matrix. The essential matrix only encodes information of the extrinsic parameters i.e. the external camera parameters which are rotation R and direction of translation T from one camera to the other. The extrinsic parameters determine the cameras orientation towards the outside world. Rotation and translation are the external parameters which signify the coordinate system transformations from 3D world coordinates to 3D camera coordinates. The rotation and translation parameters define the position of the camera center and the camera's heading in world coordinates. If the cameras are calibrated, the essential matrix can be used for determining both the relative position and orientation between the cameras and the 3D position of corresponding image points. The fundamental matrix encodes information of both the extrinsic parameters and the intrinsic parameters. The fundamental matrix encapsulates the epipolar geometry and will be further described below.

Fundamental Matrix

The fundamental matrix contains all available information on the camera geometry and it can be computed from a set of correspondence points. The fundamental matrix defines the geometry of the correspondences between two views, or images, in a compact way, encoding intrinsic camera geometry as well as extrinsic relative motion between two cameras. The fundamental matrix is a homogeneous 3x3-matrix, entity of 9 pa rameters, and is constrained to have a rank 2. The fundamental matrix has seven degrees of freedom. If a point in 3-D-space X is imaged as in the first image (left image in figure 1 ), and as x' in the second image (right image in figure 1 ), then the image points satisfy the relation of equation 1 .

F is the fundamental matrix and can be estimated linearly from equation 1 , given a minimum of 8 correspondence points between two images. 8 correspondence points results in 8 points in the first image and 8 points in the second image, giving a total number of 16 points.The fundamental matrix is the algebraic representation of epipolar geometry, independent of scene structure and can be computed from correspondences of imaged scene points alone, without requiring knowledge of the cameras' internal parameters or relative pose.

Estimating the fundamental matrix is crucial for structure and motion problems where information, such as when the location of the camera and 3D scene is retrieved from several images. Estimating the fundamental matrix is vital for image based navigation and since the fundamental matrix contains the internal camera parameters and the rigid transformation between two cameras, it is also used in a number of different areas such as video tracking, stereo matching, image rectification and restoration, object recognition, outlier detection and motion estimation.

A set of correspondence points between two images will always contain noise and outliers. Outliers refer to anomalies, or errors, in a given data set. It is an outlying observation that appears to deviate markedly from an optimal or accurate solution. The outliers may come from extreme values of noise or from erroneous measurements or incorrect hypotheses about the interpretation of data. Outliers may also be defined as data that do not fit the model. In order to estimate an accurate solution, it is preferable to find a solution without deviations and anomalies in the data set, i.e. outliers. Thus, based on a predetermined criterion, outliers are false feature correspondences.

Correct feature correspondences are called inliers and they result in an accurate solution. Inliers may also be defined as data whose distribution can be explained by some set of model parameters. The fundamental matrix can be estimated from the information of the correspondence points and in order to achieve an accurate and precise fundamental matrix, it is important to minimize the effect and influence of outliers. The fundamental matrix is sensitive to anomalies, or errors, in the corresponding points. Thus, it is essential to select inliers for a precise and accurate fundamental matrix. The methods and algorithms used for estimating the fundamental matrix are linear method, iterative method, and robust method.

For a specific method of obtaining an accurate fundamental matrix which will be further disclosed, a linear method is used in order to first estimate an initial fundamental matrix and eventually an accurate fundamental matrix. The eight-point algorithm is used when estimating an initial fundamental matrix. The eight-point algorithm is fast and easily implemented. The eight- point algorithm is used in the specific method of obtaining an accurate fundamental whether only eight corresponding points or more than eight corresponding points are used. When using eight or more than eight correspondence points, eight-point algorithm together with single value decomposition (SVD) with Frobenius norm is used in the specific method of obtaining an accurate fundamental in order to calculate an initial fundamental matrix. The SVD minimizes the Froben ius norm so that the rank of the resulting matrix is 2. SVD is an important factorization of a rectangular real or complex matrix, with many applications in, for example, signal processing and statistics. Applications which employ the SVD include computing the pseudoinverse, least squares fitting of data, matrix approximation, and determining the rank, range and null space of a matrix. Frobenius norm is a matrix norm which is a natural extension of the notion of a vector norm to matrices. The method of obtaining an accurate fundamental matrix provides a solution that combines the linear method, the iterative method and the robust method rejecting models containing outliers.

An iterative method used in the method for obtaining an accurate fundamental matrix are based on optimizing and finding a lesser geometrical projection error between the correspondence points than earlier iterations or than a threshold value. The number of iterations may vary. Criteria's for the number of iterations made in the present invention may be a time constraint and/or a threshold value and/or a pre-determined value representing the amount of iterations. Further, random sample consensus (RANSAC) is used for obtaining an accurate fundamental matrix. RANSAC is an iterative method used to estimate parameters of a mathematical model from a set of observed data which contains outliers. RANSAC has the ability to do robust estimation of the model parameters, i.e., it can estimate the parameters with a high degree of accuracy even when significant amount of outliers are present in the data set. RANSAC produces a reasonable result with a certain probability. This probability is increasing as more iterations are allowed. RANSAC also assumes that, given a set of inliers, there exists a procedure which can estimate the parameters of a model that optimally explains or fits this data.

A fundamental matrix which is obtained may then be used for correspondence of all feature points between the two images.

External camera parameters (rotation R and translation T)

When the fundamental matrix F and the internal camera parameters are known we can solve the external camera parameters (rotation and translation) and determine 3D structure of a scene. The external parameters determine the cameras orientation towards the outside world. R is the 3x3 rotation matrix and T is the 1 x3 translation vector. Referring to figure 2, if the left camera CL represents a global frame of reference in which objects exist (world frame), then the other camera CR is positioned and orientated by a Euclidean transformation (rotation R, translation T).

The essential matrix can be determined from the fundamental matrix and the camera calibration 3x3 matrix K . K is also called projection matrix and represents the intrinsic parameters of the camera. In order to determine rotation and translation from one camera to the other camera, rotation and translation can be determined by factoring the essential matrix with single value decomposition (SVD) to 4 different possible combinations of translations and rotations. One of these combinations is correct. By looking at their geometric interpretation we may determine which of these combinations are correct. Figure 3 shows how a camera may be rotated around three different axes which are the longitudinal axis, the lateral axis and the vertical axis. The rotation around these axes is called roll, heading (yaw) and pitch. The rotation and translation components are extracted from the essential matrix and then the 3D point locations can be determined.

One embodiment of the present invention

A method and apparatus are provided in this detailed disclosure for obtaining and solving rotation and translation for movements of a camera in order to find position and orientation for the camera. In the embodiment of the present invention, the method is computer-implemented. The computer-implemented method can be realized at least in part as one or more programs running on a computer as a program executed from a computer-readable medium such as a memory by a processor of a computer. The programs are desirably storable on a machine-readable medium such as a floppy disk or a CD-ROM, or solid state memory for distribution and installation and execution on other computers. Further, the computer-implemented method can be realized at least in part in hardware by using a programmable array in a processor/FPGA technique or other hardware.

In order to radpidly and with a correct and stable convergence solve position and orientation from correlated point features in images, the present method and apparatus substitutes and changes variables, using correct input values and an iterative LevenBerg-Marquard algorithm. The variable substitution strengthens the derivatives for the translation values and provides a more rapid convergence. Convergence is the approach towards a definite value, and in mathematics it describes limiting behavior, particularly of an infinite sequence or series. The present invention strengthens the derivatives for the translation values in such a manner that translation and rotation are calculated simultaneously and causing the different variables to have an equivalent effect in present method and apparatus. The variable substitution in the present invention solves rotation and translation in one simultaneous step, resulting in a balance between the rotation and the translation.

In order to minimize the number of outliers and to provide an accurate fundamental matrix, the present method and apparatus may use the geometrical projection errors of the correspondence points to choose the fundamental matrix with minimized outliers and the least error. Figure 4 schematically shows a flowchart of a specific method of obtaining an accurate fundamental matrix. This specific method of obtaining an accurate fundamental matrix starts with first, selecting a number of at least 8 start correspondence points (step I). The pairs of at least 8 corresponding start points may be selected in a normal distribution random manner. The pairs of at least 8 corresponding start points may be randomly selected. Then, an initial fundamental matrix is calculated from the eight-point method and single value decomposition (SVD) with normalized frobenius norm (step II). The number of start correspondence points which is selected in order to obtain an initial fundamental matrix in steps (I) and (II) is preferably in the range of 12 to 15. Selecting start correspondence points in the range of 12 to 15 provides a robust and statistically good start for obtaining an accurate fundamental matrix.

Then, the sum of the geometrical projection errors of said start correspondence points from said initial fundamental matrix is calculated (step III). Figure 5 shows a point X in 3D space imaged as x in a first image and as x in a second image. In order to receive the geometrical projection error, the point x in the first image is projected to the second image as x _d ' and the point x' in the second image is projected to the first image as Xd. The projection is done using the fundamental matrix in combination with SVD. Figure 5 further show the error distance do being the distance between x and Xd and the error distance di being the distance between x' and x _d ' . The error distances do and di result in the geometrical projection error d of the corresponding points. The error distances do and di is the orthogonal distance between x and Xd; and x' and x _d ' respectively. The geometrical projection error d is determined b the following equation: d = [Equation 3]

The sum of the geometrical projection errors of said start correspondence points is calculated by first calculating an estimate of each 3D point's location for said fundamental matrix F and for each pair of correspondence points which results in an estimated 3D coordinate for each pair of correspondence points. This may be done by using SVD or a cross product resulting in a calculated coordinate in 3D for each pair of correspondence points where each points respective rays r, shown in figure 5, intersect with each other. Then, calculate the geometrical projection error of said projected 3D coordinate, using the homography of said fundamental matrix. Finally, summarize the geometrical projection errors and divide the total sum with a number which represents the amount of correspondence points. The sum of the geometrical projection errors from said initial fundamental matrix is now obtained. The projected geometrical error is later used in the random sample consensus (RANSAC) to determine an error of each correspondence point. By using the estimated projected geometrical error, the errors are made clear and protrude like spikes in a distance error graph between a first and a second image whereas outliers are easily detected. Enabling greater quantifiable boundaries between inl iers and outliers and the number of outliers can be minimized.

Then, select a new correspondence point, using random sample consensus (RANSAC), recalculate the fundamental matrix with said new correspondence point, recalculate the sum of the geometrical projection errors from the recalculated fundamental matrix and add said new correspondence point if the recalculated sum of the geometrical projection errors is less than before (step IV).

Then, iterate step l-IV using new start correspondence points, until a pre- determined iteration value N is obtained, store the sum of the geometrical projection errors of said new start correspondence points and the corresponding new fundamental matrix if the new fundamental matrix has less geometrical projection errors than earlier iterations (step V). New start correspondence points may be selected in a normal distribution random manner. The new start correspondence points may be randomly selected. Criteria's for the number of iterations made in step V may be a time constraint and/or a pre-determined amount of iterations. The number of iterations made in step V depends on the pre-determined iteration value N. The number of iterations, i.e. the pre-determined iteration value N, may vary depending on a time constraint or a selected and pre-determined number of iterations. The number of iterations which shall be done in the present invention may be set to a pre-determined amount of iterations, such as for example 10 iterations or 100 iterations or 1000 iterations. The number of iterations may be set to a pre-determined number based on the number of sample points n, i.e. correspondence points. The pre-determined iteration value N in step V may be a pre-determined number of iterations determined by the following equation:

N = n ² [Equation 2]

where n is the number of the sample points, i.e. corresponding points. Further, the pre-determined iteration value N in step V may be a pre- determined number of iterations determined by the following equation:

N = nlogn [Equation 4]

where n is the number of the sample points, i.e. corresponding points. Further, a time constraint may be used to regulate the number of iterations performed in step V. The time constraint may be set to any appropriate time, such as 5 seconds or 10 seconds. The iteration in step V is completed either when a pre-determined number N of iterations are obtained or when a predetermined iteration time is obtained. The pre-determined number of iterations or the pre-determined iteration time corresponds to the predetermined iteration value in step V. A great number of points, for example 1000 or several 1000 points, may stored which results in less error per number of included sample, i.e. correspondence points. This since the sum of the errors is d ivided with a number which represents the amount of correspondence points. Thus, the more points stored the less error per number of included samples.

Then, calculate the geometrical projection error in all correspondence points of the total amount of correspondence points and select the correspondence points which have a lesser geometrical projection error than a threshold value (step VI). The threshold value may be used to regulate the number of iterations in step VI I and depends on the accuracy of the corresponding measurements of the corresponding points. The threshold value is an accuracy constraint used for the calculated geometrical projection error and may be set to any appropriate value. The threshold value is preferably a value less than a tenth of a pixel dimension size. The pixel dimension size is the height and the width of one pixel in an image. The pixel dimension size is the horizontal and vertical measurements of one pixel in each dimension in an image expressed in pixels. Further, the threshold value may be determined by the following equation:

TV = 2/h [Equation 5] where, TV is the threshold value and h is the height of one pixel in the image. Further, the threshold value may be determined by the following equation:

TV = 2/w [Equation 6] where, TV is the threshold value and w is the width of one pixel in the image. Finally, iterate step l-VI using said selected correspondence points which have a lesser geometrical projection error than a threshold value and obtain the fundamental matrix. Steps l-VI are iterated and repeated recursively and successively with lower threshold values until the number of correspondence points is stable and no correspondence points are removed and thereby obtaining the fundamental matrix (step VII). By repeating the steps l-VI the method of obtaining a fundamental matrix starts from the first step I, with a lesser amount of points which are statistically better. Most of the outliers are removed and also the points which have a lesser geometrical projection error and that does not match are removed. By recursively and successively repeating the method for obtaining an accurate fundamental matrix with different threshold values, an accurate and precise fundamental matrix is obtained until for example no points are removed. Thus, the method for obtaining a fundamental matrix can be optimized by controlling different parameters such as a threshold value or a significance value of points. Since the method for obtaining a fundamental matrix does not start with the first best solution in the beginning of the method it allows a great number of points to be included in providing a robust method with a stable convergence for obtaining an accurate fundamental matrix.

The fundamental matrix used in the present invention may also be obtained by any other type of method for estimating the fundamental matrix, preferably the method described above.

Figure 6 schematically shows a flowchart of a specific method according to the present invention. A method and apparatus for determining the position and attitude of at least one camera by calculating and extracting the estimated rotation and translation values from an estimated fundamental matrix based on information from at least a first and second 2D image is provided in the present invention. Hereinafter, referring to figure 6, the method of the present invention is explained in detail. To determine the position and attitude of at least one camera, first, generate four equations for the estimated rotation and translation values from the estimated fundamental matrix using single value decomposition (SVD) (step CI).

Then, test said four equations for the estimated rotation and translation values with a number of known points deriving one initial-equation which describes an initial estimate of the rotation and translation values (step CM). This initial-equation clearly and unambiguously describes a first initial estimate of rotation and translation.

Then, define an equation system of the equations and substitute variables by changing heading, pitch and roll variables (h, p, r) and position variables (x, y, z) with other parameters, namely: changing h, p, r with a 3x3 matrix and changing x, y, z with ln(x), ln(y) and ln(z) (step III). Heading h, pitch p and roll r normally have weak derivatives which results in poor and slow convergence. The translation (x, y, z) normally also have weak derivatives which result in poor and slow convergence close to zero which may provide an instability between the translation and rotation. In mathematics, a weak derivative is a generalization of the concept of the derivative of a function (strong derivative) for functions not assumed differentiate, but only integrable. Further, weak derivatives do not converge as fast as strong derivatives.

The changing and substitution of the variables in the present invention provides stronger derivatives and faster convergence. When changing and substituting the heading, pitch and roll variables (h, p, r) with a 3x3 matrix, i.e. with 9 variables, the heading, pitch and roll variables are further determined by adding to the equations, that each column shall have the magnitude=1 yielding 3 equations, that the columns in the matrix are orthogonal yielding 3 equations and that the scale for a specific distance D given a known point is constant yielding 1 equation. The derivative for rotation R is higher than the derivative for translation T. The substitution of the translation variables (x, y, z) with ln(x), ln(y) and ln(z) with derivatives of 1/x, 1/y and 1/z are provided and this provides strong derivatives for small values of (x, y, z) which then results in a faster convergence for the translation than for the rotation. This adds to balance the derivatives between translation and rotation resulting in that the rotation and the translation variables have equivalent effect in the determination of the position an attitude of at least one camera, resulting in a more accurate estimate of the rotation and translation. The convergence of the translation is faster after the substitution of the translation variables (x, y, z) to ln(x), ln(y) and ln(z). The present invention containing the substitution of the translation variables (x, y, z) to ln(x), ln(y) and ln(z) provides a solution which includes a faster convergence than before. Without this variable substitution the rotation would have greater effect than the translation in the determination of the position an attitude of at least one camera, providing unstable and uncertain solutions not necessarily converging fastly or to a correct solution.

Then, use a parameterization for each 3D point containing the distance D in the first camera trough each matched point (step CIV). A point is clearly and unambiguously described in 3D with only one parameter which relates to the distance D from the first camera trough the image point. The distance D, shown in figure 1 , provides depth in the image. Finally, calculate the equation system with k+12 unknown variables, where k equals the number of 3D points and where the 12 unknown variables come from the h, p, r 3x3 matrix yielding 9 unknown variables and 3 unknown variables x, y, z from the translation by solving the numerical equation system with the input values for the estimated rotation and translation from step CI using LevenBerg-Marquard or any similar process, thereby obtain a final estimate of the rotation and translation values (step CV). By iterating LevenBerg-Marquard or any similar process a final estimate of the rotation and translation values with a predefined accuracy is obtained. The final estimate is an accurate estimation of the rotation and translation values of the movements of the camera, i.e. the position and attitude of the camera. In mathematics and computing, the Levenberg-Marquardt algorithm provides a numerical solution to the problem of m in im izing a function , generally nonlinear, over a space of parameters of the function. Levenberg-Marquardt is an iterative procedure.

An apparatus may output the position and attitude for the at least one camera (step CVI) in order to find position and orientation for the camera.

The views encapsulated by the fundamental matrix may be images from a camera, or images or a video sequence from a video camera. The invention is not limited to the example described above, but may be modified without departing from the scope of the claims below.

The terminology used herein is for the purpose of describing particular examples only and is not intended to be limiting of the invention. As used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It will be further understood that the terms "comprises" "comprising," "includes" and/or "including" when used herein, specify the presence of stated features, integers, steps, operations, elements, and/or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and/or groups thereof.

Unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs. It will be further understood that terms used herein should be interpreted as having a mean ing that is consistent with their mean ing in the context of th is specification and the relevant art and will not be interpreted in an idealized or overly formal sense unless expressly so defined herein.

The foregoing has described the principles, preferred examples and modes of operation of the present invention . However, the invention should be regarded as illustrative rather than restrictive, and not as being limited to the particular examples discussed above. The different features of the various examples of the invention can be combined in other combinations than those explicitly described. It should therefore be appreciated that variations may be made in those examples by those skilled in the art without departing from the scope of the present invention as defined by the following claims.