JP4566786B2 - Position and orientation measurement method and information processing apparatus - Google Patents

Description

  The present invention relates to an object that measures the position and orientation of an object, particularly the position and orientation of an imaging apparatus.

  In recent years, research on mixed reality has been actively conducted for the purpose of seamless connection between the real space and the virtual space. An image display device that presents a mixed reality is drawn on a virtual space (for example, computer graphics) generated in accordance with the position and orientation of an imaging device on an image of the real space taken by an imaging device such as a video camera. This is realized by a video see-through method in which an image of a virtual object, character information, etc.) is superimposed and drawn and displayed.

  In addition, it is realized by an optical see-through method in which an image in a virtual space generated according to the position and orientation of the observer's viewpoint is displayed on an optical see-through display mounted on the observer's head.

  Applications of such image display devices include virtual reality, such as surgical support that superimposes the state of the body on the patient's body surface, and mixed reality games that fight against virtual enemies floating in real space. Different new fields are expected.

  A common requirement for these applications is how to accurately align the real space and the virtual space, and many attempts have been made so far. The problem of registration in mixed reality results in the problem of obtaining the position and orientation of the imaging device in the scene (that is, in the world coordinate system) in the case of the video see-through method. Similarly, in the case of the optical see-through method, it results in the problem of obtaining the observer's viewpoint or display position and orientation in the scene.

  As a method for solving the former problem, by arranging or setting a plurality of indices in the scene, and detecting the coordinates of the projected image of the indices in the image captured by the imaging apparatus, the position of the imaging apparatus in the scene and It is common practice to determine the posture. In addition, an inertial sensor is attached to the imaging device, and the position and orientation of the imaging device estimated based on sensor measurement values are used for index detection processing, or the estimation results are used as initial values for position and orientation calculation based on images. In addition, even in the situation where the index is not visible, an attempt has been made to realize stable positioning compared to the case of using only image information by using the estimation result as a rough position and orientation (for example, Non-Patent Document 1).

As a method for solving the latter problem, an imaging device (and an inertial sensor) is attached to a measurement object (that is, an observer's head or display), and the position and orientation of the imaging device are obtained by the same method as the former. In general, the position and orientation of the measurement target are obtained from the relationship between the known relative position and orientation of the imaging device and the measurement target object.
Hirofumi Fujii, Masayuki Kanbara, Hidehiko Iwasa, Haruo Takemura, Naokazu Yokoya, alignment with stereo camera combined with gyro sensor for augmented reality, IEICE Technical Report PRMU99-192 (Science Technical Report vol.99) , No. 574, pp. 1-8)

  However, in the above-described conventional method, in the situation where the image information sufficient to realize stable alignment is not included in the subjective viewpoint image, for example, the observation is performed with the index unevenly distributed in some areas on the image. The accuracy and stability of the obtained solution may be insufficient when only three points are observed, or when an error is included in the index detection. In addition, when the observed index was 2 or less, the solution could not be obtained. In order to avoid these problems, it is necessary to uniformly set a large number of indices in the scene, and there are problems that it is difficult to distinguish the indices from each other and that the aesthetics of the real space are impaired. In addition, there is a problem that alignment cannot be performed at all in a situation where the index image on the subjective viewpoint image is blocked by the hand of the observer.

The above problem is solved by the following apparatus.

Subjective viewpoint imaging means for acquiring a subjective viewpoint image obtained by imaging a subjective viewpoint point index arranged in the real space, and an objective viewpoint point index arranged in association with the subjective viewpoint imaging means in the world coordinate system of the real space An objective viewpoint imaging unit that acquires an objective viewpoint image captured from a fixed and known objective viewpoint position and orientation; an attitude measurement unit that is provided in the subjective viewpoint imaging unit and that measures the orientation measurement value of the subjective viewpoint imaging unit; Azimuth drift error correction value of the posture measuring means, position of the subjective viewpoint point index in the world coordinate system, position of the objective viewpoint point index with respect to the subjective viewpoint imaging means, focal length of the subjective viewpoint imaging means and objective holding means for previously holding a focal length of-view imaging means, on the basis of the orientation measurement value and the azimuth drift error correction value, of the subjective-view imaging means And orientation prediction means for calculating the energizing predictive value, first detecting means for detecting the image coordinates of the subjective-view-point indicator for at least one point from the subjective-view image, the objective viewpoint point index of at least one point from the objective viewpoint image A second detection means for detecting the image coordinates of the objective viewpoint position and orientation, the predicted posture value, the image coordinates of the detected subjective viewpoint point index, the position of the subjective viewpoint point index in the world coordinate system, and the detection the image coordinates of the objective viewpoint point indices which are, and on the basis of the position relative to the subjective-view imaging means of the objective viewpoint-point indicator, the position and orientation calculation means for calculating the position and orientation of the subjective-view imaging means, the calculated the based on the position and orientation of the subjective-view imaging apparatus, a heading error updating means for updating the azimuth drift error correction value the stored, said position and orientation calculation unit, the subjective vision A setting value setting unit configured to set a setting value of a position and orientation of the imaging unit; the subjective viewpoint image based on the setting value, a position of the subjective viewpoint point index in the world coordinate system, and a focal length of the subjective viewpoint imaging unit; Subjective estimate value calculating means for calculating a subjective coordinate estimated value of image coordinates of the subjective viewpoint point index, the set value, a position of the objective viewpoint point index with respect to the subjective viewpoint imaging means, the objective viewpoint position and orientation, and Objective coordinate value calculating means for calculating an objective coordinate estimated value of the image coordinate of the objective viewpoint point index of the objective viewpoint image based on the focal length of the objective viewpoint imaging means, and image coordinates of the detected subjective viewpoint point index And a subjective image error calculating means for calculating a subjective error between the subjective coordinate estimated value and an objective error between the image coordinate of the detected objective viewpoint point index and the objective coordinate estimated value. Objective image error calculation means, correction value calculation means for calculating only the azimuth component of the position and orientation of the subjective viewpoint imaging means based on the calculated subjective error and objective error, and the calculated An information processing apparatus comprising: a correction unit that corrects a position and orientation of the subjective viewpoint imaging unit based on a correction value and the set value .

  According to the present invention, it is possible to measure the position and orientation of the imaging apparatus even when the number of indices observed on the image captured by the imaging apparatus is one point. Even when three or more indices are obtained, since the number of parameters to be obtained is small, the redundancy of information is relatively increased, and a stable solution can be obtained as compared with the conventional method. This effect is exhibited particularly when the number of indices is three. Therefore, the possibility of falling into a situation where a solution cannot be obtained is reduced, and further, the number of indexes required to obtain a solution (having the same stability) is reduced. As a result, there is an advantage that the number of indexes set in the scene can be reduced, which leads to a decrease in the false recognition rate and contributes to a reduction in the problem of deteriorating the aesthetic appearance.

  Hereinafter, the present invention will be described in detail according to preferred embodiments with reference to the accompanying drawings.

[First Embodiment]
The position / orientation measurement apparatus according to the present embodiment measures the position and orientation of the imaging apparatus. Hereinafter, the position and orientation measurement apparatus and the position and orientation measurement method according to the present embodiment will be described.

  FIG. 1 shows a configuration of a position / orientation measurement apparatus according to the present embodiment. As shown in the figure, the position / orientation measurement apparatus 100 according to this embodiment includes an objective viewpoint camera 180 (180a, 180b, 180c, 180d), an image input unit 160, a data storage unit 170, an index detection unit 110, and an attitude sensor. 140, a posture prediction unit 150, and a position and posture calculation unit 120, and is connected to the imaging device 130.

At a plurality of positions in the real space, the world coordinate system (one point in the real space is defined as the origin, and three axes that are orthogonal to each other are used as indices for photographing with the imaging device 130 (hereinafter referred to as subjective viewpoint indices). A plurality of subjective viewpoint indices Q _k (k = 1,..., K _Q ) having known positions x _W ^Qk in the coordinate system defined as the X axis, Y axis, and Z axis, respectively, are arranged. In addition, a plurality of positions on the posture sensor 140 are defined by using a subjective viewpoint camera coordinate system (one point on the imaging device 130 as an origin) as an index for photographing with the objective viewpoint camera 180 (hereinafter referred to as an objective viewpoint index). Further, an objective viewpoint index P _k (k = 1,, K _P ) in which the position x _C ^Pk on the three orthogonal axes is defined as the X axis, the Y axis, and the Z axis) is set. Has been.

