主权项 |
1. A method for determining position and orientation enabling navigation of an object using image data from at least a first and a second 2D image from at least one camera mounted on said object, said method comprising the steps of:
I. matching corresponding points in at least a first and a second 2D image; II. calculating a fundamental matrix by using corresponding image points from at least the first and the second 2D image; III. calculating and extracting estimated first rotation and translation values from the fundamental matrix using singular value decomposition (SVD) based on information from at least the first and the second 2D image; IV. iterating and determining final rotation and translation values by using the LevenBerg-Marquard algorithm; and V. repeating the steps I-IV for several images, resulting in information of the relative camera movement from image 1 to M, enabling navigation of said object; wherein the step of calculating said fundamental matrix comprises the sub-steps of:
a. selecting start correspondence points;b. calculating an initial fundamental matrix;c. calculating the sum of the geometrical projection errors;d. selecting a new correspondence point, implement random sample consensus (RANSAC), recalculate the sum of the geometrical projection errors from the recalculated fundamental matrix, add said new correspondence point if the recalculated sum of the geometrical projection errors is less than before;e. iterating using new start correspondence points, storing the sum of the geometrical projection errors if the new fundamental matrix has less geometrical projection errors than earlier iterations;f. calculating the geometrical projection error in all correspondence points of the total amount of correspondence points, selecting the correspondence points which have a lesser geometrical projection error than a threshold value; andg. iterating using the selected correspondence points and obtaining the fundamental matrix. |