WO2021094636A1

WO2021094636A1 - Method and system for the spatial tracking of objects

Info

Publication number: WO2021094636A1
Application number: PCT/ES2020/070697
Authority: WO
Inventors: Aitor Olarra Urberuaga; Gorka KORTABERRIA BERRIOZABAL; Brahim Ahmed CHEKH OUMAR; Andoni Delgado Castrillo
Original assignee: Fundación Tekniker
Priority date: 2019-11-13
Filing date: 2020-11-11
Publication date: 2021-05-20
Also published as: ES2824873A1

Abstract

The invention relates to a method for estimating the position and orientation of an object with 6 degrees of freedom (T_x1, T_y1, T_z1, Φ, θ, Ψ), which comprises: connecting a device (60) to the object of which the position and orientation (T_x1, T_y1, T_z1, Φ, θ, Ψ) are to be estimated, the device (60) comprising a mirrored surface (55) and a set of N reference points (71), each reference point being defined by a position (X1, Y1, Z1) in a coordinate system defined in the device (60); placing a camera (40) aimed at the device (60) connected to the object; measuring the angles of pitch or inclination θ and of yaw or turning Ψ that represent the inclination of the mirrored surface (55) and, therefore, of the object, the measurement being carried out by applying an autocollimation technique; capturing an image of the set of reference points (71; (X1, Y1, Z1)) included in the device (60), thereby obtaining, in a plane (61) of a sensor (42) of the camera, observed image points (72; (x'1, y'1)) corresponding to the reference points (71; (X1, Y1, Z1)); obtaining the angle of rotation or roll Φ of the object and a vector of translation (Tx, Ty, Tz) between the camera (40) and the object, using the reference points (71; (X1, Y1, Z1)) and the observed image points (72; (x'1, y'1)) obtained in the sensor (42) of the camera, by applying a spatial resection algorithm restricted by the average angles of pitch or inclination θ and of yaw or turning Ψ, wherein (Τx, Τy, Tz, Φ, θ, Ψ) represent the relative position and relative orientation between the camera (40) and the object.

Description

METHOD AND SYSTEM FOR SPACE TRACKING OF OBJECTS

TECHNICAL FIELD

The present invention relates to the field of spatial object tracking. In particular, it relates to non-contact measurement techniques to improve the precision that can be achieved in spatial tracking of objects by means of a spatial resection procedure.

STATE OF THE ART

Currently, there are different technologies and measurement methods for non-contact spatial tracking of objects. One of these techniques is called photogrammetry, which is based on extracting three-dimensional measurements from two-dimensional data (that is, images). Typically, photogrammetry uses the spatial resection method to obtain the exterior orientation of a single image. In spatial resection, the spatial position and orientation of a camera is determined based on the central projection of the camera and modeling of optical distortion due to lens shape errors. The pinhole camera model represents the mathematical definition of light output through a camera lens between the 3D world (the object space) and a 2D image (the sensor plane). This is outlined in Figure 1, which depicts how to obtain the position and orientation of an object 11 using a single camera 10 by applying the spatial resection method. The following equation describes the rigid transformation between the camera coordinate system and the object coordinate system:

where it indicates the spatial position of the camera 10, it indicates the

spatial position of object 11 and the matrix represents the relative rotation and displacement from object 11 to camera 10. As shown in figure 1, θ represents elevation angle (also referred to as pitch or pitch angle) ( relative pitch angle) of object 11, ψ represents the yaw angle (also referred to as yaw angle) relative to object 11, F represents the yaw angle (also referred to as roll angle ) (roll angle) of the object 11 yd _TxTyTz represents the relative translation T _x , T _y and T _z. In essence, the spatial resection method iteratively minimizes the planar distance between the observed image points and those that are theoretically projected in order to determine the position and orientation of the camera 10 with 6 degrees of freedom (DOF DOF) that best fit the corresponding points. At least three reference points must be marked on the object. In Figure 1, six circles depicted on the object 11 represent corresponding object reference points. Each circle has an Xi, Yi, Zi coordinate.

However, photogrammetric setups using spatial resection techniques may have precision limitations when estimating the external orientation parameters of a camera with 6 DOF due to the high correlation between the orientation and translation parameters of the camera. Although, with convenient settings for camera position, object size, and reference system (which has to be previously stated), accuracies of about 1: 10,000 of the measurement volume can be achieved, translations in the Z axis and rotations around pitch or bank angles θ and yaw ψ are critical. This is shown in Figure 2. Figure 2 (top) represents an example of a low correlation between the orientation and translation parameters of a camera. Figure 2 (bottom) represents an example of high correlation between the orientation and translation parameters of a camera. In Figure 2 (top), the target reference point distribution (how the reference points are distributed on the object) generates a different image after camera translation and a different image after camera rotation. However, in Figure 2 (bottom) the target distribution generates nearly identical images after camera translation and camera rotation. As a consequence, in the second scenario (lower part), no precise differentiation is possible between the rotations around the pitch or bank angles ψ and turn ψ and the translations T _x and T _y.

