JP4612804B2 - 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.

  As a method for measuring the position and orientation of an object, there is a magnetic sensor that measures the position and orientation of an object by installing a magnetic field generator fixed in the environment and measuring the magnetic field by a magnetic field detection device attached to the object. Widely used. However, since the magnetic sensor is greatly affected by the surrounding magnetic field environment, satisfactory measurement accuracy cannot be obtained in an environment such as an office building surrounded by a steel frame (Patent Document 1).

On the other hand, in the Japanese Patent Application No. 2003-004945, the present applicant photographs this with an imaging device (hereinafter referred to as an objective viewpoint camera) in which three or more indices are set on an object and fixed on a ceiling or the like. Then, it is proposed to calculate the position and orientation of the object by detecting the image coordinates of the index from the obtained image.
JP-A-11-136706

  This method has the advantage that there are few restrictions on the environment, but when the distance between the indices on the image (or the area of the convex hull formed by the indices) is insufficient, the expected performance is obtained. I couldn't. In particular, the posture is extremely poor in terms of both accuracy and stability, and in order to improve the posture, it is necessary to devise so that the convex hull formed by the index on the image becomes sufficiently large. However, if an object is photographed using a camera with a narrow angle of view so that the index becomes a large image, there arises a problem that the measurement range for movement is extremely limited. In addition, although a certain degree of improvement can be expected by providing a wide interval between the indicators on the object, it is difficult to do so depending on the size of the object.

An information processing apparatus for calculating the position and orientation of an object, the arranged index on the object, from an objective viewpoint position and orientation are known fixed position, an image input unit for inputting a captured image captured, the Attitude measurement means for measuring an attitude measurement value of an object, an orientation drift error correction value of the attitude measurement means, a position of the index with respect to the object, and a holding means for holding the objective viewpoint position; the attitude measurement value and the Based on the azimuth drift error correction value, the posture prediction means for calculating the posture prediction value of the object, the index detection means for detecting the image coordinates of the index captured in the captured image, the posture prediction value, and the detected image coordinates of the indices, position relative to the object of the indicator, and on the basis of the objective viewpoint position and orientation, the position and orientation calculation means for calculating the position and orientation of the object, the calculated The based on the position and orientation, anda updating means for updating the azimuth drift error correction value the held, the position and orientation calculation means, set value input means for inputting a set value of the position and orientation of the object And an index determination means for determining whether or not the detected index is two or more, and when it is determined that the detected index is two or more, the posture predicted value, the set value, and the index Based on the position with respect to the object and the objective viewpoint position and orientation, estimated value calculation means for calculating an estimated value of the image coordinates of the index, image coordinates of the detected index, and estimated values of the image coordinates of the index An error calculation means for calculating the error of the object, a correction value calculation means for calculating a correction value of the position and orientation of the object based on the calculated error, and the set value based on the calculated correction value. Correction means for correcting , Characterized by having a.

  According to the position and orientation measurement apparatus of the present invention, it is possible to stably and accurately measure the position and orientation of an object even when the convex hull formed by the index on the image is small. In other words, if the same placement index is used, it is possible to obtain a stable position and posture compared to the conventional method, and there are fewer restrictions on the placement of the index, so a wider variety of objects can be measured than the conventional method. can do. In addition, since a wide-angle objective viewpoint camera that covers a wider area can be used, it is possible to keep a wide measurement range for movement.

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

[First Embodiment]
The position and orientation measurement apparatus according to the present embodiment measures the position and orientation of an arbitrary measurement target object. 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 the present 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 measurement target object 130.