These indices are the subjective viewpoint indices observed on the subjective viewpoint image acquired by the imaging device 130 when the imaging device 130 is positioned at each point in the measurement target range where the position and orientation are to be measured. The total number of objective viewpoint indices (substances) observed on the objective viewpoint image acquired by the objective viewpoint camera 180 is preferably set to always be at least two. The example of FIG. 1, three subjective-view-indices _{Q 1,} Q 2, _{Q 3} and two objective viewpoint index _P 1, _{P 2} is set, two subjective-view-indices _Q 1, Q of which ₃ is included within the field of view of the imaging device 130, a situation where the objective-view-index _{P 1} is within the field of view of the objective viewpoint camera 180c, an objective-view-index _{P 2} is included in the field of view of the objective viewpoint camera 180c and 180d Is shown.

The subjective viewpoint index Q ^k and the objective viewpoint index P ^k may be configured by circular markers having different colors, for example, or may be configured by feature points such as natural features each having a different texture characteristic. May be. Any form may be used as long as the image coordinates of the projected image on the photographed image can be detected and the index can be identified by any method. In addition, both the subjective viewpoint index and the objective viewpoint index may be set intentionally, or may be a natural shape that is not set intentionally.

  An image output from the imaging device 130 (hereinafter referred to as a subjective viewpoint image) is input to the position / orientation measurement apparatus 100.

  The objective viewpoint camera 180 (180a, 180b, 180c, 180d) is arranged so that one of the imaging devices 130 is fixed at a position where the imaging device 130 can image when the imaging device 130 is located within the measurement target range. Here, it is assumed that the position and orientation of each objective viewpoint camera 180 in the world coordinate system are stored in advance in the data storage unit 170 as known values. An image output from the objective viewpoint camera 180 (hereinafter referred to as an objective viewpoint image) is input to the image input unit 160.

  The image input unit 160 converts each of the subjective viewpoint image and the objective viewpoint image input to the position / orientation apparatus 100 into digital data and stores the digital data in the data storage unit 170.

  The posture sensor 140 is attached to the imaging device 130, measures the current posture of the posture sensor 140 itself, and outputs it to the posture prediction unit 150. The attitude sensor 140 is a sensor unit based on a gyro sensor, and is configured by, for example, TISS-5-40 of Tokimec Co., Ltd., InertiaCube 2 of InterSense, USA, and the like. The posture measurement values measured by these sensors are postures having errors that are different from the true posture. However, these attitude sensors have a tilt angle sensor for observing the direction of gravity of the earth as a constituent element and have a function of canceling accumulation of drift errors in the tilt angle direction. The pitch angle and roll angle) have the property of not generating a drift error. In other words, the azimuth angle direction (yaw angle direction) has a drift error that accumulates with time.

  The posture prediction unit 150 inputs the azimuth drift error correction value φ from the data storage unit 170, corrects the posture measurement value input from the posture sensor 140, predicts the posture of the imaging device 130, and outputs it to the data storage unit 170. To do.

  The index detection unit 110 inputs the subjective viewpoint image and the objective viewpoint image from the data storage unit 170, and detects the image coordinates of the index photographed in the input image. For example, in the case where each index is composed of markers having different colors, an area corresponding to each marker color is detected from the image, and the position of the center of gravity is set as the detected coordinate of the index. In addition, when each of the indexes is composed of feature points having different texture features, template matching is performed on the image by using a template image of each index stored in advance as known information. Detect position. It should be noted that a calculated value of the position of the imaging device 130 (output of the position / orientation calculation unit 120) and a predicted value of the attitude (output of the posture prediction unit 150) are further input from the data storage unit 170, and on the image based on the predicted value By predicting the position of the index and limiting the search range, it is possible to reduce the calculation load of the index detection process and to reduce erroneous detection and identification of the index.

The index detection unit 110 further outputs the image coordinates of the detected index and the identifier of the index to the data storage unit 170. In the following, an index detected on the subjective viewpoint image is denoted as Q _kn using identifiers n (n = 1,..., N) attached to the detected indices. In addition, the index detected on the objective viewpoint image is represented by the identifier x (x = a, b, c, d) of the camera and the identifier m (m = 1,, M) attached to each of the detected indices. _x )) and expressed as P _kxm . Here, N represents the number of indices detected on the subjective viewpoint image, and M _x represents the number of indices detected on each objective viewpoint image. M is the sum of the indices detected on each image. Further, the image coordinates of the detected index Q _kn are denoted as u ^Qkn, and the image coordinates of the detected index P _kxm are respectively represented by u _a ^Pkam and u _b according to the identifier of the objective viewpoint camera that captured the image. _{^Pkbm, u} ^c Pkcm, denoted as ^{u d pKDM.} For example, in the case of FIG. 1, N = 2, M _a = 0, M _b = 0, M _c = 2, M _d = 1, M = 3, and index identifiers k ₁ = 1, k ₂ = 3. , and _{k c1 = 1, k c2 =} 2, k d1 = 2, and camera identifier taken them, the image coordinates ^{_{u Qk1, u Qk2, u c}} pkc1, u c pkc2, u d Pkd1 corresponding to these Is output.

The position / orientation calculation unit 120 includes a predicted value of the attitude of the imaging device 130, a set of image coordinates u ^Qkn and world coordinates x _W ^Qkn of each subjective viewpoint index detected by the index detection unit 110, and each objective viewpoint index. Image coordinates u ^Pkam , u ^Pkbm , u ^Pkcm , u ^Pkdm and the corresponding subjective viewpoint camera coordinates (coordinate values in the subjective viewpoint camera coordinate system) x _C ^Pcam , x _C ^Pkbm , x _C ^Pkcm , x _C ^Pkdm Is input from the data storage unit 170, the position and orientation of the imaging device 130 are calculated based on these pieces of information, and output to the outside via the I / F. Further, the calculated position of the imaging device 130 is output to the data storage unit 170, and the data storage unit 170 holds the updated value of the orientation drift error correction value of the orientation sensor 140 derived in the position and orientation calculation process. Update the azimuth drift error correction value.

  The data storage unit 170 includes an azimuth drift error correction value, an image input from the image input unit 160, a predicted value of posture input from the posture prediction unit 150, a calculated value of position input from the position / posture calculation unit 120, and an index The image coordinates and identifier of each index input from the detection unit 110, the world coordinates of the subjective viewpoint index, which are known values, the object coordinates of the objective viewpoint index (coordinate values in the object coordinate system), and the objective viewpoint camera 180 It holds data such as camera parameters and inputs / outputs them as necessary.

  Note that each of the image input unit 160, the data storage unit 170, the index detection unit 110, the posture prediction unit 150, and the position / orientation calculation unit 120 illustrated in FIG. 1 may be handled as an independent device or as software. The functions may be realized by installing in one or a plurality of computers and executing them by the CPU of each computer. In this embodiment, each unit (the image input unit 160, the data storage unit 170, the index detection unit 110, the posture prediction unit 150, and the position / posture calculation unit 120) is handled as software to be executed in one computer.

  FIG. 2 is a diagram illustrating a basic configuration of a computer that executes each of the image input unit 160, the data storage unit 170, the index detection unit 110, the posture prediction unit 150, and the position / posture calculation unit 120 as software.

  A CPU 1001 controls the entire computer using programs and data stored in the RAM 1002 and the ROM 1003, and each of the image input unit 160, the index detection unit 110, the posture prediction unit 150, and the position / orientation calculation unit 120. The execution of software is controlled to realize the functions of each unit.

  A RAM 1002 includes an area for temporarily storing programs and data loaded from the external storage device 1007 and the storage medium drive 1008, and also includes a work area necessary for the CPU 1001 to perform various processes. The function of the data storage unit 170 is realized by the RAM 1002.

  A ROM 1003 generally stores a computer storage program, setting data, and the like. Reference numerals 1004 and 1005 denote a keyboard and a mouse, respectively. An operator can input various instructions to the CPU 1001 using the keyboard and the mouse, respectively.

  A display unit 1006 includes a CRT, a liquid crystal screen, and the like, and can display a message to be displayed for measuring the position and orientation of the imaging device 130, for example.

  Reference numeral 1007 denotes an external storage device that functions as a large-capacity information storage device such as a hard disk, and stores an OS (Operating System), software programs, and the like. In the description of the present embodiment, information that is described as being known is stored here, and loaded into the RAM 1002 as necessary.

  Reference numeral 1008 denotes a storage medium drive, which reads programs and data stored in a storage medium such as a CD-ROM or DVD-ROM in accordance with instructions from the CPU 1001 and outputs them to the RAM 1002 or the external storage device 1007.

  Reference numeral 1009 denotes an I / F, an analog video port for connecting the imaging device 130 or a digital input / output port such as IEEE1394, an RS232C or USB serial port for connecting the attitude sensor 140, and the calculated position of the imaging device 130 And an Ethernet (registered trademark) port for outputting the attitude to the outside. The data input by each is taken into the RAM 1002 via the I / F 1009. A part of the function of the image input unit 160 is realized by the I / F 1009.

  A bus 1010 connects the above-described units.

  FIG. 3 is a flowchart showing a processing procedure of the posture prediction unit 150, which is performed by the CPU 1001 executing a software program of the posture prediction unit 150. It is assumed that the program code according to the flowchart is already loaded in the RAM 1002 in the previous stage of performing the following processing.

  There are various methods for expressing the posture. Here, it is assumed that the posture is expressed by a 3 × 3 rotation matrix R.