Another well-known angle measurement technique is autocollimation. Autocollimation is an optical setup in which a collimated beam of parallel light rays exits an optical system and is reflected back to the same system by a flat mirror. Autocollimation is used to measure small tilt angles of the mirror with high precision. However, most of the autocollimation techniques have a limitation to estimate the angle of rotation or roll of a mirror. In other words, most existing autocollimation devices are capable of measuring only two bank angles (pitch or pitch and pitch). turn) of a mirror. Recently, new autocollimators have been developed, which have a certain ability to measure an angle of rotation around the normal axis of the mirror based on a special lens. This is the case, for example, of TriAngle®3D, from the company TRIOPTICS. However, the accuracy of the roll or roll angle measurement (the angle of rotation) is significantly reduced compared to the accuracy of the pitch and bank angle measurement.

Therefore, there is a need for a method and system for precise spatial tracking and / or monitoring of an object, which improves the accuracy in the 6 DOF.

DESCRIPTION OF THE INVENTION

The method and system described in the present invention are intended to solve the drawbacks of the prior art. In the present invention, two non-contact measurement techniques are combined in order to improve the precision that can be achieved in spatial monitoring or tracking of objects. The two techniques are (a) photogrammetry, which is applied with a single camera, and (b) autocollimation, which is used to estimate the absolute angles of inclination of a specular surface that, with a calibrated transformation, represents the pitch (bank) and yaw angles of the object being tracked. With regard to photogrammetry, the problem of orientation and external translation of a camera by means of a single image is addressed by applying the spatial resection technique. Regarding autocollimation, the camera orientation (pitch (bank) and yaw angles) is obtained with respect to the object's coordinate system that is constructed from a set of objectives (targets). In this way, the spatial resection technique is restricted with the orientation values obtained (the pitch (bank) and turn angles) and the remaining parameters (T _x T _and T _z and the angle of roll or roll F) are they can estimate with higher precision and lower correlation between them. Therefore, these are estimated with a better (lower) uncertainty. In other words, a restricted spatial resection is applied instead of a conventional spatial resection.

A first aspect of the invention refers to a method for estimating the position and orientation of an object with 6 degrees of freedom, comprising: attaching an artifact to the object whose position and orientation are to be estimated, where the artifact comprises a specular surface and a set of N reference points, where each reference point is defined by a position X _i , Y _i , Z _i in a coordinate system defined in the artifact, where 1 <i <N and N>2; placing a camera facing the artifact that is attached to the object; measuring the angles of elevation, pitch or inclination θ and deviation or yaw ψ that represent the inclination of the specular surface and, therefore, of the object, said measurement being carried out by applying an autocollimation technique; capturing an image of the set of reference points comprised in the artifact that is attached to the object, thereby obtaining, in the plane of the camera sensor, observed image points corresponding to said reference points; obtain the angle of rotation or roll F of the object and a translation vector (T _x T _and T _z ) between the camera and the object, using said reference points and said observed image points that are obtained in the camera sensor, by applying a spatial resection algorithm restricted by the angles of elevation, pitch or inclination θ and of deviation or yaw ψ measured, where Tc, Tg, Tz , F, q, ψ represent the relative position and orientation between the camera and the object.

In embodiments of the invention, the angles of elevation, pitch or bank θ and deviation or yaw ψ representing the inclination of the specular surface and therefore of the object, are measured as follows by applying an autocollimation technique: emit a non-collimated light beam from a light source that is arranged in the chamber; redirecting the emitted light beam by means of a beam splitter, producing non-collimated light beams; collimating said light beams by a camera lens, thereby producing collimated light beams; directing said collimated beams towards the plane mirror, in which they are reflected, thereby providing reflected collimated light beams; focusing said reflected collimated beams on the camera lens, thereby providing focused reflected beams of light to the camera sensor, said reflected beams of light being focused at a position on the plane of the camera sensor; calculate the angles of elevation, pitch or bank θ and deviation or yaw ψ as follows: θ = δx '/ 2f θ = δx '/ 2f where f is the focal length of the camera and δc', δy 'represent a plane displacement between the measured position and a fixed reference position on the plane of the camera sensor, this fixed reference representing the position (x ', y') in the sensor plane where the angles of elevation, pitch or bank θ and of deviation or yaw Ψ are equal to 0.

In embodiments of the invention, said roll or roll angle F and said translation vector between the camera and the object are obtained by implementing a spatial resection technique, restricted by the elevation, pitch or tilt angles θ and of deviation or yaw ψ already obtained, iteratively solving the following optimization problem:

where N is the number of image points observed; (x ' _i , y' _¡ ) are the observed image points; and (X ' _Ei , y' _Ei ) are the estimated image points.

In embodiments of the invention, the estimated image points (X ' _Ei , and' _Ei ) are obtained by applying a pinhole camera model that represents the projection (X ' _Ei , and' _Ei ) of each 3D reference point on the camera sensor image plane:

where:

which can be simplified as: and where: is the intrinsic matrix of the camera

and where l is a scale factor.

