Position and orientation determination method and apparatus and storage medium

[0001] The present invention relates to position and orientation determination method and apparatus for identification of parameter indicating the position and orientation of image sensing means for image sensing in actual space.

[0002] In a method for obtaining an external parameter (position and orientation) of camera for image sensing of actual space, methods other than utilization of obtained video image, e.g. measurement of position and orientation of camera by using a position and orientation sensor such as a magnetic sensor, are known. However, in such measurement by position and orientation sensor, the accuracy of measurement is insufficient. Accordingly, if a virtual object is overlay-displayed in actual space based on the position and orientation of the camera measured by the position and orientation sensor, the object may be shifted from a desired position.

[0003] Conventionally, several methods for improvement in accuracy of position and orientation of camera for image sensing of actual space have been proposed. One of these methods is providing feature points (land marks) in already-known three-dimensional positions in actual space, and correcting the position and orientation of the camera, by using the difference between an actual position of a land mark of interest on an image-sensing plane and a predicted position of the land mark of interest on the image-sensing plane based on the position and orientation of camera at this time.

[0004] Further, another one of the methods is calculating the difference between an actual position of each land mark of interest and a predicted position of land mark of interest based on the position and orientation of camera, and correcting the difference by averaging, thereby further improving the reliability of correction on external parameter of camera.

[0005] However, in any of the conventional position and orientation correction method, the difference in a rotation angle about the visual axis cannot be removed. The visual axis V here is a straight line connecting a principal point L on the image-sensing plane, as one of camera internal parameters, and a view point position (center of lens) C (FIG. 1). As shown in FIG. 1, the principal point L is an intersection between the visual axis and the image-sensing plane, and the view point position C is calculated upon calculation of camera internal parameters by preprocessing which is called camera calibration.

[0006] In general position and orientation correction methods, correction is performed by moving the view point position in parallel to the image-sensing plane or rotating a view point orientation in a pan and/or tilt direction. However, in any of these methods, there is no component in the rotational (roll) direction about the visual axis. Accordingly, in the above-described general correction methods, as the difference in the rotational angle about the visual axis cannot be corrected, even though an image of actual space and an image of unrealistic (virtual) space are combined, the respective positions do not accurately correspond with each other.

[0007] The present invention has been made in consideration of the above-described problems, and has its object to enable detection of position and orientation of image sensing means with high accuracy.

[0008] According to the present invention, the foregoing object is attained by providing a position and orientation determination method for identifying a parameter indicating position and orientation of image sensing means for performing image sensing on actual space, in which plural feature points exist in already-known three-dimensional positions, comprising: a measurement step of measuring the position and orientation of the image sensing means by a method other than utilization of video image obtained by image sensing by the image sensing means, and obtaining a first parameter indicating the position and orientation of the image sensing means; and a correction step of correcting the first parameter obtained at the measurement step based on the plural feature points in the video image of the actual space obtained by the measurement unit, and identifying a second parameter indicating the position and orientation of the image sensing means, wherein at the correction step, the first parameter is corrected by rotation of the image sensing means about a visual axis, based on the plural feature points in the video image.

[0009] According to another aspect of the present invention, the foregoing object is attained by providing a position and orientation determination apparatus for identifying a parameter indicating position and orientation of image sensing means for performing image sensing on actual space, in which plural feature points exist in already-known three-dimensional positions, comprising: a measurement unit adapted to measure the position and orientation of the image sensing means by a method other than utilization of video image obtained by image sensing by the image sensing means, and obtain a first parameter indicating the position and orientation of the image sensing means; and a correction unit adapted to correct the first parameter obtained by the measurement unit based on the plural feature points in the video image of the actual space obtained by the measurement unit, and identify a second parameter indicating the position and orientation of the image sensing means, wherein the correction unit corrects the first parameter by rotation of the image sensing means about a visual axis, based on the plural feature points in the video image.

[0010] Other features and advantages of the present invention will be apparent from the following description taken in conjunction with the accompanying drawings, in which like reference characters designate the same name or similar parts throughout the figures thereof.

[0026] A preferred embodiment of the present invention will now be described in detail in accordance with the accompanying drawings.

[0027] In the present embodiment, a parameter indicating the position and orientation of camera is corrected by using at least one of plural land marks subjected to image sensing, thereafter, correction by rotation about a visual axis is performed based on the plural land marks, and the position and orientation of the camera is accurately detected.

[0028] First, as generation correction, a method for correcting an external parameter of camera from one land mark and a method for correcting the external parameter of camera from plural land marks will be described. Next, processing characteristic of the present embodiment for additional correction by rotation about a visual axis will be described.

[0029] The land mark here means a marker, which is e.g. a sticky tab having particular color or shape information, artificially attached to an object in actual space, or a particular point in natural scene as a land mark.