In step S3000, the posture prediction unit 150 inputs a posture measurement value R ^# ( ^# is a symbol representing a measurement value by the sensor) from the posture sensor 140.

In step S3010, posture prediction unit 150 inputs azimuth drift error correction value φ ^* from data storage unit 170.

In step S3020, the posture prediction unit 150 converts the posture measurement value R ^# (representing the posture of the posture sensor 140) to the posture conversion from the posture sensor 140 to the measurement target object 130 and the heading drift error correction value φ ^* . Reflecting the drift error correction, the posture of the imaging apparatus 130 after the azimuth drift error correction is calculated, and this is set as a predicted posture value R ^* .

  Here, ΔR (φ) is a rotation matrix that adds rotation by φ in the azimuth direction, and is defined by the following equation as a function of φ.

Here, l = (l ₁ , l ₂ , l ₃ ) represents a known vector representing the vertical upward direction (the direction opposite to the gravity of the earth) in the world coordinate system.

In addition, the _RSC is a 3 × 3 transforming posture from a subjective viewpoint camera coordinate system (a coordinate system representing the position and orientation of the imaging device 130) to a sensor coordinate system (a coordinate system representing the position and orientation of the posture sensor 140). It is a matrix and is set in advance as a known value based on the relative attitude between the attitude sensor 140 and the imaging device 130, which are fixed values.

In step S <b ^> 3030, posture prediction section 150 outputs posture prediction value R ^* to data storage section 170.

  In step S3040, posture prediction unit 150 determines whether or not to end the process. If the process is not to be ended, the process proceeds to step S3000.

  FIG. 4 is a flowchart of processing for calculating parameters indicating the position and orientation of the imaging apparatus 130, and is performed by the CPU 1001 executing a software program of the position / orientation calculation unit 120. It is assumed that the program code according to the flowchart is already loaded in the RAM 1002 in the previous stage of performing the following processing.

The position / orientation calculation unit 120 handles a total of four parameters of the position t = [xyz] ^T of the imaging device 130 and the updated value φ of the orientation drift error correction value of the orientation sensor 140 as unknown parameters to be calculated. That is, in this embodiment, not all the postures are unknown, but the posture prediction value R ^* includes only the azimuth drift error, and the azimuth drift error correction value update value φ A model in which the orientation of the imaging device 130 can be obtained by obtaining only this is applied. Hereinafter, an unknown parameter to be obtained is described by a quaternary state vector s = [xyzφ] ^T.

In step S <b> 4000, the position / orientation calculation unit 120 inputs a predicted value R ^* of the attitude of the imaging device 130 (output of the attitude prediction unit 150) from the data storage unit 170.

In step S <b> 4010, the position / orientation calculation unit 120 sets an initial value of the state vector s as s = [x ₋₁ y ₋₁ z ₋₁ 0] ^T. Here, x ₋₁ , y ₋₁ , and z ₋₁ represent the position of the measurement target object 130 calculated in step S4110 in the process before one loop.

In step S4020, the position / orientation calculation unit 120 stores the set of the image coordinates of the subjective viewpoint index and the world coordinates detected by the index detection unit 110, the set of the image coordinates of the objective viewpoint index and the world coordinates, and the data storage unit. Input from 170. For example, in the case of FIG. 1, the image coordinates u ^Q1 and u ^Q3 of the subjective viewpoint index and the corresponding world coordinates x _W ^Q1 and x _W ^Q3 and the image coordinates u _c ^P1 and u _c ^P2 and u of the objective viewpoint index. _d ^P2 and the corresponding subjective viewpoint camera coordinates (coordinate values in the subjective viewpoint camera coordinate system) x _C ^P1 and x _C ^P2 are input.

  In step S4030, the position / orientation calculation unit 120 determines whether or not the input index information includes information sufficient to estimate the position and orientation, and branches the process accordingly. Specifically, if the total number of input index entities is two or more, the process proceeds to step S4040, and if it is less than two, the process proceeds to step S4100. For example, in the case of FIG. 4, two subjective viewpoint indices and two objective viewpoint indices (three projected images, but two actual indicators) are detected, and the total number is 4. The process proceeds to S4040.

  In step S4040, the position / orientation calculation unit 120 calculates the estimated values of the image coordinates of each subjective viewpoint index and objective viewpoint index input in step S4020 based on the current state vector s.

Calculation of the estimated value u ^{Qkn *} of the image coordinate of the subjective viewpoint index Q ^kn is a function of the world coordinate x _W ^Qkn of the index Q ^kn and the current state vector s:

... Formula (3)
To do. Specifically, the function Fc () is ^obtained by the following equation for ^{obtaining the} subjective viewpoint camera coordinates (coordinates in the subjective viewpoint camera coordinate system) x _C ^Qkn from the world coordinates x _W ^Qkn and s:

And the following equation for ^{obtaining the} image coordinates u ^{Qkn *} from the camera coordinates x _C ^Qkn :

... Formula (5)
It is constituted by. Here, R ^* represents the predicted value of the posture input in step S4000. ΔR (φ) represents a rotation matrix for adding rotation by φ in the azimuth direction, and is defined by Equation 2. Further, f ^C _x and f ^C _y are focal lengths of the imaging device 130 in the x-axis direction and the y-axis direction, respectively, and are held in advance as known values.

On the other hand, the estimated value ^{u Pkm *} calculating the image coordinates of the objective-view-index _{P miles} is (coordinate value in the subjective-view camera coordinate _system) objective-view-index _{P miles} each of the subjective-view camera coordinate ^{x C Pkm} the current state vector s Function:

・・・式（６）

... Formula (6)

To do. Specifically, the function F _B () is ^obtained by the following equation for ^obtaining the coordinate x _W ^{Pkm of the} index on the world coordinate system from x _C ^Pkm and s:

From the world coordinate system x _W ^Pkm , an objective viewpoint camera coordinate (an objective viewpoint camera coordinate system (one point on the objective viewpoint camera 180 is defined as an origin, and three axes orthogonal to each other are defined as an X axis, a Y axis, and a Z axis, respectively). The coordinate of the index on the coordinate system defined as) x _B ^Pkm

And the following equation for ^{obtaining the} image coordinates u ^{Pkm *} from the objective viewpoint camera coordinates x _B ^Pkm ,

・・・式（９）

... Formula (9)

It is constituted by. Here, f ^B _x and f ^B _y are focal lengths in the x-axis direction and y-axis direction of each objective viewpoint camera 180, and R _WB is a 3 × 3 matrix representing the attitude of each objective viewpoint camera 180 in the world coordinate system, t _WB is a three-dimensional vector representing the position of each objective viewpoint camera 180 in the world coordinate system, and is held in advance as a known value for each objective viewpoint camera 180.

In step S4050, the position and orientation calculation unit 120, for each of the indicators (subjective-view-indices and objective-view-index), the estimated value of the image coordinates and the error △ ^{u Qkn} and △ ^{u Pkm} of the measured values, the following equation calculate.

・・・式（１０）

... Formula (10)

・・・式（１１）

... Formula (11)

In step S4060, the position / orientation calculation unit 120 calculates an image Jacobian related to the state vector s for each index (subjective viewpoint index and objective viewpoint index). The image Jacobian of the subjective viewpoint index Q _kn has a 2 × 4 Jacobian matrix J _us ^Qkn (= ∂u /) having a solution obtained by partial differentiation of the function Fc () of Equation 3 with each element of the state vector s. ∂s), specifically, a 2 × 3 Jacobian matrix J _ux ^Qkn (= ∂u /) having a solution obtained by partial differentiation of the right side of Equation 5 with each element of camera coordinates x _C ^Qkn. ∂x) and 3 rows × 4 columns Jacobian matrix J _xs ^Qkn (= ∂x / ∂s) each having a solution obtained by partial differentiation of the right side of Equation 4 with each element of state vector s, Using these, it is calculated by the following equation.

・・・式（１２）

... Formula (12)