In embodiments of the invention, the estimated image points (X ' _Ei , and' _Ei , 0) are calculated as follows by applying the central projection equation in computer vision:

In embodiments of the invention, the estimated image points (X ' _Ei , and' _Ei , 0) are calculated as follows by applying collinearity equations:

In embodiments of the invention, before applying an autocollimation technique to measure the angles of elevation, pitch or pitch 0 and deviation or yaw ψ and capturing, with the camera, an image of the artifact that is attached to the object, it is established a geometric relationship between the plane of the specular surface and a coordinate system formed by the set of reference points. In embodiments of the invention, the artifact has been measured in a Coordinate Measuring Machine (CMM) in order to establish said geometric relationship.

The proposed method can be implemented either in a single device that encompasses both the functionality of a camera and an autocollimator; or in two separate devices - a camera and an autocollimator - that work together.

A second aspect of the invention refers to a system for estimating the position and orientation of an object with 6 degrees of freedom, comprising: means for taking photogrammetry measurements; means for carrying out autocollimation; and an artifact that is attached to the object whose position and orientation are to be estimated, wherein said artifact comprises a specular surface and a set of N reference points, where each reference point is defined by a position in a coordinate system defined in the artifact, where 1 <i <N and N>2; wherein the means for carrying out autocollimation are configured to measure the angles of elevation, pitch or bank θ and deviation or yaw ψ that represent the inclination of the specular surface and, therefore, of the object; wherein the means for taking photogrammetry measurements are configured to: capture an image of the set of reference points comprised in the artifact that is attached to the object, thereby obtaining some points observed image images corresponding to said reference points; and obtain the angle of rotation or roll F of the object and a translation vector (T _x T _and T _z ) between the means for taking photogrammetry measurements and the object, using said reference points and said observed image points, by means of the application of a spatial resection algorithm constrained by the measured elevation, pitch or bank angles θ and deviation or yaw ψ.

In embodiments of the invention, the means for taking photogrammetric measurements and the means for carrying out autocollimation are comprised of a chamber.

In embodiments of the invention, the means for taking photogrammetric measurements are comprised of a camera and the means for performing autocollimation are comprised of an autocollimator.

In embodiments of the invention, the N reference points represent a checkerboard pattern, or a circular pattern, or a square pattern, or a cross-shaped pattern, or follow special markers.

In embodiments of the invention, the specular surface is a flat mirror or any flat or semi-flat surface capable of producing a specular reflection of light. In embodiments of the invention, the system further comprises mounting means for mounting a plurality of measurement means for measuring the spatial location and orientation of the mirror.

The device can be embodied in at least two different configurations. In a first embodiment, a single device integrates the two technologies (camera and autocollimator). In this case, the device, such as A camera is made up of a light source - such as an LED source - and a beam splitter. The light source and beam splitter can be mounted on the camera. Therefore, simply by modifying the acquisition parameters of the camera, the camera can act as an autocollimator. In a second embodiment, the device is comprised of a single camera and a separate 2D autocollimator. This configuration requires a previous extrinsic calibration phase, to collect independent data and obtain output data with respect to the same reference system.

A third aspect of the invention relates to a computer program product comprising computer program instructions / code for carrying out the disclosed method.

A fourth aspect of the invention relates to a computer-readable memory / medium that stores instructions / program code to carry out the disclosed method. The combination of a camera configured for photogrammetry and an autocollimator, either in a single device or in two separate devices, provides the following advantages: (a) It enhances the precision of measurements that can be obtained as a consequence of the reduction in the uncertainty to identify four camera parameters (the orientation - the roll or roll- and T _x T _and T _z ) using spatial resection techniques, thanks in turn to the restriction of the orientation parameters - the pitch or tilt and the yaw - by autocollimator. (b) Increases the number of DOF in measurements with respect to existing autocollimators, by adding the necessary DOF through spatial resection techniques, (c) Enhances the performance and applicability of the techniques of Spatial resection to the demands of high precision and application thanks to the introduction of angle constraint (pitch or bank and yaw).

Certain additional advantages and features of the invention will become apparent from the detailed description that follows and will be pointed out in particular in the appended claims.

BRIEF DESCRIPTION OF THE DRAWINGS

To complete the description and in order to provide a better understanding of the invention, a set of drawings is provided. Said drawings form an integral part of the description and illustrate one embodiment of the invention, which should not be construed as restricting the scope of the invention, but merely as an example of how the invention can be carried out. The drawings comprise the following figures:

Figure 1 depicts the spatial resection technique, which is a well-known technique for determining the spatial position and orientation of a camera based on the central projection of the camera.

Figure 2 shows exemplary photogrammetric configurations with different correlation between translation and rotation of a camera.

Figures 3 (a) and 3 (b) show side views of a chamber used in embodiments of the present invention. Part of the outer casing has been removed to show the inside of the camera. Figure 3 (c) shows an exploded view of the chamber shown in Figures 3 (a) and 3 (b).

Figure 4 shows a measurement artifact to be attached to an object to perform 6 DOF follow-up measurements, according to embodiments of the present invention. The artifact includes a mirror surface and a set of reference points.