At a plurality of positions on the posture sensor 140, as an index for photographing with the objective viewpoint camera 180, an object coordinate system (one point on the measurement target object 130 is defined as an origin, and three axes orthogonal to each other are defined as X An index P _k (k = 1,..., K) having a known position x _C ^Pk on the coordinate system defined as the axis, the Y axis, and the Z axis is set.

These indices are indices observed on the objective viewpoint image acquired by the objective viewpoint camera 180 when the measurement target object 130 is positioned at each point within the measurement target range where the position and orientation are to be measured. It is desirable that the total number is always at least 2 or more. In the example of FIG. 1, two indexes P ₁ and P ₂ are set, and the index P ₁ is within the field of view of the objective viewpoint camera 180c, and the index P ₂ is within the field of view of the objective viewpoint cameras 180c and 180d. Indicates an included situation.

The index P _k may be configured by, for example, circular markers each having a different color, or may be configured by feature points such as natural features each having a different texture feature. 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. The indicator may be set intentionally, or may be a natural shape that is not set intentionally.

  The objective viewpoint camera 180 (180a, 180b, 180c, 180d) is arranged with one of the measurement target objects 130 fixed at a position where the measurement target object 130 can be imaged when the measurement target object 130 is located within the measurement target range. Yes. Here, it is assumed that the position and orientation of each objective viewpoint camera 180 in the world coordinate system are previously stored 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 objective viewpoint image into digital data and stores the digital data in the data storage unit 170.

  The posture sensor 140 is attached to the measurement target object 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 sensor for observing the direction of the earth's gravity as a constituent element and have a function of canceling accumulation of drift errors in the tilt angle direction. And the 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 to predict the posture of the measurement target object 130, and transfers the data to the data storage unit 170. Output.

  The index detection unit 110 receives an objective viewpoint image from the data storage unit 170 and detects the image coordinates of the index captured 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 objective viewpoint 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 configured by feature points having different texture features, by performing template matching on the template image of each index stored in advance as known information on the objective viewpoint image, Detect the position of the indicator. Note that the calculated value of the position of the measurement target object 130 (output of the position / orientation calculation unit 120) and the predicted value of the attitude (output of the posture prediction unit 150) are further input from the data storage unit 170, and an image is generated based on these values. By predicting the position of the index above and limiting the search range, it is possible to reduce the calculation load of the index detection process and to reduce the 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. Hereinafter, the index detected by the index detection unit 110 with respect to the captured image of the objective viewpoint camera 180 (a, b, c, d) is referred to as a camera identifier x (x = a, b, c, d), The identifier m (m = 1,, M _x ) attached to each of the detected indices is represented as P _kxm . Here, M _x represents the number of indices detected for each objective viewpoint image. In addition, the image coordinates of the detected index P _kxm are _expressed as u _a ^Pkam , u _b ^Pkbm , u _c ^Pkcm , and u _d ^Pkdm , respectively, according to the identifier of the objective viewpoint camera that captured the image. M is the sum of the indices detected on each image. For example, in the case of FIG. 1, M _a = 0, M _b = 0, M _c = 2, M _d = 1, M = 3, and index identifiers k _c1 = 1, k _c2 = 2 and k _d1 = 2, these and identifiers of the captured camera image coordinates _u ^c pKC1 corresponding to _these, ^u c _pkc2, ^{u d Pkd1} is output.

The position and orientation calculation unit 120 corresponds to the predicted value of the orientation of the measurement target object 130, the image coordinates u _a ^Pcam , u _b ^Pkbm , u _c ^Pkcm , and u _d ^Pkdm detected by the index detection unit 110. Object coordinates (coordinate values in the object coordinate system) x _C ^Pkam , x _C ^Pkbm , x _C ^Pkcm , x _C ^Pkdm are input from the data storage unit 170, and the position of the measurement target object 130 is based on these ^pieces of information And the attitude are calculated and output to the outside via the I / F. In addition, the calculated position of the measurement target object 130 is output to the data storage unit 170, and the data storage unit 170 further updates the azimuth drift error correction value of the orientation sensor 140 derived in the position and orientation calculation process. Update the retained 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 Data such as the image coordinates and identifiers of the indices input from the detection unit 110 and the object coordinates (coordinate values in the object coordinate system) of the indices, which are known values, are held, and these are input / output 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 measurement target object 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 measurement target object 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 measurement target object 130. Ethernet (registered trademark) port or the like for outputting the position and posture of the camera 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 uses the posture measurement value R ^# (representing the posture of the posture sensor 140) to change the posture from the posture sensor 140 to the measurement target object 130 and the azimuth drift error correction value φ ^* . Reflecting the drift error correction, the posture of the measurement target object 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.

The _RSC is a 3 × 3 matrix for converting the posture from the object coordinate system (the coordinate system representing the position and posture of the measurement target object 130) to the sensor coordinate system (the coordinate system representing the position and posture of the posture sensor 140). And is set in advance as a known value based on the relative posture of the posture sensor 140 and the measurement target object 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 measurement target object 130, and is performed by the CPU 1001 executing a 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.

The position / orientation calculation unit 120 handles a total of four parameters of the position t = [xyz] ^T of the measurement target object 130 and the updated value φ of the orientation drift error correction value of the attitude sensor 140 as unknown parameters to be calculated. That is, in the present 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 posture of the measurement target object 130 can be obtained by obtaining only this is applied. In the following, the unknown parameter to be obtained is described by a quaternary state vector s = [xyzφ] ^T.

In step S4000, the position / orientation calculation unit 120 inputs a predicted value R ^* of the attitude of the measurement target object 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 the initial value of the state vector s as s = [x _τ−1 y _τ−1 z _τ−1 0] ^T. Here, x _τ−1 , y _τ−1 , z _τ−1 represent the position of the measurement target object 130 calculated in step S <b> 4110 in the process one loop before (denoted as time τ−1).

In step S <b> 4020, the position / orientation calculation unit 120 inputs a set of the image coordinates of the index detected by the index detection unit 110 and the world coordinates from the data storage unit 170. For example, in the case of FIG. 1, image coordinates u _c ^P1 , u _c ^P2 and u _d ^P2 and corresponding object coordinates (coordinate values in the object 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. 1, since two indices (three projected images but two substance of indices) are detected, the process proceeds to step S4040.

In step S4040, the position and orientation calculation unit 120, for each of the indicators _{P miles,} to calculate an estimated value ^{u Pkm *} of the image coordinates. The calculation of u ^{Pkm *} is a function of the object coordinates (coordinate values in the object coordinate system) x _C ^{Pkm of} each index P _km previously held as known information and the current state vector s:

Based on. Specifically, the function F _B () is ^obtained by the following formula 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 ,

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 ^B _x and f ^B _y are focal lengths of the objective viewpoint cameras 180 in the x-axis direction and the y-axis direction, 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 / orientation calculation unit 120 calculates an error Δu ^Pkm between the estimated value u ^{Pkm *} of the image coordinates and the actual measurement value u ^Pkm for each index P _km based on Expression 7.

In step S4060, the position / orientation calculation unit 120 performs, for each index P _km , an image Jacobian relating to the state vector s (that is, a solution obtained by partially differentiating the function Fb () of Equation 3 with each element of the state vector s. 2 rows × 4 columns Jacobian matrix) J _us ^Pkm (= ∂u / ∂s) of the element is calculated. Specifically, a 2 × 3 Jacobian matrix J _uxB ^Pkm (= ∂u / ∂x _B ) having a solution obtained by partial differentiation of the right side of Equation 6 with each element of the objective viewpoint camera coordinates x _B ^Pkm. 3 rows × 3 columns Jacobian matrix J _xBxW ^Pkm (= ∂x _B / ∂x _W ) having a solution obtained by partial differentiation of the right side of Equation 5 with each element of world coordinates x _W ^Pkm and Equation 4 3 × 4 Jacobian matrix J _xWs ^Pkm (= ∂x _W /） s) having each element as a solution obtained by partial differentiation of the right side of state vector s with each element, and J _us ^Pkm is calculated by the following equation: calculate.

In step S4070, the position / orientation calculation unit 120 calculates the correction value Δs of the state vector s based on the error Δu ^Pkm and the image Jacobian J _us ^Pkm calculated in steps S4050 and S4060. Specifically, a 2M-dimensional error vector U in which errors Δu ^Pkm obtained for each index P _km are arranged vertically and an image Jacobian J _us ^Pkm obtained for each index P _km are arranged vertically. 2M rows × 4 columns matrix θ, and using a pseudo inverse matrix θ ′ of θ,

Calculate as In the example of FIG. 1, since M = 3, U is a 6-dimensional vector, and θ is a 6 × 4 matrix.

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

  In step S4090, the position / orientation calculation unit 120 converges the calculation by 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 measurement target object 130 from the obtained state vector s. Specifically, an updated value φ of the azimuth drift error correction value is obtained from the state vector s obtained up to the previous step, and the following formula:

To calculate the posture R of the measurement target object 130.

  In step S <b> 4110, the position / orientation calculation unit 120 outputs the obtained position and orientation information of the measurement target object 130 to the outside via the I / F 1009. Further, the position t of the measurement target object 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 a posture component converted into Euler angles, A modeling conversion 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 measurement target object 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, it is the same as in the present embodiment. Needless to say, an effect can be obtained.

[Second Embodiment]
In the above embodiment, the update value φ of the orientation sensor error correction value of the orientation sensor is obtained as an unknown value. However, when the accuracy of the orientation sensor is good, the usage time is short, or the orientation drift error When the correction value update value can be input manually, the parameter to be obtained by the position / orientation calculation unit 120 may be limited to only the position of the measurement target object 130. 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 measurement target object, and calculates the position / orientation of the position / orientation measurement apparatus according to the first embodiment. The function of the unit 120 is changed. 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, all φ in the first embodiment are set to 0. That is, in the position / orientation calculation unit 120 in this embodiment, the unknown parameter to be obtained is described by a ternary state vector s ′ = [xyz] ^T. Further, in the position / orientation calculation unit 120 in the present embodiment, a unit obtained by removing a term related to φ from each processing step (each Jacobian matrix, Equation 4, etc.) of the position / orientation calculation unit 120 in the first embodiment may be used. For example, Equation 4 is changed to the following equation.

  According to the position / orientation measurement apparatus according to the present embodiment, since the number of unknown parameters to be obtained is reduced, further improvement in the stability of the obtained solution (position and orientation of the measurement target object 130) can be expected.

In order to manually input the update value of the azimuth drift error correction value, for example, a correction value update means may be further added to the configuration of FIG. The correction value update means obtains the updated value φ of the azimuth drift error correction value according to the operator's input, and updates the azimuth drift error correction value φ ^* stored in the data storage unit 170 according to Equation 12. The correction value updating unit is realized by using a specific key of the keyboard 1004 as an interface, for example. For example, the update value may be set to +0.1 degree by the [+] key and −0.1 degree by the [−] key. Note that it is needless to say that the correction value updating means by manual input can also be used in the form in which the update value of the azimuth drift error correction value is derived based on the image information as in the first embodiment. Absent.

[Third Embodiment]
In each of the above-described embodiments, the parameter to be obtained as an unknown value is fixed to either the updated value φ of the position and orientation drift error correction value or only the position. However, it is not necessarily fixed which parameter is unknown, and a more suitable position and orientation can be estimated by appropriately changing the unknown parameter according to the characteristics of each parameter. . 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 measurement target object, and the position / orientation calculation unit of the position / orientation measurement apparatus according to the first embodiment. It has a configuration in which 120 functions are changed. Hereinafter, the position and orientation measurement apparatus and the position and orientation measurement method according to the present embodiment will be described.

  The position / orientation calculation unit according to the present embodiment has both the functions of the position / orientation calculation unit according to the first embodiment and the position / orientation calculation unit according to the second embodiment. Normally, only the position is an unknown parameter. The position / orientation calculation unit according to the second embodiment performs the process, and the update value of the position and orientation drift error correction value is set as an unknown parameter at regular time intervals (for example, once every 10 seconds (300 frames)). The process of the position and orientation calculation unit in the embodiment is executed. Note that the time interval for updating the azimuth drift error correction value is preferably set according to the drift characteristics of the attitude sensor 140, and is preferably set by an interactive operation by the operator.

  According to the position and orientation measurement apparatus according to the present embodiment, when the orientation sensor 140 is used as an orientation sensor 140 that can obtain an accuracy sufficient to ignore the orientation drift error for a short time, while correcting the orientation drift error, It can be expected to improve the stability of the obtained solution.

[Fourth Embodiment]
In each of the above embodiments, the updated value of the azimuth drift error correction value is obtained from image information at a certain time. However, since the value of the azimuth drift error has a high correlation between frames, it can be obtained with higher accuracy by using information of a plurality of frames. 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 measurement target object, and the position / orientation calculation unit of the position / orientation measurement apparatus according to the first embodiment. It has a configuration in which 120 functions are changed. Hereinafter, the position and orientation measurement apparatus and the position and orientation measurement method according to the present embodiment will be described.

  The position / orientation calculation unit 520 according to the present embodiment has both the functions of the position / orientation calculation unit according to the first embodiment and the position / orientation calculation unit according to the second embodiment, and executes both parameter estimation processes. . FIG. 5 is a flowchart of processing for calculating parameters indicating the position and orientation of the measurement target object 130, and is performed by the CPU 1001 executing a software program of the position / orientation calculation unit 520. 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 S5000, the position / orientation calculation unit 520 inputs the predicted value R ^* of the posture of the measurement target object 130 (output of the posture prediction unit 150) from the data storage unit 170, as in step S4000 in the first embodiment. .

  In step S5010, as in step S4020 in the first embodiment, the position / orientation calculation unit 520 inputs the image coordinates of the index detected by the index detection unit 110 and the world coordinates thereof from the data storage unit 170.

In step S5020, the position / orientation calculation unit 520 uses the position t = [xyz] ^T of the measurement target object 130 and the updated value φ of the azimuth drift error correction value of the attitude sensor 140 as unknown parameters. These are estimated by the same processing as S4010 and steps S4030 to S4090.

In step S5030, the position and orientation calculating section 520 integrates the update value phi azimuth drift error correction value calculated in step S5020, obtains the integrated value phi _SUM.

  In step S5040, the position / orientation calculation unit 520 determines whether or not an integration process for a predetermined number of frames (for example, 30 frames) has been performed. If yes, the process proceeds to step S5050, and has not been performed. Advances the process to step S5080.

In step S5050, the position and orientation calculation unit 520, by dividing the integrated value phi _SUM calculated in step S5030 by the number of processing frames, calculates an average value of the updated value of the azimuth drift error correction value obtained in each frame This is newly set as an updated value φ of the azimuth drift error correction value. Thereafter, the integrated value _φSUM is cleared to zero.

In step S5060, the position / orientation calculation unit 520 is stored in the data storage unit 170 using the updated value φ of the azimuth drift error correction value obtained in step S5050, as in step S4120 in the first embodiment. The azimuth drift error correction value φ ^* is updated by Equation 12.

  In step S5070, the position / orientation calculation unit 520 uses the updated value φ of the azimuth drift error correction value obtained in step S5050 to calculate the attitude of the measurement target object 130 using Equation 11, and obtains the obtained attitude as a new one. The predicted value of the posture.

In step S5080, the position / orientation calculation unit 520 estimates the position t = [xyz] ^T of the measurement target object 130 as an unknown parameter by the same process as in the second embodiment.

  In step S5090, the position / orientation calculation unit 520 outputs information on the position and orientation of the measurement target object 130 obtained in the same manner as in step S4110 in the first embodiment.

  In step S5100, the position / orientation calculation unit 520 determines whether or not to end the process. If not, the process proceeds to step S5000, and the same processing is performed on input data after the next frame (time τ + 1). Execute the process.

  As described above, the accuracy of the update value of the correction value of the azimuth drift error can be improved by using information of a plurality of frames. In this embodiment, the average value of the update values obtained in each frame is used. However, the median value of the update values obtained in each frame may be used, or any other low-pass filter may be used. . Also, as a matter of course, the position / orientation calculation unit according to the present embodiment can be used as position / orientation calculation processing means using the position and the update value of the drift error correction value in the third embodiment as unknown parameters.

(Modification 1)
In the above embodiment, the steepest descent method expressed by Equation 9 is used to calculate the state vector correction value Δs based on the error vector U and the matrix θ, but the correction value Δs is not necessarily calculated by the steepest descent method. It does not have to be. 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.

  In the above embodiment, the nonlinear optimization method for obtaining the optimum (minimizing error) solution for each input image is used. However, the error is eliminated based on the index error on the image. The method for obtaining such a solution is not limited to this. By calculating the position of the measurement target object and the azimuth drift error correction value of the orientation sensor from the image information captured by the objective viewpoint camera, the measurement target The essence of the present invention of obtaining the position and orientation of an object stably and with high accuracy is not impaired even when other methods are used. For example, an extended Kalman filter (iterative extended Kalman filter) or an iterative extended Kalman filter (it is known as a technique for obtaining a solution that eliminates the error based on the error of the index on the image) J. Park, B. Jiang, and U. Neumann, Vision-based posted computation: robust and accumulated augmented reality tracking, Proc. Second International Worship. Details are described) If the s in the above embodiment is defined as a state vector, by defining the formula 3 and the observation equation, it is possible to construct a filter having the advantages of the present invention.

(Modification 2)
In the above embodiment, an index such that one index represents one coordinate (hereinafter referred to as a point index) is used as the index P. However, other indicator types can be applied.

  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 also be used as an 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 installed on the measurement target object (or a configuration in which a single quadrangular marker is previously installed on the attitude sensor). Since good 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, 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 the calculation formula of d ^* Is formed by a 1 row × 4 column Jacobian matrix J _ds (= ∂d / ∂s) having a solution obtained by partially differentiating each of the state vectors with each element of the state vector s. The position and orientation can be measured by the 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)