[0030] FIG. 2 is a schematic diagram for explanation of general correction method for external parameter of camera (parameter indicating the position and orientation). In FIG. 2, a point A indicates the position of land mark predicted based on a parameter indicating the position and orientation of camera; a point B, the actual position of the land mark; and a point C, the position of camera view point. Note that the positions of points A and B are positions in a camera coordinate system, and the point C is an origin point of the camera coordinate system. Further, a point P indicates the position of the point A on an image-sensing plane; and a point Q, the position of the point B on the image-sensing plane. As shown in FIG. 2, the coordinates of the points P and Q are (x_p, y_p) and (x_q, y_q); the width and height of the image-sensing plane are w and h; the focal distance of the camera (the distance between the point C and the image-sensing plane), d; a vector from the point C to the point Q, v₁; a vector from the point C to the point P, v₂; and an angle formed with the vectors v₁ and v₂, θ.

[0031] At this time, as shown in FIG. 2, there is a shift (difference) between the predicted position of the land mark on the image-sensing plane based on the position and orientation of the camera and the actual position. As a method for correcting the shift, 2 types of methods utilizing one land mark (correction by rotation of camera and correction by parallel movement of camera) and further 2 types of methods utilizing plural land marks (correction by rotation of camera and correction by parallel movement of camera) are known.

[0032] First, the methods utilizing one land mark will be described, and the methods utilizing plural land marks will be described next.

<1. Correction Method Utilizing One Land Mark>

[0033] First, correction by using one land mark will be described.

[0034] As the method using one land mark, a method for changing the orientation of camera from a point B direction to a point A direction by θ while the position of camera is fixed (correction method by rotation of camera), and a method for moving the camera in parallel to the image-sensing plane from the point B to the point A direction by the distance between the points A and B (correction method by parallel movement of camera) are known. Hereinbelow, these 2 methods will be described with reference to the flowcharts of FIGS. 10 and 11.

<1-1. Correction Method by Rotation of Camera Utilizing One Land Mark>

[0035] The above-described vectors v₁ and v₂ are obtained. From the above settings, the respective components of the vectors v₁ and v₂ are as follows (step S1001). $\begin{array}{cc}\begin{array}{c}{v}_1=\left(x_q-\frac{w}{2},y_q-\frac{h}{2},-d\right)\\ v_2=\left(x_p-\frac{w}{2},y_p-\frac{h}{2},-d\right)\end{array}& \left[\mathrm{Expression}1\right]\end{array}$

[0036] Note that the respective vectors are normalized to vectors having a size 1 (step S1002). Note that |v| represents the size of vector v. $\begin{array}{cc}\begin{array}{c}{v}_1^{\prime }=\frac{v_1}{v_1}\\ v_2^{\prime }=\frac{v_2}{v_2}\end{array}& \left[\mathrm{Expression}2\right]\end{array}$

[0037] In a case where the camera is rotated, the rotation axis is a straight line orthogonal to a plane formed with the vectors v₁ and v₂ passing through the camera view point position (point C). The vector in the rotation axis direction can be obtained from the vector product between the vectors v₁ and v₂ (actually, vectors normalized from the vectors v₁ and v₂ (v′₁, v′₂) are used) (step S1003).

 v_x=v′₁×v′₂.  [Expression 3]

[0038] Note that v_x is the directional vector of the rotation axis having components (l, m, n). Further, as the rotation angle θ is formed with the vectors v₁ and v₂, it can be obtained as follows (actually, vectors normalized from the vectors v₁ and v₂ (v′₁, v′₂) are used) (step S1004).
θ=arc cos(v′₁·v′₂)  [Expression 4]

[0039] Accordingly, a correction matrix ΔM_c used for correction by rotation of camera is calculated as follows (step S1005). $\begin{array}{cc}\Delta M_c=\lfloor \begin{array}{cccc}ll\left(1-\cos \theta \right)+\cos \theta & ml\left(1-\cos \theta \right)+n\sin \theta & nl\left(1-\cos \theta \right)+m\sin \theta & 0\\ lm\left(1-\cos \theta \right)+n\sin \theta & mm\left(1-\cos \theta \right)+\cos \theta & nm\left(1-\cos \theta \right)+l\sin \theta & 0\\ ln\left(1-\cos \theta \right)+m\sin \theta & mn\left(1-\cos \theta \right)+l\sin \theta & nn\left(1-\cos \theta \right)+\cos \theta & 0\\ 0& 0& 0& 1\end{array}\rfloor & \left[\mathrm{Expression}5\right]\end{array}$