On the other hand, the image Jacobian of the objective viewpoint index P _km has a 2 × 4 Jacobian matrix J _us ^Pkm (= ∂) having a solution obtained by partial differentiation of the function Fb () of Equation 6 with each element of the state vector s. u / ∂s), specifically, a 2-row × 3-column Jacobian matrix J _uxB ^Pkm (each element having a solution obtained by partially differentiating the right side of Expression 9 with each element of the objective viewpoint camera coordinates x _B ^Pkm. = ∂u / ∂x _B ), and a 3 row × 3 column Jacobian matrix J _xBxW ^Pkm (= ∂x _B /) having a solution obtained by partial differentiation of the right side of Expression 8 with each element of the world coordinate x _W ^Pkm. ∂x _W ) and 3 rows × 4 columns Jacobian matrix J _xWs ^Pkm (= ∂x _W / ∂s) each having a solution obtained by partial differentiation of the right side of Equation 7 with each element of state vector s Later, using these, the following formula is calculated.

In step S4070, the position / orientation calculation unit 120 calculates the correction value Δs of the state vector s by the following equation based on the error Δu and the image Jacobian J _us for each index calculated in steps S4050 and S4060.

  Here, U is a 2- (N + M) -dimensional error vector in which errors Δu obtained for each index (subjective viewpoint index and objective viewpoint index) are arranged vertically.

In and, theta 'is the matrix of aligned images Jacobian J _us obtained for each of the indicators (subjective-view-indices and objective-view-index) perpendicularly 2 (N + M) rows × 4 columns

Represents the pseudo inverse matrix. In the example of FIG. 1, since N = 2 and M = 3, U is a 10-dimensional vector and θ is a matrix of 10 rows × 4 columns.

  In step S4080, the position / orientation calculation unit 120 corrects the state vector s according to the following equation using the correction value Δs calculated in step S4070, and sets the obtained value as a new estimated value of s.

・・・式（１７）

... Formula (17)

  In step S4090, the position / orientation calculation unit 120 converges the calculation using some criterion such as whether the error vector U is smaller than a predetermined threshold or whether the correction value Δs is smaller than a predetermined threshold. It is determined whether or not. If not converged, the processing after step S4040 is performed again using the corrected state vector s.

  If it is determined in step S4090 that the calculation has converged, in step S4100, the position / orientation calculation unit 120 calculates the orientation of the imaging device 130 from the obtained state vector s. Specifically, an updated value φ of the drift error correction value is obtained from the calculated value of the state vector s obtained up to the previous step, and the following formula:

To calculate the orientation R of the imaging device 130.

  In step S <b> 4110, the position / orientation calculation unit 120 outputs the obtained position and orientation information of the imaging device 130 to the outside via the I / F 1009. In addition, the position t of the imaging device 130 is output to the data storage unit 170. The output form of the position and orientation may be, for example, a set of a 3 × 3 matrix R representing the posture and a three-dimensional vector t representing the position, or may be obtained by converting the posture into Euler angles. A viewing transformation matrix calculated from the orientation may be used, or any other position and orientation description method may be used.

In step S4120, the position / orientation calculation unit 120 uses the updated value φ of the azimuth drift error correction value obtained in the above calculation process to calculate the azimuth drift error correction value φ ^* held by the data storage unit 170. Update with an expression.

  In step S4130, the position / orientation calculation unit 120 determines whether or not to end the process. If not, the process proceeds to step S4000, and the same process is performed on the input data from the next frame onward. .

  As described above, the position and orientation of the imaging apparatus are measured.

  According to the position and orientation measurement apparatus according to the present embodiment, the position and orientation of the imaging apparatus can be measured if the total number of indices observed on the subjective viewpoint image and the objective viewpoint image is two or more. . Therefore, even if the video of the subjective viewpoint is occluded (by concealment by hand etc.), it is possible to measure the position and orientation by the information of the objective viewpoint image (observation of two or more objective viewpoint indices) On the other hand, even in situations where all objective viewpoint indices are blocked, measurement of position and orientation is continued using subjective viewpoint image information (observation of two or more subjective viewpoint indices). It becomes possible to do.

  In the above embodiment, a plurality of objective viewpoint cameras 180 are used. However, a plurality of objective viewpoint cameras are not necessarily required, and even if there is only one objective viewpoint camera, the same as in the present embodiment. Needless to say, an effect can be obtained.

[Second Embodiment]
The first embodiment is intended to measure the position and orientation of the imaging means itself that moves in the space. The position / orientation measurement apparatus according to the present embodiment is a position / orientation measurement apparatus for measuring the position and orientation of an arbitrary target object, and the measurement object is placed in the position / orientation measurement apparatus according to the first embodiment. It has an added configuration. Hereinafter, the position and orientation measurement apparatus and the position and orientation measurement method according to the present embodiment will be described.

  FIG. 5 is a diagram illustrating a configuration of the position and orientation measurement apparatus according to the present embodiment. As illustrated in FIG. 5, the position / orientation measurement apparatus 500 according to the present embodiment includes an image input unit 160, a data storage unit 170, an index detection unit 110, an attitude sensor 140, an attitude prediction unit 150, a position / orientation calculation unit 520, And a subjective viewpoint camera 530.

  Portions having the same functions as those in the first embodiment are denoted by the same reference numerals as those in FIG. However, the image acquired by the subjective viewpoint camera 530 is input to the image input unit 160 as a subjective viewpoint image, the posture prediction value by the posture prediction unit 150 is the posture of the subjective viewpoint camera 530, and the posture sensor. The point 140 is mounted on the subjective viewpoint camera 530 is different from the first embodiment.

  The subjective viewpoint camera 530 is fixedly attached to the measurement target object 580. Here, it is assumed that the position and orientation of the measurement target object 580 in the subjective viewpoint camera coordinate system are known.

The position / orientation calculation unit 520 includes a predicted value R ^* of the attitude of the subjective viewpoint camera 530, a set of image coordinates and world coordinates of each subjective viewpoint index detected by the index detection unit 110, and an image of each objective viewpoint index. Coordinates and a corresponding set of subjective viewpoint camera coordinates are input from the data storage unit 170, and based on these pieces of information, the same processing as that performed by the position / orientation calculation unit 120 in the first embodiment is performed. Calculate the position and orientation. Further, the calculated position is output to the data storage unit 170, and the azimuth drift held by the data storage unit 170 is updated by the updated value φ of the azimuth drift error correction value of the orientation sensor 140 derived in the position and orientation calculation process. Update the error correction value.

  Further, the position / orientation calculation unit 520 is based on the position and orientation of the subjective viewpoint camera 530 (in the world coordinate system) calculated as described above and the position and orientation of the measurement target object 580 in the camera coordinate system that are known values. The position and orientation of the measurement target object 580 are calculated. The calculated position and orientation are output to the outside via the I / F 1009.

  As described above, measurement of the position and orientation of an arbitrary measurement target object is realized.

Note that the position / orientation calculation unit 520 in the present embodiment obtains the position and orientation of the measurement target object 580 after once obtaining the position and orientation of the subjective viewpoint camera 530, but directly obtains the position and orientation of the measurement target object 580. May be. In this case, orientation prediction unit 150 so as to estimate the position and orientation of the measurement object 580 (the R _SC of formula 1, and a 3 × 3 matrix for converting the orientation of the measuring object coordinate system to the sensor coordinate system), Formula 4 which sets the position of the measurement target object 580 as a component of the state vector s and obtains the subjective viewpoint camera coordinates x _C ^Qkn of the subjective viewpoint index from s,

Equation 7 for ^obtaining the world coordinate x _W ^Pkm of the objective viewpoint index from s,

Should be replaced. Here, _RCO is a subjective viewpoint from a measurement object coordinate system (a coordinate system in which one point on the measurement target object 580 is defined as an origin and three axes orthogonal to each other are defined as an X axis, a Y axis, and a Z axis, respectively). A matrix for converting the posture into the camera coordinate system, t _CO is a vector for converting the position between the coordinate systems, and is used to change the position and posture of the measurement target object 580 in the subjective viewpoint camera coordinate system held in advance as a known value. It is assumed that it is calculated in advance.

Note that the measurement target object 580 in the present embodiment may be an imaging device that captures a scene. Then, the subjective viewpoint camera 530 is disposed, for example, upward so as to have a field of view different from the field of view of the imaging device that captures a scene, and accordingly, the subjective viewpoint index Q _k is disposed within the field of view of the subjective viewpoint camera 530. Also good. By doing so, it becomes not enter subjective-view-indices Q _k in the field of view of an imaging apparatus for imaging a scene, to contribute to alleviate such a problem that impairs the appearance.

  In the present embodiment, by mounting a plurality of subjective viewpoint cameras 530 on the measurement target object 580, the measurement of the position and orientation of the measurement target object 580 can be realized with high accuracy in both position and orientation. It may be.