Figure 5 shows a schematic according to embodiments of the invention, in which autocollimation measurements are taken with the camera of Figures 3 (a) - (c) and the measurement artifact of Figure 4.

Figure 6 shows exemplary components of a device for performing the autocollimation technique.

Figure 7A shows a schematic according to embodiments of the invention, in which photogrammetry measurements are taken with the camera of Figures 3 (a) - (c) and the artifact of Figure 4. Figure 7B shows the schematic of Figure 7A, further including the 6 parameters to be measured.

Figure 8 shows an alternative scheme according to some embodiments of the invention, in which the spatial monitoring or tracking of an object is achieved by means of a camera and a separate 2D autocollimator, plus a measurement artifact as shown. in figure 4.

DESCRIPTION OF A WAY OF CARRYING OUT THE INVENTION

Figures 3 (a) to 3 (c) show different views of a camera 40 suitable for carrying out spatial monitoring or tracking of an object according to an embodiment of the invention. The camera 40 integrates the two technologies that are required to improve the precision in measurements necessary for the spatial tracking of an object: a camera as such, for photogrammetry measurements, and an autocollimator. In Figures 3 (a) and 3 (b), part of the outer casing has been removed to show the inner part of the camera. Figure 3 (c) shows an exploded view of the chamber shown in Figures 3 (a) and 3 (b). Some of the most relevant parts of the camera 40 are identified in the figures: a lens 41 to focus and create the image of the object on the camera sensor and to collimate the light in autocollimation measurements; and a sensor 42 for capturing images. Non-limiting examples of the sensor 42 can be a matrix CCD or a CMOS sensor. The image projected by the objective lens is formed on the sensor. These elements are generally present in all cameras. Camera 40 also includes a light source 43 necessary to implement autocollimation functionality. Light source 43 can be a light emitting diode (LED). Alternatively, this can be a laser emitter. This can increase the power and therefore the operating range, or it can make it possible to work in lighting environments. Camera 40 also includes a beam splitter 44, also necessary to implement autocollimation functionality. Beam splitter 44 can be implemented, for example, as a plate or a cube.

Chamber 40 also has structural elements 45-50 to enable mounting, docking, and / or support of optical and / or electrical components. The body 45 is the main structural element of the chamber 40, to which the other elements of the chamber are attached. The beam splitter is supported by element 46, which ensures contact between the beam splitter and the camera sensor on one side, and between the beam splitter and the light source on the other side. The camera 40 also comprises special spacer elements 47, which are used to set the focal length depending on the camera lens. The camera sensor is fixed by means of the structural element 48. Chamber 40 also comprises a light source support 49. A cover 50 encapsulates and protects all internal elements. Chamber 40 also has fixing elements 51-54 for fixing and coupling the different (structural) mounting, optical and / or electrical components.

In order to spatially track an object, an additional assembly, artifact, or device is required. A further artifact in accordance with a possible embodiment is shown in Figure 4. The measurement artifact 60 shown in Figure 4 is composed of a mirror surface 55 and a set or grid of reference points 56 that is attached or attached to the artifact. In Figure 4, the specular surface 55 is implemented as a flat mirror. Alternative specular surfaces can be any flat or semi-flat surface capable of producing a specular reflection of light, such as a part that has a good surface finish. Flat mirror 55 is used for self-collimation measurements. It is mounted on a frame 57. The set of reference points 56 are targets that are required for the measurements that are carried out by the camera 40 using the spatial resection technique. These targets can take different shapes or be of different types, such as following a checkerboard, circular, square, cross-shaped pattern or following special markers, among others. The set of reference points 56 requires at least 3 points or targets. It should be noted that, in general, the more reference points there are, the less uncertainty there will be in the measurements. In Figure 4, the targets are set up in the form of a checkerboard. The reference points correspond to the sides of each square on the chessboard. In other words, the artifact 60 shown in Figure 4 is comprised of a flat mirror 55 and a calibrated chessboard 56. The chessboard is widely used for internal camera calibration. The coordinates of the reference points 56 are known by a coordinate system defined in the artifact 60 (the mirror 55 and the set of reference points 56).

In the artifact 60, the geometric relationship between the plane of the plane mirror 55 and the coordinate system formed by the set of reference points 56 has to be established. In other words, a phase of characterization of the artifact 60 (mirror 55 and set of reference points 56), preferably offline or offline, before starting the phase of measuring the position and orientation of an object. The results of this characterization are used later during the measurements. This means that a common reference system has to be defined for the plane mirror 55 and the set of reference points 56. In other words, the normal vector of the mirror 55 and the Z axis of the coordinate system that is created at starting from the set of reference points 56 (eg, a chessboard) they may be aligned, but they may not coincide. Therefore, it is necessary that both the normal vector of the mirror 55 and the Z axis of the coordinate system that is created from the set of reference points 56 be measured or characterized, in order to compensate for any potential misalignment that they may have the same. In order to establish the aforementioned geometric relationship between these two vectors, the artifact 60 shown in figure 4 (the mirror 55 with the set of reference points 56) is measured, for example, in a Measuring Machine of Coordinates (Coordinate Measuring Machine, CMM). The artifact 60 further includes a mounting means, such as a plurality of housings, for mounting a plurality of measurement means to measure the spatial location and orientation of the artifact 60. For example, the measurement means may be retroreflective, with the that the spatial location and orientation of the artifact can be measured by conventional laser tracker technology, which is outside the scope of the present invention.