[0040] A matrix indicating the position and orientation of the camera (viewing transformation matrix) is multiplied by the correction matrix ΔM_c, thereby the position and orientation of the camera (external parameter) can be corrected. That is, the point P is displayed in the position of point Q, thus the position of the land mark on the image-sensing plane predicted from the position and orientation parameter corresponds with the actual position. Note that the rotation transformation is made by appropriately combining rotation in a pan direction and/or rotation in a tilt direction.

<1-2. Correction Method by Parallel Movement of Camera Utilizing One Land Mark>

[0041] In a case where the difference is corrected by parallel movement of camera, the position of the camera is moved in parallel to the image-sensing plane from the point B in the point A direction by the distance between the points A and B. As a result, what is seen in the position of point P on the image-sensing plane can be seen in the position of point Q. First, the vectors v₁, and v₂ are obtained (step S1101). Further, a vector v_m from the point P to the point Q is as follows (step S1102).
v_m=v₁−v₂  [Expression 6]

[0042] Assuming that a vector from the point C to the point A is v_a (step S1103), as a triangle CPQ and a triangle CAB are similar figures, a vector V₃ from the point A to the point B can be obtained as follows (step S1104). $\begin{array}{cc}{v}_3=\frac{v_a}{v_2}·v_m& \left[\mathrm{Expression}7\right]\end{array}$

[0043] As the shift (|v₃|) corresponds to the shift of the position of land mark in camera space, if v₃=(s, t, u) holds, the correction matrix ΔM_c used for parallel movement of camera is calculated as follows (step S1105). $\begin{array}{cc}\Delta M_c=\lfloor \begin{array}{cccc}1& 0& 0& -s\\ 0& 1& 0& -t\\ 0& 0& 1& -u\\ 0& 0& 0& 1\end{array}\rfloor & \left[\mathrm{Expression}8\right]\end{array}$

[0044] The position and orientation of camera (external parameter) can be corrected by multiplying the matrix indicating the position and orientation of camera by the correction matrix ΔM_c, as a result, what is seen in the position of point P on the image-sensing plane can be seen in the position of point Q.

<2. Correction Method Utilizing Plural Land Marks>

[0045] Next, a correction method in a case where plural land marks are observed will be described. In the above methods utilizing one land mark, the above difference is reduced only around the land mark of interest, however, differences may increase around other land marks. Accordingly, shift detection is performed for all the land marks included in the visual field of camera (plural land marks) and utilized by averaging.

[0046] FIG. 3 is a schematic diagram for explanation of correction method utilizing plural land marks. Points P₁, P₂ and P₃ indicate respective land mark positions on the image-sensing plane predicted based on position and orientation of camera, and points Q₁, Q₂ and Q₃, positions on which the actual land mark positions are projected on the image-sensing plane. The rotation and/or parallel movement of camera is performed such that the points P₁, P₂ and P₃ correspond with the points Q₁, Q₂ and Q₃ by averaging. Hereinbelow, the correction will be more particularly described.

<2-1. Correction Method by Rotation of Camera Utilizing Plural Land Marks>

[0047] In this method, correction is made by rotation of camera. Instead of a representative point, rotation angles of camera and directional vectors of rotation axis are obtained for the respective land marks, by the above-described correction method by rotation of camera, and the obtained values are used by averaging. FIG. 4 is a schematic diagram showing the averaging correction method by utilizing rotation of camera in a case where plural land marks are used.

[0048] Hereinbelow, the correction method by rotation of camera in a case where plural land marks are used will be described with reference to the flowchart of FIG. 12.

[0049] A variable indicating respective land mark positions on the image-sensing plane predicted based on the position and orientation of camera is P_i (1≦i≦n), a variable indicating positions where actual land mark positions are projected on the image-sensing plane is Q_i (1≦i≦n).

[0050] First, with regard to P_i and Q_i, a directional vector v_xi of straight line of rotation axis is obtained by a similar method to the above-described “correction method by rotation of camera utilizing one land mark” (steps S1201 to S1203). Then, a mean value v_xmean of all the vectors v_xi is obtained (step S1204) $\begin{array}{cc}{v}_{\mathrm{xmean}}=\frac{\left(v_{\mathrm{x1}}+v_{\mathrm{x2}}+\cdots v_{\mathrm{xn}}\right)}{n}& \left[\mathrm{Expression}9\right]\end{array}$

[0051] Next, a rotation angle for rotation about the rotation axis (the straight line with v_xmean as a directional vector) is obtained (step S1206). First, a plane S_i passing through a vector v_1i from the point C to the point Q_i and v_xmean is obtained. Then, a vector v_4i, which is projected from a vector v_2i from the point C to the point P_i on the plane S_i, is obtained. FIG. 5 is a schematic diagram showing a method for generating the vector V_4i calculated by the above processing. In FIG. 5, the rotation angle when the point P_i corresponds with the point Q_i by rotation of camera is θi. The rotation angle θi can be obtained by the following expression by using vectors v_2i and v_4i. $\begin{array}{cc}\begin{array}{c}{v}_{2i}^{\prime }=\frac{v_{2i}}{v_{2i}}\\ v_{4i}^{\prime }=\frac{v_{4i}}{v_{4i}}\\ {\theta }_i=\arccos \left(v_{2i}^{\prime }·v_{4i}^{\prime }\right)\end{array}& \left[\mathrm{Expression}10\right]\end{array}$