[Third Embodiment]
In the above embodiment, each of the position / orientation calculation units 120 and 520 detects the subjective viewpoint index and the objective viewpoint index using the quaternary state vector s representing the updated value and the position of the azimuth drift error correction value as an unknown. S that minimizes the sum of errors between the coordinates (measured values) and the calculated values of those points has been obtained. However, the method of acquiring the position and orientation of the measurement target object by using geometric constraint conditions obtained from both the subjective viewpoint image and the objective viewpoint image is not limited to this. The position / orientation measurement apparatus according to the present embodiment includes a position / orientation calculation unit using a method different from the method of minimizing the overall error as its constituent elements.

  The configuration of the position / orientation measurement apparatus according to the present embodiment is basically the same as that of the first embodiment, but the position / orientation calculation unit 120 is replaced with a position / orientation calculation unit 120 ′ different from that of the first embodiment. This is the configuration (not shown). That is, the processing performed by the position / orientation calculation unit is different.

  Hereinafter, the position and orientation measurement apparatus and the position and orientation measurement method according to the present embodiment will be described.

  In this embodiment, each unit (the image input unit 160, the data storage unit 170, the index detection unit 110, the posture prediction unit 150, and the position / posture calculation unit 120 ') is handled as software to be executed in one computer. The basic configuration of this computer is the configuration shown in FIG.

  FIG. 6 is a flowchart of processing for calculating parameters indicating the position and orientation of the imaging apparatus 130, which are performed by the CPU 1001 executing the software program of the position and orientation calculation unit 120 '. It is assumed that the program code according to the flowchart is already loaded in the RAM 1002 in the previous stage of performing the following processing.

In step S6000, the position / orientation calculation unit 120 ′ inputs the predicted value R ^* of the attitude of the imaging device 130 (output of the attitude prediction unit 150) from the data storage unit 170.

  In step S6003, the position / orientation calculation unit 120 'inputs a set of image coordinates and camera coordinates for the objective viewpoint index detected by the index detection unit 110.

When multiple objective viewpoint indices are installed, or when multiple objective viewpoint cameras are installed, the projected images of multiple objective viewpoint indices are detected, and each image coordinate is input Occurs. In the present embodiment, even in such a case, only one objective viewpoint index is used for the subsequent processing, and the position / orientation calculation unit 120 ′ determines that an appropriate point is an image of the objective viewpoint index P. It is selected as the coordinate ^{u P.}

In step S6006, the position and orientation calculation unit 120 'based on the image coordinates u ^P, calculates a parameter representing a single straight line constraining the position of the objective-view-index P in the world coordinate system. First, the slope (direction vector) hx, hy, hz of the straight line in the world coordinate system is

Calculated on the basis of the image coordinates u ^P by. Here, R _WB is a 3 × 3 matrix representing the attitude of the objective viewpoint camera 180 that detected the objective viewpoint index P in the world coordinate system, and f ^B _x and f ^B _y are the objective viewpoint camera in the x-axis direction and the y-axis direction, respectively. It is assumed that the focal length is 180 and both are stored in the external storage device 1007 in advance as known values. At this time, the point on the straight line in the world coordinate system is a function of the parameter τ.

It can be expressed as Here, _tWB is the position of the objective viewpoint camera 180 in the world coordinate system, and is assumed to be held in the external storage device 1007 in advance as a known value. The straight line represented by Expression 23 is a straight line that passes through the position of the objective viewpoint camera 180 in the world coordinate system and the position of the objective viewpoint index P in the world coordinate system, and the position of the objective viewpoint index P in the world coordinate system is The parameter τ is obtained by taking an appropriate value.

Hereinafter, a total of two parameters, the parameter τ that determines the position of the objective viewpoint index P in the world coordinate system and the updated value φ of the azimuth drift error correction value of the attitude sensor 140, are treated as unknown parameters to be calculated. In the following, the unknown parameter to be obtained is described by a binary state vector s ′ = [τφ] ^T.

In step S6010, the position / orientation calculation unit 120 ′ sets an appropriate initial value s ′ = [τ ₋₁ 0] ^T in the state vector s ′. Here, as τ ₋₁ , for example, the world coordinate of the objective viewpoint index P is obtained from the position of the imaging device 130 obtained in the processing one loop before, and the point on the straight line l _W closest to the obtained world coordinate is obtained. Is set to represent τ.

  In step S6020, the position / orientation calculation unit 120 'inputs a set of image coordinates of each subjective viewpoint index detected by the index detection unit 110 and its world coordinates.

  In step S6030, the position / orientation calculation unit 120 'determines whether the input subjective viewpoint index number is 1 or more. If the total number N of indexes is less than 1, the process proceeds to step S6100 without performing the s ′ update process realized by steps S6040 to S6090.

In step S6040, the position / orientation calculation unit 120 ′ obtains the estimated value u ^{Qkn *} of the image coordinate for each subjective viewpoint index Q ^kn and the subjective viewpoint camera coordinates (subjective viewpoint) from the world coordinates x _W ^Qkn and s ′. (Coordinate in camera coordinate system) x _C ^Qkn

And, calculated on the basis of the camera coordinate _x ^{C Qkn} in Equation 5 to determine the image coordinates ^{u Qkn *.} Here, x _C ^P is the coordinate value of the index P in the subjective-view camera coordinate system, is held in advance in the external storage device 1007 as known information.

  That is, assuming that the position and orientation of the imaging device 130 are in accordance with the state vector s ′ obtained previously, the image coordinate of each subjective viewpoint index is determined according to the position and orientation relationship between the imaging device 130 and the subjective viewpoint index. Get an estimate.

In step S6050, the position and orientation calculation unit 120 'for each of the subjective-view-indices ^{Q kn,} estimated values of the image coordinates ^{u Qkn *} and the error △ ^{u Qkn} of the measured values ^{u Qkn,} based on Equation 10 is calculated To do.

In step S6060, the position / orientation calculation unit 120 ′ performs, for each subjective viewpoint index Q ^kn , an image Jacobian (that is, a function Fc () of Expression 1 (in this embodiment, Expression 24 and Expression 5), s is s ′), and a 2 row × 2 column Jacobian matrix having a partial differential of each element of s ′) J _{us ′} ^Qkn (= ∂u / ∂s ′) is calculated. Specifically, a Jacobian matrix J _ux ^Qkn (= ∂u / ∂x) of 2 rows × 3 columns having a solution obtained by partial differentiation of the right side of Equation 5 with each element of camera coordinates x _C ^Qkn , and an equation After calculating a Jacobian matrix J _{xs ′} ^Qkn (= ∂x / ∂s ′) of 3 rows × 2 columns having each element a solution obtained by partial differentiation of the right side of 24 with each element of the vector s ′, these are used. Thus, calculation is performed according to Equation 12 (where s is replaced with s ′).

In step S6070, the position / orientation calculation unit 120 ′ calculates the correction value Δs ′ of the state vector s ′ using Expression 14 (where s is replaced with s ′). However, in this embodiment, U is a 2N-dimensional error vector in which errors ^{Δu Qkn} obtained for each subjective viewpoint index are vertically arranged, and θ is an image obtained for each subjective viewpoint index. It represents a 2N × 2 matrix of Jacobian J _{us ′} ^Qkn arranged vertically.

  In step S6080, the position / orientation calculation unit 120 ′ uses the correction value Δs ′ calculated in step S6070 to correct the state vector s ′ using Equation 17 (where s is replaced with s ′), and the obtained value. Is the new estimate of s ′.

  In step S6090, the position / orientation calculation unit 120 ′ performs calculation using some criterion such as whether the error vector U is smaller than a predetermined threshold or whether the correction value Δs ′ is smaller than a predetermined threshold. It is determined whether or not has converged. If not converged, the processing after step S6040 is performed again using the corrected state vector s'.

  If it is determined in step S6090 that the calculation has converged, in step S6100, the position / orientation calculation unit 120 'calculates the position and orientation of the imaging device 130. The posture R is calculated by Equation 18 using the updated value φ of the drift error correction value obtained up to the previous step. On the other hand, the position t is calculated by the following equation using the parameter τ and the posture R obtained up to the previous step.

  In step S6110, the position / orientation calculation unit 120 'outputs the obtained position and orientation information of the imaging device 130 to the outside via the I / F 1009. In addition, the position t of the imaging device 130 is output to the data storage unit 170. The output form of the position and orientation may be, for example, a set of a 3 × 3 matrix R representing the posture and a three-dimensional vector t representing the position, or may be obtained by converting the posture into Euler angles. A viewing transformation matrix calculated from the orientation may be used, or any other position and orientation description method may be used.