Next, the process of spatial tracking of an object using the camera 40 and the artifact 60 is explained. In order to perform the spatial tracking of an object, an artifact 60 comprising a plane mirror and a set of reference points, it is attached to the object whose position and orientation (T _x , T _y , T _z , F, q, ψ), with respect to the camera, will be estimated. The camera 40 is positioned facing the artifact 60 that is attached to the object, for example, as shown in Figures 5, 7A and 7B, in such a way that the images of the set of reference points that are comprised in the artifact 60 can be captured by camera 40.

First, the pitch or bank angles θ and yaw ψ are measured, which represent the tilt of the plane mirror 55. This is done by applying the autocorrelation functionality available in the camera 40 and based on the mirror of type specular 55 (the plane mirror 55). The set of reference points 56 is not used in this phase. Figure 5 shows a scheme to carry out autocollimation measurements to obtain pitch or bank angles θ and deviation or yaw ψ with camera 40 and a flat mirror. 55. Figure 6 shows the components of an optical device, such as a camera, relevant for performing autocollimation measurements, plus the required flat mirror. In figure 6, it is shown in detail how the pitch or bank angle θ is obtained. In Figure 6, in a camera, a light source 43, such as a diode

LED emits a non-collimated light beam 62 which is redirected by a beam splitter 44, also producing non-collimated beams 63. These beams 63 are collimated by a collimation lens 41. The focal length of the camera is represented by the letter f (the distance between the plane of the collimating lens and the plane of the sensor or image plane). The collimated beams 65 are reflected by the mirror 55, thereby providing reflected collimated light beams 66. Some of these again reach the collimating lens 41, which provides a focused reflected beam of light 67 to the camera sensor. 42. This configuration allows the angle of inclination of the mirror 55 (the inclination or pitch θ or the deflection or yaw ψ) and, therefore, of the object that is attached to the artifact 60 to be determined, but not the angle of rotation or roll F. In fact, a plane displacement δ of the reflected light beam 67 at a point 75 on the camera sensor 42 is studied on the image plane 61 (the camera sensor plane) in order to estimate the Angular variation (the pitch or bank angle θ) based on the following trigonometric relationship: θ = δx '/ 2f. Plane offset d represents the offset between a camera sensor reference point 76 and the position where the focused reflected beam of light 75 is captured. Point 75 is the point on the image plane 61 at which the focuses the reflected light beam 67. θ indicates the tilt or pitch angle of the mirror. I know takes a similar approach to obtain the yaw angle ψ.

Referring now to Figure 5, a non-collimated beam of light is emitted by the light source 43 arranged in the chamber 40 (not shown in Figure 5, see, for example, Figure 6). The emitted light beam is redirected by beam splitter 44, also producing non-collimated beams (not shown). These beams are collimated by the camera lens 41. The collimated beams 65 are directed towards the plane mirror 55. The collimated beams 65 are reflected by the plane mirror 55, thereby providing reflected collimated light beams 66. The collimated beams The reflected beams 66 reach the camera lens 41, which provides focused reflected light beams 67 to the camera sensor 42. The point (dx ', δy') represents the plane shift between the position (at the camera sensor 42) in which the focused reflected light beam 67, that is, point 75, and a fixed reference point 76 in the plane 61 of the camera sensor 42 is captured. This reference position represents the position where the angles of pitch or bank θ and yaw or yaw ψ are equal to zero, usually the position x '= 0 and y' = 0 in the plane of the camera sensor. Because the point (dx ', δy') is well known, and the focal length f of the camera 40 is also well known, the pitch or bank angles θ and deviation or yaw ψ are obtained by applying the following trigonometric relation: θ = δx '/ 2f ψ = δx' / 2f

Once the pitch or bank angles θ and yaw or yaw ψ have been calculated, image points 72 corresponding to Reference points 71 in the object's coordinate system (not shown, but attached to measurement artifact 60), are measured and identified using the camera. This is done by applying camera vision techniques. Figure 7A shows a scheme for carrying out photogrammetry measurements with camera 40 and set of reference points 56 that is comprised in artifact 60 and that is attached to the object - not shown - to be performed. a trace (in Figure 7A, reference points are referred to as 71). Therefore, these points 71 are reference points in the 3D coordinate system of the object whose position and orientation is being tracked. The coordinates of the reference points 71 are known by a coordinate system defined in the artifact 60. In Figure 7A, five reference points 71 are illustrated, each expressed as (X _i , Y _i , Z _i ), where 1 <i <N, where N is a natural number greater than 2 (in Figure 7A, N = 5).