[0052] Then a mean value θ_mean of all the rotation angles θ_i is calculated (step S1208). $\begin{array}{cc}{\theta }_{\mathrm{mean}}=\frac{\left({\theta }_1+{\theta }_2+\cdots {\theta }_n\right)}{n}& \left[\mathrm{Expression}11\right]\end{array}$

[0053] As a result, the camera is rotated by the rotation angle θ_mean about the rotation axis with v_xmean as its directional vector, thereby the rotation of camera reflecting all the points P_i and Q_i on the image-sensing plane can be performed. As a result of the above calculation, the correction matrix ΔM_c is obtained by using v_xmean and θ_mean (step S1209). Assuming that V_mean=(l′, m′, n′) holds, the correction matrix ΔM_c is represented by the following expression. $\begin{array}{cc}\Delta Mc=\lfloor \begin{array}{cccc}{l}^{\prime }{l}^{\prime }\left(1-\cos {\theta }_{\mathrm{mean}}\right)+\cos {\theta }_{\mathrm{mean}}& m^{\prime }{l}^{\prime }\left(1-\cos {\theta }_{\mathrm{mean}}\right)+n\sin {\theta }_{\mathrm{mean}}& n^{\prime }{l}^{\prime }\left(1-\cos {\theta }_{\mathrm{mean}}\right)+m^{\prime }\sin {\theta }_{\mathrm{mean}}& 0\\ l^{\prime }{m}^{\prime }\left(1-\cos {\theta }_{\mathrm{mean}}\right)+n\sin {\theta }_{\mathrm{mean}}& m^{\prime }{m}^{\prime }\left(1-\cos {\theta }_{\mathrm{mean}}\right)+\cos {\theta }_{\mathrm{mean}}& n^{\prime }{m}^{\prime }\left(1-\cos {\theta }_{\mathrm{mean}}\right)+l^{\prime }\sin {\theta }_{\mathrm{mean}}& 0\\ l^{\prime }{n}^{\prime }\left(1-\cos {\theta }_{\mathrm{mean}}\right)+m^{\prime }\sin {\theta }_{\mathrm{mean}}& m^{\prime }{n}^{\prime }\left(1-\cos {\theta }_{\mathrm{mean}}\right)+l^{\prime }\sin {\theta }_{\mathrm{mean}}& n^{\prime }{n}^{\prime }\left(1-\cos {\theta }_{\mathrm{mean}}\right)+\cos {\theta }_{\mathrm{mean}}& 0\\ 0& 0& 0& 1\end{array}\rfloor & \left[\mathrm{Expression}12\right]\end{array}$

[0054] The position and orientation of camera (external parameter) can be corrected by multiplying the matrix indicating the position and orientation of camera by the correction matrix ΔM_c. That is, the respective points P_i on the image-sensing plane are displayed on the respective Q_i positions, by averaging, thus the predicted positions of land marks correspond with the actual positions by averaging. By this method, the correction matrix can be generated by reflecting information on all the land marks on the image-sensing plane.

<2-2. Correction Method by Parallel Movement of Camera Utilizing Plural Land Marks>

[0055] In this method, parallel movement components of camera are obtained for the respective land marks by using the above-described correction method by parallel movement of camera and used by averaging. FIG. 6 is a schematic diagram showing a method for correcting plural land marks by parallel movement of camera.

[0056] Hereinbelow, the processing of the correction method will be described with reference to the flowchart of FIG. 13.

[0057] A variable indicating respective land mark positions on the image-sensing plane predicted based on the position and orientation of camera is P_i (1≦i≦n), a variable indicating positions where actual land mark positions are projected on the image-sensing plane is Q_i (1≦i≦n). First, a vector v_ki=(s_i, t_i, u_i) with respect to the points P_i and Q_i is obtained by using the above-described correction method by parallel movement of camera using one feature point (step S1301 to S1303). Then, mean values of the respective points P_i and Q_i are obtained (step S1304). $\begin{array}{cc}\begin{array}{c}\frac{s_{\mathrm{mean}}\left(s_1+s_2+\cdots +s_n\right)}{n}\\ \frac{t_{\mathrm{mean}}\left(t_1+t_2+\cdots +t_n\right)}{n}\\ \frac{u_{\mathrm{mean}}\left(u_1+u_2+\cdots +u_n\right)}{n}\end{array}& \left[\mathrm{Expression}13\right]\end{array}$