In step S6120, the position / orientation calculation unit 120 ′ uses the updated value φ of the azimuth drift error correction value obtained in the above calculation process to calculate the azimuth drift error correction value φ ^* held in the data storage unit 170. Update with Equation 19.

  In step S6130, the position / orientation calculation unit 120 ′ determines whether or not to end the process. If the position / orientation calculation unit 120 ′ does not end the process, the process proceeds to step S6000, and the same process is performed on the input data from the next frame onward. To do.

  With the above processing, a straight line that should have an objective viewpoint index obtained from the objective viewpoint camera 180 is used as a constraint condition, and imaging is performed so as to minimize the error of the subjective viewpoint index on the subjective viewpoint image under the constraint condition. The position and orientation of the device 130 can be obtained.

  Note that the position and orientation measurement results according to this embodiment are preferentially trusted with the information obtained from the objective viewpoint camera 180 compared to the position and orientation measurement results according to the first embodiment. Accordingly, the reliability of the information obtained from the objective viewpoint camera 180 is relatively superior to the information obtained from the imaging device 130, for example, when a high-resolution objective viewpoint camera can be used, or the detection accuracy is very high. In a case where a high marker can be used only as an objective viewpoint index, the position / orientation measurement apparatus according to this embodiment functions more effectively than the first embodiment.

[Fourth Embodiment]
In the above embodiment, the position / orientation calculation units 120, 520, and 120 ′ all obtain the updated value of the orientation drift error correction value of the orientation sensor 140 as an unknown. However, it is of course possible to obtain error correction values for the orientation sensor 140 with respect to the three-axis directions without limiting the orientation correction items only in the azimuth direction. Since the configuration of the position / orientation measurement apparatus according to the present embodiment is substantially the same as the configuration of the position / orientation measurement apparatus according to the first embodiment, only the parts different from those of the first embodiment will be described below. .

The data storage unit 170 in this embodiment stores an error correction rotation matrix ΔR ^* of the attitude sensor 140 instead of the orientation drift error correction value φ ^* of the attitude sensor 140.

The posture prediction unit 150 in this embodiment inputs the error correction rotation matrix ΔR ^* of the posture sensor 140 instead of inputting the azimuth drift error correction value φ ^* of the posture sensor 140 (step S3010), and instead of the equation 1 A predicted posture value R ^* is calculated by the following equation (step S3020).

In the position / orientation calculation unit 120 according to the present embodiment, a total of six parameters, that is, the position t = [xyz] ^T of the imaging device 130 and the ternary expression ω = [ξψζ] ^T of the orientation R of the imaging device 130 should be calculated. Treat as a parameter. In the following, the unknown parameter to be obtained is described by a six-valued state vector s ″ = [xyzξψζ] ^T.

  There are various methods for expressing the posture (rotation matrix) by ternary values. Here, a ternary vector in which the rotation angle is defined by the magnitude of the vector and the rotation axis direction is defined by the vector direction. It is assumed that The posture ω is given by

(However,

It is. )
Can also be represented by a 3 × 3 rotation matrix R, and ω and R can be uniquely converted from each other. Since the conversion method from R to ω is known, the detailed description thereof is omitted.

In the position / orientation calculation unit 120 according to this embodiment, the initial value of the state vector s ″ is set as s ″ = x ₋₁ y ₋₁ z ₋₁ ξ ^* ψ ^* ζ ^* ] ^T (step S4010). Here, x ₋₁ , y ₋₁ , and z ₋₁ represent the position of the measurement target object 130 calculated in the process one loop before, and ξ ^* , ψ ^* , and ζ ^* are the predicted values of the posture. This represents a ternary expression of the posture obtained from R ^* .

  In the position / orientation calculation unit 120 according to the present embodiment, the branch of the processing based on the input index number (step S4030) is determined based on whether the total number of input index entities is three or more.

In the position / orientation calculation unit 120 in the present embodiment, a relational expression for ^obtaining the subjective viewpoint camera coordinates x _C ^Qkn of the subjective viewpoint index Q _{kn from} the world coordinates x _W ^{Qkn of the} index and s (s ″ in the present embodiment) The relational expression for ^obtaining the world coordinate x _W ^Pkm of the objective viewpoint index P _{km from} the subjective viewpoint camera coordinates x _C ^{Pkm of the index} and s (s ″ in the present embodiment) is the expression 4 in the first embodiment: Expression 7 is changed to the following expressions (Expression 28 and Expression 29), respectively. Accordingly, the image Jacobian for each index is a Jacobian matrix J _{us ″} ^Qkn (= ｋu / ∂s ″) of 2 rows × 6 columns.

  In the position / orientation calculation unit 120 according to the present embodiment, the attitude R of the imaging device 130 is calculated (step S4100) using the obtained state vector s ″ according to Expression 27.

In step S4120, the position / orientation calculation unit 120 according to the present embodiment obtains an error correction rotation matrix ΔR ^* of the attitude sensor 140 using the attitude R of the imaging device 130 obtained in the above calculation process by the following equation, and the data The value held in the storage unit 170 is updated.

  As described above, the position and orientation of the imaging apparatus are measured.

[Fifth Embodiment]
In the first embodiment, the objective viewpoint index _Pk arranged on the imaging device 130 (the posture sensor attached to the imaging device 130) is photographed by the objective viewpoint camera 180 fixed in the world coordinate system. However, the configuration for acquiring the position and orientation of the imaging device 130 is not limited to this. The position / orientation measurement apparatus according to the present embodiment is configured such that an objective viewpoint index _Pk arranged in the world coordinate system is captured by an objective viewpoint camera 180 fixed on the imaging apparatus 130. . Hereinafter, the position and orientation measurement apparatus and the position and orientation measurement method according to the present embodiment will be described.

  FIG. 7 is a diagram illustrating a configuration of the position and orientation measurement apparatus according to the present embodiment. As shown in FIG. 7, the position / orientation measurement apparatus 700 according to the present embodiment includes an objective viewpoint camera 180 (180a, 180b), an image input unit 160, a data storage unit 770, an index detection unit 110, an attitude sensor 140, an attitude. The prediction unit 150 and the position / orientation calculation unit 720 are configured to be connected to the imaging device 130. Note that portions having the same functions as those of the first embodiment are denoted by the same reference numerals as those in FIG. 1, and description thereof is omitted.

  On the imaging device 130, an objective viewpoint camera 180 (180a, 180b) having a known position and orientation in the subjective viewpoint camera coordinate system is fixedly disposed.

At a plurality of positions in the real space, the position x _W ^Pk in the world coordinate system is known as an index for photographing with the objective viewpoint camera 180 in addition to the subjective viewpoint index Q _k for photographing with the imaging device 130. A plurality of objective viewpoint indices P _k (k = 1,, K _P ) are also arranged.

Example of FIG. 7, three subjective-view-indices _{Q 1,} Q 2, _{Q 3} and two objective viewpoint index _P 1, _{P 2} is set, two subjective-view-indices _Q 1, Q of which ₃ is included within the field of view of the imaging device 130, a situation where the objective-view-index _{P 1} is within the field of view of the objective viewpoint cameras 180a and 180b, an objective-view-index _{P 2} is included in the field of view of the objective viewpoint camera 180b Is shown.

In the case of FIG. 7, the number of indices detected on the subjective viewpoint image and the number of indices detected on each objective viewpoint image are N = 2, M _a = 1, and M _b = 2. From the index detection unit 110, index identifiers k ₁ = 1, k ₂ = 3, k _a1 = 1, k _b1 = 1, k _b2 = 2 and the identifiers of the cameras that have captured them, and the images corresponding thereto The coordinates u ^Qk1 , u ^Qk2 , u _a ^Pka1 , u _b ^Pkb1 , u _b ^Pkb2 are output.

The objective viewpoint camera 180 (180a, 180b) is arranged such that one of the objective viewpoint cameras _Pk can image the objective viewpoint index _Pk when the imaging device 130 is located within the measurement target range. Here, it is assumed that the position and orientation of each objective viewpoint camera 180 in the subjective viewpoint camera coordinate system are stored in the data storage unit 770 as known values in advance.

The position / orientation calculation unit 720 includes a predicted value of the orientation of the imaging device 130, a set of image coordinates u ^Qkn and world coordinates x _W ^Qkn of each subjective viewpoint index detected by the index detection unit 110, and each objective viewpoint index. Image coordinates u ^Pkam , u ^Pkbm and a corresponding set of world coordinates x _W ^Pkam , x _W ^Pkbm are input from the data storage unit 770, and the position and orientation of the imaging device 130 are calculated based on these information, Output to the outside via the I / F. In addition, the calculated position of the imaging device 130 is output to the data storage unit 770, and the data storage unit 770 holds the updated value of the orientation drift error correction value of the orientation sensor 140 derived in the position and orientation calculation process. Update the azimuth drift error correction value.

  The data storage unit 770 includes an azimuth drift error correction value, an image input from the image input unit 160, a predicted value of posture input from the posture prediction unit 150, a calculated value of position input from the position / posture calculation unit 720, and an index The image coordinates and identifier of each index input from the detection unit 110, the world coordinates of the subjective viewpoint index, which are known values, the world coordinates of the objective viewpoint index, the position of the objective viewpoint camera 180 in the subjective viewpoint camera coordinate system, and Holds data such as posture and inputs / outputs them as needed.

  The flowchart of the process for calculating the parameters indicating the position and orientation of the imaging apparatus 130 according to the present embodiment is almost the same as the flowchart according to the first embodiment (FIG. 4). Only the parts different from the form will be described.