The camera 40 captures an image of the object - in general, the artifact - which has attached the set of reference points 71 that are located at positions (X _i , Y _i , Z _i ). In plane 61 of image sensor 42, image points 72 are obtained which correspond to respective reference points 71. Image points 72, hereinafter referred to as reference points The observed image defines the projection of the 3D reference points 71 on the plane 61 of the sensor. Because the camera sensor 52 defines a 2D plane, the Z component is set to 0. The observed image points are expressed as (x ' _i , y' _i , 0). In Figure 7A, five image points are shown observed (x ' ₁ , y' ₁ , 0), (x ' ₂ , y' ₂ , 0), (x ' ₃ , y' ₃ , 0), (x ' ₄ , y' ₄ , 0), ( x ' ₅ , y' ₅ , 0), which correspond to respective reference points (X ₁ , Y ₁ , Z ₁ ), (X ₂ , Y ₂ , Z ₂ ), (X ₃ , Y ₃ , Z ₃ ), (X ₄ , Y ₄ , Z ₄ ), (X ₅ , Y ₅ , Z ₅ ). Because the coordinates of the image points are in 2D, they are also referred to as (x ' ₁ , y' ₁ ), (x ' ₂ , y' ₂ ), (x ' ₃ , y' ₃ ), (x ' ₄ , y' ₄ ), (x ' ₅ , y' ₅ ). The position of the image points observed in the sensor plane is known. For example, in order to obtain said position, computational image processing, such as operators and / or filters, can be applied to identify the center of the features of interest (such as intersecting edge points, centers of ellipses, etc.). For example, a surface contour detection function can be applied in order to obtain the position of the image points observed in the sensor plane.

Next, in order to obtain the 6 parameters - which are represented in figure 7B - that define the position and orientation of the object with 6 DOF, a spatial resection algorithm is applied, restricted by the already known orientation angles ( pitch or bank angles θ and yaw or yaw ψ). Therefore, these two angles are entered as known parameters in the spatial resection algorithm, which is thus simplified. In summary, by applying a spatial resection algorithm restricted by the pitch or bank angles θ and deviation or yaw ψ, the angle of rotation F of the artifact 60 is obtained, which is attached to the object to be measured ( not shown) and a translation vector (T _x T _and T _z ) between camera 40 and the object. This translation vector corresponds to the position of the object. The restricted spatial resection algorithm is preferably applied such as follows:

The following optimization problem is implemented, in which it is necessary to minimize the difference between the observed image points (x ' _i , y' _i , 0), which are also referred to as (x ' _i , y ' _i ,) and the estimated image points (X' _Ei , y ' _Ei , 0), also referred to as (X'E,

Y'EÍ):

where N is the number of the observed image points 72 (and hence the number of reference points 71). The difference between the observed image points and the estimated image points is repeatedly minimized until a certain precision threshold is obtained.

The estimated image points (X ' _Ei , y' _Ei , 0) are obtained by applying the pinhole camera model that represents the projection of a 3D point (X _i , Y _i , Z _i ) on an image plane (a camera sensor) (X ' _Ei , y' _Ei ):

where:

Y:

Simplifying:

Therefore, there are two ways to calculate the estimated image points (X ' _Ei , and' _Ei, 0) which are also referred to as (X ' _Ei , and' _Ei ):

1. Apply the projection equation in computer vision, where the estimated image points (X ' _Ei , and' _Ei , 0) are calculated as follows:

By replacing the estimated image points in equation 1 with those obtained in equation 3, the minimization problem results in the following:

2. Apply the collinearity equations, where the estimated image points (X ' _Ei , and' _Ei ) are calculated as follows:

By replacing the estimated image points in equation 1 with those obtained in equation 5, the minimization problem results in the following:

In the above equations, they are well known input parameters: for each i, the observed image points 72, at the sensor of the

camera; the position of the reference points in the object's coordinate system a scale factor λ, which represents a conversion factor

between reference points on the object and image points observed on the camera sensor; and the intrinsic camera matrix [A], which is a matrix that contains the different internal characteristics of the camera (the focal length (f), the main central point (cx, cy), the obliquity factor, among others). The scale factor l is not required if a normalization process is previously applied. The intrinsic matrix of the chamber [A] can be obtained, for example, by applying a calibration process.

The disclosed algorithm can be implemented either taking into account the distortion of the image produced by the camera lens or without considering this distortion. This distortion can be accounted for using well known radial and tangential distortion errors in photogrammetric applications.

In the above equation, the input restriction parameters that are obtained after applying the autocollimation technique are: the pitch or pitch rotation matrix R (ángulo) obtained from pitch or pitch angle θ; and the deviation or yaw rotation matrix R (ψ) obtained from the deflection or yaw angle ψ. And the output parameters, estimated by applying the spatial resection algorithm, are: the rotation rotation matrix R (Φ) (in general, the angle of rotation or balance F) and the translation vector (Tx Tg TZ) between the camera coordinate system and the object coordinate system, which corresponds to the position of the object. In other words, T _x T _and T _z , Φ, θ, ψ represent the relative position and orientation between the camera 40 and the object.

Figure 8 shows an alternative scheme to implement the method to estimate the position and orientation of an object with 6 degrees of freedom. In this case, the spatial tracking of the object is performed by means of two separate devices: a camera and a 2D autocollimator, plus the artifact of Figure 4. In other words, the camera 40 of Figures 3 (a - c ) is replaced by the chamber 140 and the 2D autocollimator 150 of FIG. 8. In FIG. 8, the chamber 140 and the autocollimator 150 are disposed on a support 160. depicts an extrinsic calibration transformation between the autocollimator and the chamber. These are oriented towards the artifact 60 that is attached to the object (not shown), in such a way that the images of the set of reference points that are comprised in the artifact can be captured by the camera 140. Apart from this difference Related to the components (camera and autocollimator in separate devices), the measurement procedure is the same as in the previous case. In other words, firstly, the pitch or bank angles θ and deviation or yaw ψ, which represent the inclination of the plane mirror (generally a specular surface) that is attached to the artifact and, therefore, the inclination of the object being tracked, are measured by carrying out autocollimation measurements with autocollimator 150. Then the observed image points (x ' ₁ , y' ₁ ), (x ' ₂ , y' ₂ ), (x ' ₃ , y' ₃ ), (x ' ₄ , y ' ₄ ), (x' ₅ , y ' ₅ ) that correspond to respective (at least 3) reference points (Xi, Y ₁ , Z ₁ ), (X ₂ , Y ₂ , Z ₂ ), (X ₃ , Y ₃ , Z ₃ ), (X ₄ , Y ₄ , Z ₄ ), (X ₅ , Y ₅ , Z ₅ ) in the object's coordinate system (not shown, but attached to the measurement artifact 60), are obtained by the camera 140, which captures an image of the object - in general, the artifact - that has joined the set of reference points that are located in some positions (X _i , Y _i , Z _i ). In the plane of the camera image sensor, the observed image points are obtained (x ' ₁ , y' ₁ ), (x ' ₂ , y' ₂ ), (x ' ₃ , y' ₃ ), (x ' ₄ , y ' ₄ ), (x' ₅ , y ' ₅ ). Next, in order to obtain the 6 parameters (Tc, Tg, Tz, F, q, ψ) that define the position and orientation of the object with respect to the camera, with 6 DOF, the spatial resection algorithm is applied , constrained by already known yaw angles (pitch or bank angles θ and yaw or yaw ψ), as previously disclosed.

The disclosed spatial resection algorithm can be implemented and executed in a processing means, such as a processor, and a data storage means, such as a memory. The processing means can be incorporated in the chamber 40, 140, for example in the processing means, generally comprised in or associated with a chamber sensor. Alternatively, the processing means may be located in a different device with respect to the camera 40, 140, for example in a computer system or a computing device, such as a personal computer. In this case, the algorithm can be run offline. In this text, the term "comprises" and its derivations (such as "comprising / comprising", etc.) should not be understood in an exclusive sense, that is, these terms should not be construed as excluding the possibility that what is described and defined may include additional elements, steps, etc.

The invention is obviously not limited to the specific embodiment or embodiments described herein, but also encompasses any variation that may be considered by any person skilled in the art (for example, as regards the choice of materials, dimensions, components, configuration, etc.), within the general scope of the invention as defined in the claims.

Claims

1. A method to estimate the position and orientation of an object with 6 degrees of freedom (T _x , T _y , T _z , Φ, θ, ψ), comprising: attaching an artifact (60) to the object from which it is is going to estimate the position and orientation (T _x , T _y , T _z , Φ, θ, ψ), where said artifact (60) comprises a specular surface (55) and a set of N reference points (71) , where each reference point is defined by a position (X _i , Y _i , Z _i ) in a coordinate system defined in the artifact (60), where 1 <i <N and N>2; positioning a camera (40) facing the artifact (60) that is attached to the object; measuring the pitch or inclination angles θ and deviation or yaw ψ that represent the inclination of the specular surface (55) and, therefore, of the object, said measurement being carried out by applying an autocollimation technique; capture an image of the set of reference points (71; (X _i , Y _i , Z _i )) comprised in the artifact (60) that is attached to the object, thus obtaining in the plane (61) of the sensor (42 ) of the camera some observed image points (72; (x ' _i , y' _i )) that correspond to said reference points (71; (X _i , Y _i , Z _i )); obtain the angle of rotation or roll F of the object and a translation vector (Tc, Tg, T _Z ) between the camera (40) and the object, using said reference points (71; (X _i , Y _i , Z _i )) and said observed image points (72; (x ' _i , y' _i )) obtained in the sensor (42) of the camera, by applying an algorithm of spatial resection constrained by the measured pitch or bank angles Q and yaw or yaw ψ, where (T _x , T _y , T _z , Φ, θ, ψ) represent the relative position and orientation between the camera (40) and the object.

The method of claim 1, wherein the pitch or bank angle θ and yaw or bank angle ψ representing the inclination of the specular surface (55) and hence of the object, are measured as follows applying an autocollimation technique: emitting a non-collimated light beam (62) from a light source (43) that is arranged in the chamber (40); redirecting the emitted light beam (62) by means of a beam splitter (44), producing non-collimated light beams (63); collimating said light beams (63) by means of a camera lens (41), thereby producing collimated light beams (65); directing said collimated beams (65) towards the plane mirror (55), in which the collimated beams are reflected, thereby providing reflected collimated light beams (66); focusing said reflected collimated beams (66) onto the camera lens (41), thereby providing focused reflected beams of light (67) to the camera sensor (42), said reflected beams of light being focused (67 ) at a position (75) in the plane (61) of the camera sensor (42); calculate pitch or bank angles θ and yaw or yaw ψ as follows: θ = δx '/ 2f ψ = δx '/ 2f where f is the focal length of the camera (40) and δx', δy 'represent a plane displacement between the measured position (75) and a fixed reference position on the plane (61) of the sensor of camera (42), representing this fixed reference the position (x ', y') in the plane of the sensor (61) where the angles of pitch or inclination θ and of deviation or turn ψ are equal to 0.

The method of any one of claims 1-2, wherein said angle of rotation or roll F and said translation vector (X, Y, Z) between the camera (40) and the object, are obtained by means of implementation of a spatial resection technique, restricted by the pitch or bank angles θ and deviation or yaw ψ already obtained, iteratively solving the following optimization problem:

where N is the number of image points observed; (x'i, y'¡) are the observed image points; and (X ' _Ei , y' _Ei ) are the estimated image points.

The method of claim 3, wherein the estimated image points (X ' _Ei , and' _Ei )) are obtained by applying a pinhole camera model representing the projection (X ' _Ei , and' _Ei ) of each 3D reference point (X _i , Y _i , Z _i ) on the image plane (61) of the camera sensor (42):