[0058] Then, the correction matrix is obtained by using a mean vector V_kmean=(s_mean, t_mean, u_mean) as follows (step S1305). $\begin{array}{cc}\Delta M_c=\left[\begin{array}{cccc}1& 0& 0& -s_{\mathrm{mean}}\\ 0& 1& 0& -t_{\mathrm{mean}}\\ 0& 0& 1& -u_{\mathrm{mean}}\\ 0& 0& 0& 1\end{array}\right]& \left[\mathrm{Expression}14\right]\end{array}$
<3. Correction Method by Rotation about Axis Parallel to Visual Axis>

[0059] In the above-described 4 correction methods, as it is understood from the directions of correction as shown in FIGS. 4 and 6, the difference in rotational angle about the visual axis cannot be corrected even using correction values obtained by rotation and parallel movement. Accordingly, in the present embodiment, a correction matrix ΔM′_c is obtained by multiplying the correction matrix ΔM_c obtained by the above methods by a rotation correction matrix ΔM_rc for the visual axis, and the position and orientation matrix of camera is multiplied by the matrix ΔM′_c, thereby the position and orientation of camera can be corrected. Note that in this method, it is necessary to use 2 or more land marks on the image-sensing plane.

[0060] FIG. 7 is a schematic diagram for explanation of correction method by rotation about the visual axis according to the present embodiment. Points P′₁, P′₂ and P′₃ are values corrected by any of the above-described 4 methods. The visual axis V is a vector from the view point position (center of camera) C to the principal point L. In the present embodiment, the rotation of camera is performed about the visual axis V such that the points P′₁, P′₂ and P′₃ correspond with the points Q₁, Q₂ and Q₃ by averaging. Note that θ_R1, θ_R2 and θ_R3 are angles formed with the points P′₁ and Q₁, P′₂ and Q₂, and P′₃ and Q₃ (with the principal point L as the center). Further, vectors from the principal point L to the points P′₁, P′₂ and P′₃ are v_p1, v_p2 and v_p3, and vectors from the principal point L to the points Q₁, Q₂ and Q₃ are v_q1, v_q2 and v_q3.

[0061] The correction processing of the present embodiment is performed on the assumption that 2 or more land marks exist on the image-sensing plane. In a case where there is only one land mark, the matrix ΔM_c is regarded as the matrix ΔM′_c. Further, if there is no land mark, the position and orientation of camera obtained by another position and orientation sensor can be utilized.

<3-1. Method 1>

[0062] Hereinbelow, the processing of the correction method according to the present embodiment will be described with reference to the flowchart of FIG. 14.

[0063] First, a predicted position P′_i of the position and orientation of camera is obtained by any of the 4 correction methods described in 1-1, 1-2, 2-1 and 2-2 (step S1401).

[0064] Next, an angle θ_Ri formed with vectors v_qi and v_pi is obtained. In FIG. 7, the rotation angle for correspondence between the point P′_i with the point Q_i is θ_Ri, which is obtained by using the vectors v_qi and v_pi by the following expression (step S1402). $\begin{array}{cc}{v}_{\mathrm{qi}}^{\prime }=\frac{v_{\mathrm{qi}}}{v_{\mathrm{qi}}}{v}_{\mathrm{pi}}^{\prime }=\frac{v_{\mathrm{pi}}}{v_{\mathrm{pi}}}{\theta }_{\mathrm{Ri}}=\arccos \left(v_{\mathrm{qi}}^{\prime }·v_{\mathrm{pi}}^{\prime }\right)& \left[\mathrm{Expression}15\right]\end{array}$

[0065] Then, a mean value θ_Rmean of all the rotation angles θ_Ri is calculated. $\begin{array}{cc}{\theta }_{\mathrm{Rmean}}=\frac{\left({\theta }_{\mathrm{R1}}+{\theta }_{\mathrm{R2}}+{\theta }_{\mathrm{R3}}+\cdots +{\theta }_{\mathrm{Rn}}\right)}{n}& \left[\mathrm{Expression}16\right]\end{array}$

[0066] As a result, the camera is rotated about the rotation axis with the visual axis as a directional vector by the rotation angle θ_Rmean, thereby the point P′_i on the image-sensing plane corresponds with the point Q_i.