In step S4020, the position / orientation calculation unit 720 inputs the world coordinates x _W ^P1 and x _W ^P2 instead of inputting the subjective viewpoint camera coordinates (coordinate values in the subjective viewpoint camera coordinate system) x _C ^P1 and x _C ^P2. .

In step S4040, the objective viewpoint estimate ^{u Pkm *} calculation of the index _{P miles} of image coordinates, objective-view-index _{P miles} each world coordinates _x ^{W Pkm} and functions of the current state vector s:

To do. Specifically, the function F _D () is ^obtained by the following equation for ^{obtaining the} subjective viewpoint camera coordinates (coordinates in the subjective viewpoint camera coordinate system) x _C ^Pkm from the world coordinates x _W ^Pkm and s:

The objective viewpoint camera coordinates (objective viewpoint camera coordinate system (one point on the objective viewpoint camera 180 is defined as the origin, and three axes orthogonal to each other are defined as the X axis, the Y axis, and the Z axis ⁾ from the subjective viewpoint camera coordinates x _C ^Pkm. The coordinate of the index on the coordinate system defined as the axis) x _B ^Pkm

And, and a objective viewpoint camera coordinate _x ^{B Pkm} by equation 9 to obtain the image coordinates ^{u Pkm *.} Here, f ^B _x and f ^B _y are focal lengths in the x-axis direction and the y-axis direction of each objective viewpoint camera 180, and R _CB is a 3 × 3 matrix representing the attitude of each objective viewpoint camera 180 in the subjective viewpoint camera coordinate system. , T _CB is a three-dimensional vector representing the position of each objective viewpoint camera 180 in the subjective viewpoint camera coordinate system, and is held in advance as a known value for each objective viewpoint camera 180.

  As described above, the position and orientation of the imaging means are measured.

  In the above embodiment, a plurality of objective viewpoint cameras 180 are used. However, a plurality of objective viewpoint cameras are not necessarily required, and even if there is only one objective viewpoint camera, the same as in the present embodiment. Needless to say, an effect can be obtained.

  Also in the second to fourth embodiments, the configuration described in this embodiment for photographing the objective viewpoint index Pk arranged in the world coordinate system by the objective viewpoint camera 180 fixed on the imaging device 130 is used. It goes without saying that it can be done.

(Modification 1)
In each of the above-described embodiments, the steepest descent method expressed by Equation 11 is used to calculate the correction value Δs of the state vector based on the error vector U and the matrix Θ. It does not have to be performed by the steep descent method. For example, the LM method (Levenberg-Marquardt method) which is an iterative solution of a known nonlinear equation may be used, or a statistical method such as M estimation which is a known robust estimation method may be combined. Any numerical calculation method may be applied.

(Modification 2)
In each of the above-described embodiments, an index (hereinafter referred to as a point index) in which one index represents one coordinate is used as the index. However, it is not limited to the type of indicator, and other types of indicators may be used.

  For example, a known position and orientation measurement device (for example, “Takahashi, Ishii, Makino, Nakashizuka, Monocular rectangular marker position / posture high-precision real-time estimation method for VR interface, 3D image conference 96 performance papers, pp. 167-172, 1996.)), a marker having a specific geometric shape can be used as a subjective viewpoint index and / or an objective viewpoint index. For example, when a square marker is used as an index, the world coordinates of each vertex of the square are held as known values (or these values are calculated from the position, orientation, and size of the marker), and each vertex is obtained from the image. By detecting the image coordinates, it is possible to obtain the same effect as the four-point index in the above embodiment. In particular, one rectangular marker having ID information (for example, “Kato, M. Billing Hurst, Asano, Tachibana, Augmented Reality System based on marker tracking and its calibration, Journal of the Virtual Reality Society of Japan, vol. 4, no. 4, pp. 607-616, 1999. ”) is used as an objective viewpoint index in the imaging apparatus (or a configuration in which one square marker is previously installed in the attitude sensor). Since good detection accuracy and identification accuracy can be expected, it can be said to be a particularly suitable form.

In addition, another known position and orientation measurement apparatus (for example, “DG Low: Fitting parametrized three-dimensional models to images, IEEE Transactions on PAMI, vol. 13, no. 5, pp. 441-450, 1991.). An index composed of line features (hereinafter referred to as a line index), such as that used in “)”, may be used. For example, using the distance from the origin of the straight line as a reference for evaluation, an error vector U is constituted by the error Δd calculated from the detected value d from the image and the estimated value d ^* from the state vector s, and d ^* is calculated. Similar to the above embodiment, the matrix θ is configured by a 1 × 6 Jacobian matrix J _ds (= ∂d / ∂s) having a solution obtained by partial differentiation of the equation with each element of the state vector s. The position and orientation can be measured by a simple framework. In addition, it is possible to use these features together by stacking error and image Jacobian obtained from the line index, point index, and other indices.

(Modification 3)
Although the number of subjective viewpoint cameras 530 in the second embodiment is one, a plurality of subjective viewpoint cameras may be installed on the measurement target object 580 and used for position and orientation measurement. In this case, the image input unit 150 and the index detection unit 110 process input images from the respective cameras. In addition, the posture prediction unit 150 and the position / orientation calculation unit 520 perform calculations based on the position and posture of the measurement target object 580. In the position / orientation calculation unit 520, the state vector s is configured by the position of the measurement target object 580 and the azimuth drift correction value, and from the index information obtained from each image, Expression 20 ( _RCO and _tCO differ for each camera) The error and image Jacobian of each index are obtained based on the above, and the error vector U and the matrix θ are formed by stacking them (with objective viewpoint index information), thereby estimating the position and orientation in the same framework as in the above embodiment. It can be performed.

  In addition, although the number of imaging devices 130 to be measured in each of the first, third, and fourth embodiments is one, two imaging devices such as a stereo video see-through head mounted display are measured. Even so, the position and orientation can be measured by a similar method by using one imaging device (for example, the imaging device for the left eye) as a reference.

(Modification 4)
In each of the above embodiments, an attitude sensor having an azimuth drift error is used, but other attitude sensors can also be used. For example, in an orientation sensor that measures an angle in an inclination direction with an acceleration sensor and measures an angle in an azimuth direction with a geomagnetic sensor, an updated value of an error between the position and the azimuth direction is used as an unknown parameter, similar to the above embodiment. The position and orientation of the measurement target can be measured by the processing. Also, even when using an orientation sensor that measures only the tilt direction, the position and orientation of the measurement target can be measured by a similar process, assuming a 3-axis orientation sensor in which the measured value in the azimuth direction is always 0. it can.

(Modification 5)
As the subjective viewpoint camera or objective viewpoint camera used in each of the above embodiments, a camera that captures light having a wavelength different from that of visible light may be used. For example, a camera that captures infrared light may be used as the objective viewpoint camera, and an index that emits or reflects infrared light may be used as the objective viewpoint index. In this case, since the subjective viewpoint index is not captured by the objective viewpoint camera, there is an effect that the subjective viewpoint index is not erroneously detected on the objective viewpoint image.

  Here, if each objective viewpoint index is configured by a marker that emits infrared light at different timings, it is possible to identify which index is the index. That is, the index detection unit may extract an area corresponding to the marker from the objective viewpoint image, and the center of gravity position may be set as the detection coordinates of the index emitted at the same timing as the imaging by the objective viewpoint camera. Of course, when there is one objective viewpoint index, it is not necessary to control the light emission timing.

  It is also possible to use a camera that captures infrared light as the objective viewpoint camera, and use an index that emits or reflects infrared light as the objective viewpoint index. Furthermore, a camera that captures infrared light may be used for both the subjective viewpoint camera and the objective viewpoint camera, and an index that emits or reflects infrared light may be used for both the subjective viewpoint index and the objective viewpoint index.

  The camera that captures light having a wavelength different from that of visible light is not limited to a camera that captures infrared light, and a camera that captures ultraviolet light or the like may be used. Furthermore, you may use the camera which images simultaneously both the light of a wavelength different from visible light, and visible light.