where:

which can be simplified as: and where: is the intrinsic matrix of the camera

and where l is a scale factor.

The method of claim 4, wherein the estimated image points ((X ' _Ei , and' _Ei , 0) are calculated as follows by applying the central projection equation in computer vision:

The method of claim 4, wherein the estimated image points (X ' _Ei , and' _Ei , 0) are calculated as follows by applying collinearity equations:

The method of any one of claims 1-6, wherein prior to applying an autocollimation technique to measure pitch or bank angles θ and yaw or yaw ψ and to capture with the camera an image of the artifact that is attached to the object (40), a geometric relationship is established between the plane of the specular surface (55) and a coordinate system formed by the set of reference points (71).

The method of claim 7, wherein the artifact (60) has been measured in a Coordinate Measuring Machine (CMM) in order to establish said geometric relationship.

9. A system for estimating the position and orientation of an object with 6 degrees of freedom (Tc, Tg, Tz, F, q, ψ), comprising: means for taking photogrammetry measurements; means for carrying out autocollimation; and an artifact (60) that is attached to the object whose position and orientation are to be estimated (T _x , T _y , T _z , Φ, θ, ψ), wherein said artifact (60) comprises a specular surface (55) and a set of N reference points (71), where each reference point is defined by a position (X _i , Y _i , Z _i ) in a coordinate system defined in the artifact (60), in where 1 <i <N and N>2; in which the means for carrying out autocollimation are configured to measure the pitch or bank angles de and deviation or yaw ψ that represent the inclination of the specular surface (55) and, therefore, of the object; where the means for taking photogrammetry measurements are configured to: capture an image of the set of reference points (71; (X _i , Y _i , Z _i )) comprised in the artifact (60) attached to the object, obtaining from this mode observed image points (72; (x'i, y'i,)) that correspond to said reference points (71; (X _i , Y _i , Z _i )); and obtain the angle of rotation or roll F of the object and a translation vector (T _x T _and T _z ) between the means for taking photogrammetry measurements and the object, using said reference points (71; (X _i , Y _i , Z _i )) and said observed image points (72; (x ' _i , y' _i )), by applying a spatial resection algorithm restricted by the measured pitch or bank angles θ and deviation or yaw ψ .

The system of claim 9, wherein said means for taking photogrammetric measurements and said means for performing autocollimation are comprised of a chamber (40).

The system of claim 9, wherein said means for taking photogrammetric measurements are comprised of a camera (140) and said means for performing autocollimation are comprised of an autocollimator (150).

The system of any one of claims 9-11, wherein the N reference points represent a checkerboard pattern, or a circular pattern, or a square pattern, or a cross-shaped pattern or follow special markers.

The system of any one of claims 9-12, wherein the specular surface (55) is a flat mirror or any flat or semi-flat surface capable of producing a specular reflection of light.

The system of any one of claims 9-13, further comprising mounting means for mounting a plurality of measurement means for measuring the spatial location and orientation of the mirror (55).

A computer program product comprising computer program instructions / code for carrying out the method according to any one of claims 1-8, or a computer-readable memory / medium that stores program instructions / code to carry carrying out the method according to any one of claims 1-8.