In the above embodiment, an attitude sensor having an azimuth drift error is used, but another attitude sensor having a significant error only in the azimuth direction can be used as the attitude sensor. For example, in an orientation sensor of a type that measures an angle in an inclination direction with an acceleration sensor and measures an angle in an azimuth direction with a geomagnetic sensor, the update value of the position and the azimuth error correction value is used as an unknown parameter, as in the above embodiment. The position and orientation of the measurement target can be measured by the processing. However, in this case, since the nature of the error is different from the azimuth drift error, it is not suitable for use in the form as in the third embodiment or the fourth embodiment. 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.

[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. Is also 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 above-mentioned storage medium, the program code corresponding to the flowchart described above is stored in the storage medium.

It is a figure which shows the structure of the position and orientation measurement apparatus in 1st Embodiment. It is a figure which shows the basic composition of a computer. 10 is a flowchart of processing for calculating a parameter indicating the position and orientation of the measurement target object 130, which is performed by the CPU 1001 executing a software program of the posture prediction unit 150. 10 is a flowchart of processing for calculating parameters indicating the position and orientation of the measurement target object 130, which are performed by the CPU 1001 executing a software program of the position and orientation calculation unit 120. 10 is a flowchart of processing for calculating parameters indicating the position and orientation of the measurement target object 130, which are performed by the CPU 1001 executing a software program of the position and orientation calculation unit 520.

Claims (5)

An information processing apparatus for calculating the position and orientation of an object,
An image input means for inputting a captured image of an index placed on the object from an objective viewpoint position and orientation whose fixed position is known;
Posture measuring means for measuring the posture measurement value of the object;
A holding means for holding the orientation drift error correction value of the posture measuring means, the position of the index with respect to the object, and the objective viewpoint position;
Posture prediction means for calculating a posture prediction value of the object based on the posture measurement value and the azimuth drift error correction value;
Index detecting means for detecting the image coordinates of the index imaged in the captured image;
Position and orientation calculation means for calculating the position and orientation of the object based on the predicted orientation value, the image coordinates of the detected index, the position of the index with respect to the object, and the objective viewpoint position and orientation;
Updating means for updating the held azimuth drift error correction value based on the calculated position and orientation;
Have
The position and orientation calculation means is
Setting value input means for inputting a setting value of the position and orientation of the object;
Index determining means for determining whether or not the detected index is two or more;
When it is determined that there are two or more detected indices, the image coordinates of the indices are estimated based on the predicted posture value, the set value, the position of the index with respect to the object, and the objective viewpoint position and orientation. An estimated value calculating means for calculating a value;
Error calculating means for calculating an error between the image coordinates of the detected index and an estimated value of the image coordinates of the index;
Correction value calculation means for calculating a correction value of the position and orientation of the object based on the calculated error;
Correction means for correcting the set value based on the calculated correction value;
An information processing apparatus comprising: The position / orientation calculation means includes a convergence determination means for determining whether the calculated error or the calculated correction value is smaller than a threshold value held in advance .
When the set value input means determines that the calculated error or the calculated correction value is not smaller than a pre-stored threshold value, the calculated position / orientation is re-used as the set value. The information processing apparatus according to claim 1, wherein the information processing apparatus is input. A position and orientation measurement method performed by an information processing apparatus that calculates the position and orientation of an object,
The image input means of the information processing apparatus inputs an imaged image captured from an objective viewpoint position and orientation with a known fixed position and an index that is arranged on the object and whose position relative to the object is known Input process;
A posture measurement step in which the posture measurement means of the information processing apparatus measures a posture measurement value of the object;
A posture prediction step of calculating a posture prediction value of the object based on the posture measurement value and the orientation drift error correction value of the posture measurement means held in advance;
An index detection unit in which the index detection unit included in the information processing apparatus detects an image coordinate of the index captured in the captured image;
The position and orientation calculation means of the information processing device calculates the position and orientation of the object based on the predicted orientation value, the image coordinates of the detected index, the position of the index with respect to the object, and the objective viewpoint position and orientation. A position and orientation calculation step to calculate;
An updating step that the update unit included in the information processing apparatus updates the held azimuth drift error correction value based on the calculated position and orientation;
Have
In the position and orientation calculation step,
A set value input step in which the set value input means of the position and orientation calculation means inputs a set value of the position and orientation of the object;
An index determination unit included in the position and orientation calculation unit determines whether or not the detected index is two or more points; and
When the estimated value calculating means included in the position / orientation calculating means determines that there are two or more detected indices, the estimated attitude value, the set value, the position of the index with respect to the object, and the objective viewpoint An estimated value calculating step of calculating an estimated value of the image coordinates of the index based on the position and orientation;
An error calculating step of calculating an error between the detected image coordinates of the index and an estimated value of the image coordinates of the index;
A correction value calculation step in which the correction value calculation means included in the position and orientation calculation means calculates a correction value of the position and orientation of the object based on the calculated error; and
A correction step in which the correction means included in the position and orientation calculation means corrects the set value based on the calculated correction value;
A position and orientation measurement method characterized by comprising:   The program for making a computer perform each process which the position and orientation measurement method of Claim 3 has.   A computer-readable storage medium storing the program according to claim 4.

Images