[Other Embodiments]
An object of the present invention is to supply a storage medium (or recording medium) in which a program code of software for realizing the functions of the above-described embodiments is recorded to a system or apparatus, and a computer (or CPU or MPU) of the system or apparatus. Needless to say, this can also be achieved by reading and executing the program code stored in the storage medium. In this case, the program code itself read from the storage medium realizes the functions of the above-described embodiments, and the storage medium storing the program code constitutes the present invention. Further, by executing the program code read by the computer, not only the functions of the above-described embodiments are realized, but also an operating system (OS) running on the computer based on the instruction of the program code. It goes without saying that a case where the function of the above-described embodiment is realized by performing part or all of the actual processing and the processing is included.

  Furthermore, after the program code read from the storage medium is written into a memory provided in a function expansion card inserted into the computer or a function expansion unit connected to the computer, the function is determined based on the instruction of the program code. It goes without saying that the CPU or the like provided in the expansion card or the function expansion unit performs part or all of the actual processing and the functions of the above-described embodiments are realized by the processing.

  When the present invention is applied to the storage medium, the storage medium stores program codes corresponding to the flowcharts described above.

It is a figure which shows the structure of the position and orientation measuring apparatus in 1st Embodiment. It is a figure which shows the basic composition of a computer. 10 is a flowchart of processing for calculating parameters indicating the position and orientation of the imaging apparatus 130, which are performed by the CPU 1001 executing a software program of the orientation prediction unit 150. 10 is a flowchart of processing for calculating parameters indicating the position and orientation of the imaging apparatus 130, which are performed when the CPU 1001 executes a software program of the position and orientation calculation unit 120. It is a figure which shows the structure of the position and orientation measurement apparatus in 2nd Embodiment. 10 is a flowchart of processing for calculating a parameter indicating the position and orientation of the imaging apparatus 130, which is performed by the CPU 1001 executing a software program of the position and orientation calculation unit 120 '. It is a figure which shows the structure of the position and orientation measurement apparatus in 5th Embodiment.

Claims (6)

A subjective viewpoint imaging means for acquiring a subjective viewpoint image obtained by imaging a subjective viewpoint point index arranged in a real space;
Objective viewpoint imaging means for acquiring an objective viewpoint image obtained by imaging an objective viewpoint point index arranged with the subjective viewpoint imaging means from a known objective viewpoint position and orientation fixed in the world coordinate system of the real space; and
Posture measuring means provided in the subjective viewpoint imaging means for measuring posture measurement values of the subjective viewpoint imaging means;
Azimuth drift error correction value of the posture measuring means, the position of the subjective viewpoint point index in the world coordinate system, the position of the objective viewpoint point index with respect to the subjective viewpoint imaging means, the focal length of the subjective viewpoint imaging means, and the objective viewpoint Holding means for previously holding the focal length of the imaging means ;
Posture prediction means for calculating a posture prediction value of the subjective viewpoint imaging means based on the posture measurement value and the azimuth drift error correction value;
First detection means for detecting image coordinates of at least one subjective viewpoint point index from the subjective viewpoint image;
Second detection means for detecting image coordinates of at least one objective viewpoint point index from the objective viewpoint image;
The objective viewpoint position and orientation, the predicted posture value, the image coordinates of the detected subjective viewpoint point index, the position of the subjective viewpoint point index in the world coordinate system, the image coordinates of the detected objective viewpoint point index, and the objective Position and orientation calculation means for calculating the position and orientation of the subjective viewpoint imaging means based on the position of the viewpoint point index with respect to the subjective viewpoint imaging means;
Azimuth error update means for updating the held azimuth drift error correction value based on the calculated position and orientation of the subjective viewpoint imaging device;
With
The position and orientation calculation means is
Setting value setting means for setting a setting value of the position and orientation of the subjective viewpoint imaging means;
Based on the set value, the position of the subjective viewpoint point index in the world coordinate system, and the focal length of the subjective viewpoint imaging means, a subjective coordinate estimation value of the image coordinate of the subjective viewpoint point index of the subjective viewpoint image is calculated. A subjective estimate calculation means;
Based on the setting value, the position of the objective viewpoint point index with respect to the subjective viewpoint imaging means, the objective viewpoint position and orientation, and the focal length of the objective viewpoint imaging means, the image coordinates of the objective viewpoint point index of the objective viewpoint image Objective estimated value calculation means for calculating an objective coordinate estimated value of
A subjective image error calculating means for calculating a subjective error between the image coordinates of the detected subjective viewpoint point index and the subjective coordinate estimated value;
Objective image error calculation means for calculating an objective error between the image coordinates of the detected objective viewpoint point index and the objective coordinate estimation value;
Correction value calculation means for calculating a correction value of only the azimuth component of the position and orientation of the subjective viewpoint imaging means based on the calculated subjective error and objective error;
An information processing apparatus comprising: a correction unit that corrects the position and orientation of the subjective viewpoint imaging unit based on the calculated correction value and the set value . The position and orientation calculation means is
A determination means for determining whether the error or the correction value is smaller than a preset threshold;
The set value setting means resets the corrected position and orientation of the subjective viewpoint imaging means as the set value when the error or the correction value is not smaller than the threshold value. The information processing apparatus according to 1. The subjective viewpoint imaging means is attached to a measurement target object existing in a real space,
The holding means further holds the relative position and orientation of the subjective viewpoint imaging means and the measurement target object;
The position / orientation calculation unit calculates the position / orientation of the measurement target object based on the calculated position / orientation of the subjective viewpoint imaging apparatus and the relative position / orientation. Information processing device. A position and orientation measurement method performed by an information processing apparatus,
The subjective viewpoint imaging means possessed by the information processing apparatus inputs a subjective viewpoint image obtained by imaging a subjective viewpoint point index placed in a real space; and
The objective viewpoint imaging unit included in the information processing apparatus images the objective viewpoint point index arranged along with the subjective viewpoint imaging unit from an objective viewpoint position and orientation that is fixed and known in the world coordinate system of the real space. An objective viewpoint imaging process for inputting an objective viewpoint image;
A posture measuring step that the posture measuring means of the information processing apparatus is provided in the subjective viewpoint imaging device and measures a posture measurement value of the subjective viewpoint imaging means;
A posture prediction step in which a posture prediction unit included in the information processing apparatus calculates a posture prediction value of the subjective viewpoint imaging unit based on the posture measurement value and a azimuth drift error correction value of the posture measurement unit held in advance; ,
A first detection step in which a first detection unit included in the information processing apparatus detects image coordinates of at least one subjective viewpoint point index from the subjective viewpoint image;
A second detection step in which a second detection means included in the information processing apparatus detects image coordinates of at least one subjective viewpoint point index from the objective viewpoint image;
The position and orientation calculation means of the information processing apparatus includes the objective viewpoint position and orientation, the estimated posture value, the image coordinates of the detected subjective viewpoint point index, the position of the subjective viewpoint point index in the world coordinate system, and the detected position. A position and orientation calculation step of calculating a position and orientation of the subjective viewpoint imaging device based on the image coordinates of the objective viewpoint point index and the position of the objective viewpoint point index with respect to the subjective viewpoint imaging means;
An azimuth error update step in which an azimuth error update unit included in the information processing apparatus updates the held azimuth drift error correction value based on the calculated position and orientation of the subjective viewpoint imaging device;
Have
In the position and orientation calculation step,
A setting value setting step in which a setting value setting unit of the position and orientation calculation unit sets a setting value of the position and orientation of the subjective viewpoint imaging unit;
The subjective estimated value means of the position / orientation calculating means is configured to determine the position of the subjective viewpoint image based on the set value, the position of the subjective viewpoint point index in the world coordinate system, and the focal length of the subjective viewpoint imaging means stored in advance. A subjective estimated value calculating step of calculating a subjective coordinate estimated value of the image coordinates of the subjective viewpoint point index;
The objective estimated value means of the position / orientation calculating means is configured to set the set value, the position of the objective viewpoint point index with respect to the subjective viewpoint imaging means, the objective viewpoint position / posture, and the focal length of the objective viewpoint imaging means held in advance. An objective estimated value calculating step of calculating an objective coordinate estimated value of the image coordinates of the objective viewpoint point index of the objective viewpoint image;
A subjective image error calculating step in which a subjective image error calculating unit of the position / orientation calculating unit calculates a subjective error between the image coordinates of the detected subjective viewpoint index and the subjective coordinate estimated value;
An objective image error calculating step in which the objective image error calculating unit of the position and orientation calculating unit calculates an objective error between the image coordinates of the detected objective viewpoint point index and the objective coordinate estimated value;
A correction value calculating step in which the correction value calculating means of the position / orientation calculating means calculates a correction value of only the azimuth component of the position and orientation of the subjective viewpoint imaging means based on the calculated subjective error and objective error; ,
A correcting step in which the correcting means of the position and orientation calculating means corrects the position and orientation of the subjective viewpoint imaging means based on the calculated correction value and the set value;
A position and orientation measurement method characterized by comprising: The program for making a computer perform each process of the position and orientation measurement method of Claim 4 . A computer-readable recording medium storing the program according to claim 5 .

Images