[0067] However, as it is understood from the above expression to obtain the vectors v′_qi and v′_pi, if |v_qi| or |v_pi| is 0, i.e., if the position of Q_i is on the principal point L as the center of rotation on the image-sensing plane, the angle θ_Ri is not fixed and the correction rotation angle cannot be calculated. One method for calculation of the correction rotation angle in this state is weighting the rotation angle θ_Ri, and reducing the weight of the rotation angle θ_Ri as the point Q_i is closer to the principal point L, and the weight is 0 on the principal point L. By this method, the rotation correction angle can be obtained from other rotation angle than the unfixed rotation angle without influence by the unfixed rotation angle. The correction rotation angle using weighting is obtained by the following expression (step S1404).
θ_Rmean=w_i·θ_R1+w₂·θ_R2+w₃·θ_R3+ . . . +w_n·θ_Rn  [Expression 17]

[0068] Note that the method for eliminating the influence of the unfixed rotation angle is not limited to the method by weighting, but any other method is applicable to the present embodiment as long as it can obtain an appropriate correction rotation angle.

[0069] A function to determine a weighted coefficient w_i is set such that the weight is reduced as the position is closer to the principal point L and becomes 0 on the principal point L. Assuming that the function is f and |v_qi| is d_qi, the following expression holds.
w_i=f(d_qi)  [Expression 18]

[0070] An example of the function f is as follows. $\begin{array}{cc}f\left(d_{\mathrm{qi}}\right)=\{\begin{array}{cc}1& \left(d_{\mathrm{qi}}>t\right)\\ \frac{d_{\mathrm{qi}}}{t}& \left(d_{\mathrm{qi}}\le t\right)\end{array}& \left[\mathrm{Expression}19\right]\end{array}$

[0071] Note that in the present embodiment, after the correction by parallel movement or rotation utilizing marker(s), rolling correction about the visual axis is performed as secondary correction. Immediately before the secondary correction, there is not a wide difference occurs between |v_qi|(=d_qi) and |v_pi|(=d_pi). Accordingly, correction can be sufficiently made by weighting with d_qi as an argument. Even if the point P_i is on the principal point L, the unfixed θ_Ri is ignored. However, weighting in consideration of d_pi can be performed. That is, the function to determine the weighted coefficient w_i is not limited to the function f with d_qi as an argument, but other function obtained by multiplying the function f by a function with d_pi as a coefficient, as follows, may be employed.
w_i=f(d_qi)·f(d_pi)

[0072] The correction matrix ΔM′_c is obtained from the result of the above calculation (step S1405). As the visual axis vector V is a straight line vertical to the image-sensing plane, the expression to obtain the rotation correction matrix ΔM_rc to the visual axis is as follows. $\begin{array}{cc}\Delta M_{\mathrm{rc}}=\left[\begin{array}{cccc}\cos {\theta }_{\mathrm{Rmean}}& -\sin {\theta }_{\mathrm{Rmean}}& 0& 0\\ \sin {\theta }_{\mathrm{Rmean}}& \cos {\theta }_{\mathrm{Rmean}}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]& \left[\mathrm{Expression}20\right]\end{array}$

[0073] The correction matrix ΔM_c obtained by any of the above-described 4 correction methods is multiplied by the rotation correction matrix ΔM_rc to the visual axis, thereby the final correction matrix ΔM′_c is obtained.
ΔM′_c=ΔM_rc·ΔM_c  [Expression 21]

[0074] The matrix indicating the position and orientation of camera is multiplied by the final correction matrix ΔM′_c, thereby the position and orientation of camera (external parameter) can be corrected.

[0075] The difference of position and orientation of camera to rotation in the visual axis direction can be corrected by application of this method, and the difference between the predicted land mark position and the actual position can be further reduced.

<3-2. Method 2>

[0076] In the above <Method 1>, when the mean value of rotation angles is obtained at step S1404, the weight around the principal point L is reduced. However, in a case where a virtual object is overlaid, a user often pays attention to a position around the virtual object of interest. Accordingly, the position and orientation of camera can be determined by utilizing this phenomenon. That is, in a case where plural land marks obtained by image sensing are used by averaging, the influence of land mark near a position of interest is increased in comparison with other land marks, and emphasis is placed on the position of interest. By this processing, the influence of noise in positions other than the position of interest can be reduced, and the parameter of position and orientation of camera with higher reliability can be identified.

[0077] In the <Method 2>, as an example of increasing the influence of land mark around the point of interest, a method for increasing the influence of land mark around an object of interest by utilizing a mean weight value upon averaging calculation of rotation angle will be described.

[0078] Note that the object of interest is a virtual object. In a case where plural virtual objects exist in a scene, since there is a high possibility that a virtual object closest to the center of image (not the principal point L) attracts attention, a virtual object drawn in a position closest to the center of image is selected as an object of interest. Further, it may be arranged such that not only the closest virtual object but also all the virtual objects are weighted. As a method for weighting, a method allowing a user to select a weight or a method of increasing the weight of virtual object at the center of image while reducing the weight as the object is away from the center, and the like, are applicable.

[0079] The expression of weighted averaging is as follows.
θ_mean=w_e1·θ_R1+w_e2·θ_R2+w_e3·θ_R3+ . . . +w_enθ_Rn  [Expression 22]
w_e1+w_e2+w_e3+ . . . +w_en=1  [Expression 22]

[0080] Note that the method for increasing the influence of land mark around the point of interest is not limited to the method utilizing weighted averaging, but any other method is applicable to the present embodiment as long as it improves position accuracy around the object of interest.

[0081] A function to determine w_ei is set such that the weight is the minimum around the principal point L and is the maximum on the point of interest. As an example of the function, the following expression can be given. Assuming that the distance between a point of interest S and a point Q is d_si, the function to determine w_ei is expressed as follows.
w_ei=f(d_qi)·g(d_si)  [Expression 23]

[0082] The function f is the function as described in the above <Method 1>. As an example of the function g, the following expression is given.
g(d_si)=u−d_si  [Expression 24]

[0083] Note that u is a constant such that the sum of w_ei is 1.

<3-3. Method 3>

[0084] In the <Method 1> and <Method 2>, the final correction matrix ΔM′_c is obtained by utilizing the correction matrix ΔM_c obtained by selecting one of the general 4 types of correction methods.

[0085] In the <Method 3>, the correction matrix ΔM_c is obtained by combination of the above 4 methods, and the final correction matrix ΔM′_c is obtained by multiplying the matrix ΔM_c by the rotation correction matrix ΔM_rc to the visual axis. By performing the correction by parallel movement conversion and rotation transformation, thereby the difference in the predicted land mark position can be reduced.

[0086] Note that the order of execution of correction by rotation and correction by parallel movement and the order of execution of correction using one land mark and correction using plural land marks is not fixed, but any one of the correction processings may be performed first. However, the type of correction processing performed next must be different. For example, after the correction by parallel movement of camera, the correction by rotation of camera is performed. If the same type of correction processings are continuously performed, such as continuous execution of correction by parallel movement utilizing plural land marks by averaging and correction by parallel movement utilizing one land mark, the subsequent correction cancels the result of correction by the immediately prior correction, and the result is the same as that of the latter correction.

[0087] Further, the above-described 4 correction processings (1-1, 1-2, 2-1 and 2-2) may be sequentially repeated several times. Note that as described above, at this time, the same type of correction processings are not continuously performed (1-1 and 2-1 processings must not be continuously performed, and 1-2 and 2-2 processings must not be continuously performed). Further, at this time, in addition to the repetition of the processings predetermined number of times, the repetition may be determined based on the difference between a predicted land mark position on the image-sensing plane predicted from the position and orientation of camera and the position obtained by actual image sensing. For example, the processings may be repeated until the difference becomes equal to or less than a predetermined threshold value, or may be repeated until the change amount of difference becomes a predetermined value. Otherwise, the combination thereof (e.g. the processings are repeated until the difference becomes equal to or less than the threshold value or the change amount of difference becomes equal to or less than the threshold value) may be employed.

[0088] In this manner, the final correction matrix ΔM′_c is obtained by multiplying the ΔM_c obtained by any of the above methods by the rotation correction matrix ΔM_rc with the straight line parallel to the visual axis as its axis.

<4. Example of Application of Correction to Combined Reality Producing Apparatus>

[0089] FIG. 8 is a block diagram showing the schematic construction of combined reality producing apparatus for combining an image of actual space with an image of nonrealistic space such as computer graphics, to which the above correction processing of the present embodiment is applied. Numeral 801 denotes a head mount display (HMD) having a three-dimensional position and orientation sensor 801a which outputs a signal based on the position and orientation of the HMD 801 in three-dimensional space to a position and orientation measurement unit 802 to be described later, a camera 801b which performs image sensing on actual space, and a display unit 801c which provides a video image to a user who wears the HMD 801 on the head. Note that the HMD 801 of the present embodiment is a video see-through type display and the position and orientation of the HMD 801 means the position and orientation of the camera 801b.

[0090] The position and orientation measurement unit 802 generates a matrix indicating the position and orientation of the HMD 801 (viewing transformation matrix) based on the signal outputted from the three-dimensional position and orientation sensor 801a. Numeral 803 denotes a land mark detection unit which detects a land mark from the video image obtained by image sensing by the camera 801b and detects the position of land mark. The method for detection is not particularly limited, but detection of land mark and its position can be made by using a marker having a particular color and detecting the particular color in the video image obtained by the camera 801b.

[0091] Numeral 804 denotes a position and orientation correction unit which calculates the correction matrix ΔM′_c by using the correction processing (<Method 1>, <Method 2>, <Method 3>) including the above-described correction by rotation about the visual axis, based on position information of respective land marks outputted from the land mark detection unit 803, multiplies the matrix indicating the position and orientation of the HMD 801 outputted from the position and orientation measurement unit 802 by the correction matrix ΔM′_c, thus corrects the position and orientation (external parameter) of the HMD 801. Numeral 805 denotes an image generation unit which generates a video image of virtual object based on the matrix corrected by the position and orientation correction unit 804, combines the video image of virtual object with the video image of actual space inputted from the camera 801b, and generates a combined reality video image (image). Note that data on the virtual object is stored in an external memory. The generated image is outputted to the display unit 801c.

[0092] The position and orientation measurement unit 802, the land mark detection unit 803, the position and orientation correction unit 804 and the image generation unit 805 constructs a controller of the HMD 801. The controller may be installed in the HMD 801 or provided as another device connected to the HMD 801 by cable or wireless communication.

[0093] The processing by the controller having the above construction will be described with reference to the flowchart of FIG. 9. Further, program code according to the flowchart of FIG. 9 is stored in a memory (not shown) such as a RAM or ROM in the controller according to the present embodiment, and read and executed by a CPU (not shown).

[0094] First, the signal indicating the position and orientation of the HMD 801 from the three-dimension position and orientation sensor 801a is inputted into the position and orientation measurement unit 802 (step S901), and the position and orientation measurement unit 802 generates the viewing transformation matrix indicating the position and orientation of the HMD 801 based on the input signal (step S902). On the other hand, a video image of actual space is obtained by image sensing by the camera 801b (step S903), and the obtained video image is inputted into the land mark detection unit 803, and a land mark and its position are detected (step S904). The position and orientation correction unit 804 generates the correction matrix ΔM′_c by the above-described method (<Method 1>, <Method 2>, <Method 3>) by using the detected land mark position (step S905).

[0095] The position and orientation correction unit 804 corrects the position and orientation of the HMD 801 by using the viewing transformation matrix indicating the position and orientation of the HMD 801 and the correction matrix ΔM′_c, generated by the above respective processings (step S906). The image generation unit 805 generates a video image of virtual object based on the external parameter indicating the corrected position and orientation of the HMD 801, and generates a combined reality video image (step S907). Then, the generated combined reality video image is outputted to the display unit 801c (step S908), and displayed by the display unit 801c (step S909).

[0096] As described above, according to the present embodiment, the parameter indicating the position and orientation of camera, measured by the method other than utilization of video image obtained by image sensing of actual space by an image sensing apparatus (e.g., by using a three-dimensional position and orientation sensor), is corrected by utilizing correction method by rotation of camera in pan and/or tilt direction and/or parallel movement of camera, using land mark(s) obtained by image sensing, then, correction to eliminate the difference regarding the visual axis is performed. Accordingly, correction with higher reliability can be performed on the parameter indicating the position and orientation of camera. Further, any form of correction can be utilized as long as it is used for the purpose of determination of position and orientation of image sensing apparatus.

[0097] Accordingly, according to the correction processing apparatus and method of the present embodiment, even if the accuracy of measurement of the position and orientation of the HMD 801 by the three-dimensional position and orientation sensor 801a is not sufficient, the position and orientation of the HMD 801 can be corrected with high accuracy.

[0098] Further, even though the video image of actual space obtained by image sensing by the camera 802b in the HMD 801, and the video image of virtual object generated based on the position and orientation of the HMD 801 are combined, a positional shift therebetween can be prevented.

[0099] Further, as the correction of the position and orientation of the HMD 801 is performed by using all the land marks on the image-sensing plane, the correction values can be prevented from greatly varying due to change of visual field of the camera.

[0100] The object of the present invention can be also achieved by providing a storage medium (or recording medium) holding software program code for performing the aforesaid functions according to the embodiment to a system or an apparatus, reading the program code with a computer (e.g., CPU, MPU) of the system or apparatus from the storage medium, then executing the program. In this case, the program code read from the storage medium realizes the functions according to the embodiment, and the storage medium holding the program code constitutes the invention. Furthermore, besides aforesaid functions according to the above embodiment are realized by executing the program code which is read by a computer, the present invention includes a case where an OS (operating system) or the like working on the computer performs a part or entire actual processing in accordance with designations of the program code and realizes functions according to the above embodiment.

[0101] Furthermore, the present invention also includes a case where, after the program code is written in a function expansion card which is inserted into the computer or in a memory provided in a function expansion unit which is connected to the computer, a CPU or the like contained in the function expansion card or unit performs a part or entire actual processing in accordance with designations of the program code and realizes the functions of the above embodiment.

[0102] As described above, according to the present invention, the position and orientation of image sensing means can be detected with high accuracy.

[0103] As many apparently widely different embodiments of the present invention can be made without departing from the spirit and scope thereof, it is to be understood that the invention is not limited to the specific embodiments thereof except as defined in the appended claims.