JP2007048068A - Information processing method and device - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To accelerate a bundle adjustment calculation. <P>SOLUTION: Information about image coordinates of an index detected from an image obtained by photographing the index is acquired, an initial value of the arrangement information of the index is set, and an observation equation about the information about the image coordinates of the index, a position posture of an imaging device when photographing the image and the initial value of the arrangement information of the index is solved to thereby calculate the arrangement information of the index. When solving the observation equation, an unknown parameter in the observation equation is divided into three or more parameters, and the divided unknown parameters are calculated in stages. <P>COPYRIGHT: (C)2007,JPO&INPIT

Description

  The present invention relates to a method for calculating index arrangement information using an image obtained by photographing an index.

  In recent years, research on mixed reality (MR) has been actively conducted for the purpose of seamless fusion of real space and virtual space. Among MR technologies, Augmented Reality (also referred to as AR, augmented reality, augmented reality) technology that displays a virtual space superimposed on a real space is attracting particular attention. An image display device used in the AR system is mainly realized by a video see-through method. The display image is a virtual space generated in accordance with the position and orientation of the imaging device (for example, a virtual object or character information drawn by computer graphics) on a real space image taken by an imaging device such as a video camera. ) Is superimposed and drawn.

  One of the most important issues in AR technology is how to accurately align the real space and the virtual space, and many efforts have been made so far. The problem of alignment in AR results in a problem of obtaining the position and orientation of the imaging device with respect to a target on which virtual information is to be superimposed.

  A plurality of methods have been proposed as a method for solving this problem.

  The following methods are used in the fields of computer vision and photogrammetry. An index whose arrangement information in the target coordinate system (hereinafter referred to as a reference coordinate system) is known is set or set in the environment or on the target object. Then, using the correspondence between the three-dimensional coordinates of the index and the two-dimensional coordinates of the projected image of the index in the image captured by the imaging apparatus, the position and orientation of the imaging apparatus with respect to the superimposition target are obtained (Non-Patent Document 1).

  In addition, a method has been proposed in which a 6-DOF position / orientation sensor such as a magnetic sensor is attached to the imaging apparatus, and the position / orientation of the imaging apparatus obtained from the sensor is corrected based on image information of an index. (Non-patent document 2).

  In the alignment method of the real space and the virtual space using the index, information of the index is necessary to obtain the position and orientation in the reference coordinate system of the imaging device that is the measurement target.

  In the case of a point index as shown in FIG. 6 (A), a position in the reference coordinate system is required, and in the case of a two-dimensional shape index (for example, a square index) as shown in FIG. 5 (A), in the reference coordinate system. Position and posture are required. When a square index is used, the square index itself can be used as a reference for the coordinate system without providing a reference coordinate system separately. However, when a plurality of square indices are used, a reference coordinate system is necessary because the relationship between the positions and orientations of each other must be known.

  The position and orientation of the index can be measured manually using a tape measure or a protractor, or by a surveying instrument. However, since this method takes time and effort and has a problem with accuracy, measurement using an image has been proposed.

  The position measurement of the point index can be obtained by a method called a bundle adjustment method. The bundle adjustment method is a method for obtaining the position and orientation of the photographing apparatus and the position of the point index using the photographed images of a plurality of point indices. The projection position of the index in the captured image is detected. Next, the projection position is estimated from the position and orientation of the imaging apparatus and the position of the index. Then, the position and orientation of the imaging device that captured the image by iterative calculation and the position of the point index so that the sum of errors (projection errors) between the detected projection position and the estimated projection position is minimized. Ask for.

  Non-Patent Document 3 describes a method for measuring the position and orientation of a number of square markers arranged in a three-dimensional space. The method described in Non-Patent Document 3 captures a large number of images of a large number of square markers arranged in a three-dimensional space. Then, the position and orientation of the imaging device that captured each image and the position and orientation of the square marker are obtained by repeatedly performing calculation so that the total sum of projection errors is minimized.

  In Non-Patent Document 4, a priori knowledge about index placement is used as a constraint condition in the bundle adjustment method in order to obtain index placement information with high accuracy. For example, when it is known in advance that the index is arranged on the plane, the index is constrained on the plane, and the position of the index on the plane and the equation of the plane are obtained by the bundle adjustment method.

Also, a method for efficiently calculating the bundle adjustment method has been devised (Non-Patent Document 5, Non-Patent Document 6). These focus on the fact that a sparse matrix containing many zeros appears in a certain pattern in the Jacobian matrix used in the calculation of the bundle adjustment method. In these methods, the position and orientation of the imaging device and the position of the measurement point are divided and solved, thereby eliminating unnecessary calculations and improving efficiency.
R. M.M. Haralick, C.I. Lee, K.M. Ottenberg, and M.M. Nolle: “Review and analysis of solutions of the three point perspective probe,” International Journal of Computer Vision, Vol. 13, no. 3, pp. 331-356, 1994. Shinji Uchiyama, Hiroyuki Yamamoto, Hideyuki Tamura: “Hybrid registration method for mixed reality: combined use of 6-DOF sensor and vision method”, Transactions of the Virtual Reality Society of Japan, vol. 8, no. 1, pp. 119-126, 2003. G. Baratoff, A.M. Neubeck and H.M. Regenbrecht: “Interactive multi-marker calibration for augmented reality applications”, Proc. ISMAL2002, pp. 107-116, 2002. D. Kotake, S .; Uchiyama, and H.K. Yamamoto: “A marker calibration method a priori knowledge on marker arrangement,” Proc. 3rd IEEE / ACM Int'l Symp. on Mixed and Augmented Reality (ISMAR 2004), pp. 89-98, November 2004. W. Triggs, P.M. McLauchlan, R.M. Hartley, and A.M. Fitzgibbon. "Bundle Adjustment: A Modern Synthesis," In Vision Algorithms: Theory and Practice, number 1883 in LNCS, pp. 298373. Springer-Verlag, Corfu, Greece, September 1999. O. Faugeras, and QT. Luong, The Geometry of Multiple Images, The MIT Press, 2001.

  The speed-up method of the bundle adjustment method described in the prior art targets the position and orientation of the imaging device and the position of the measurement point as unknown parameters. Therefore, when a point index and a square index are mixed as an index to be measured, the conventional method for speeding up the bundle adjustment method cannot be applied as it is. Also, as shown in Non-Patent Document 6, the unknown parameter is different when the index arrangement information is constrained. Therefore, the conventional method for speeding up the bundle adjustment method is also applied to the case shown in Non-Patent Document 6. It is not possible.

  As described above, conventionally, the bundle adjustment calculation when the unknown parameter is not the position and orientation of the imaging apparatus and the position of the measurement point cannot be performed at high speed.

  The present invention has been made in view of the above, and an object of the present invention is to speed up the bundle adjustment calculation even in the case of an unknown parameter to which the conventional method of speeding up the bundle adjustment method cannot be applied.

  In order to achieve the above object, the present invention has the following structure.

The invention according to claim 1 is an information processing method for measuring the arrangement information of an index, and obtains information related to image coordinates of the index detected from an image obtained by photographing the index;
An observation equation for a setting step for setting an initial value of the marker placement information, information regarding the image coordinate of the marker, a position and orientation of the imaging device at the time of shooting the image, and an initial value of the marker placement information A calculation step of calculating the placement information of the index by solving
The arrangement information calculating step divides the unknown parameter in the observation equation into three or more, and calculates the divided unknown parameter stepwise.

  The invention according to claim 9 is an acquisition step of acquiring an image obtained by photographing an index, an index image coordinate acquisition step of detecting the index from the image, and acquiring information on image coordinates of the detected index, An index information acquisition step for acquiring index information regarding the index, a setting step for setting an observation equation based on the image information of the index and the index information, and a method for dividing an unknown parameter in the observation equation based on the index information A division method setting step; and a calculation step of dividing the unknown parameter of the observation equation based on the set division method and calculating the divided unknown parameter stepwise.

  According to the present invention, the bundle adjustment method can be speeded up.

  According to the first aspect of the present invention, the bundle adjustment calculation can be speeded up even in the case of an unknown parameter to which the conventional method for speeding up the bundle adjustment method cannot be applied.

  According to the ninth aspect of the invention, the bundle adjustment method can be speeded up using a processing method according to the type of unknown parameter.

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

  The present case relates to a method of calculating index placement information using a captured image obtained by shooting an index. In this case, the unknown parameter in the observation equation is divided according to the type of the unknown parameter (presence / absence of constraint condition, point marker, two-dimensional shape marker, etc.), and the divided unknown parameter is calculated step by step.

  Hereinafter, an embodiment in each of a plurality of cases where the types of unknown parameters are different will be described.

[First embodiment]
This embodiment speeds up the bundle adjustment calculation when measuring the placement information of an index having placement information parameters with different degrees of freedom such as a point index and a square index. Hereinafter, the index arrangement information measuring apparatus according to the present embodiment will be described in detail.

  FIG. 1 shows a schematic configuration of an index arrangement information measuring apparatus 3000 according to the present embodiment. As shown in FIG. 1, the index arrangement information measuring device 3000 includes an image input unit 3020, an index detection unit 3030, an imaging device approximate position and orientation input unit 3040, an approximate index position and orientation input unit 3050, a data management unit 3060, and a position and orientation. The calculation unit 3070 is configured. An imaging device 3010 is connected to the arrangement information measuring device 3000.

As shown in FIG. 1, a plurality of indices photographed by the imaging device 3010 are arranged in the environment or on the target object. In the present embodiment, a two-dimensional shape index as shown in FIG. 5A is represented by P ^k (k = 1,..., K _o ), and the feature points in the image shown in FIG. A point index indicated by a point is represented by Q ^j (j = 1,..., J ₀ ). However, K _o is the number of arranged two-dimensional shape indices (K _o = 3 in the example of FIG. 1), and J ₀ is the number of feature points or point indices arranged (J ₀ = in the example of FIG. 1). 2). Further, as shown in FIG. 5B, the two-dimensional shape index P ^k is composed of vertices p ^ki (i = 1,, N _k ). However, N _k represents the total number of vertices constituting the index P ^k (N _k = 4 in this embodiment).

  In the following, a coordinate system (here, a coordinate system in which one point in the environment or on the target object 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) is used as a measurement reference. This is called a reference coordinate system, and the position or orientation of each index in the reference coordinate system is a measurement target.

The indices P ^k and Q ^j can detect the screen coordinates of the projected image on the photographed image, and identify which index (in the case of a two-dimensional shape index, each vertex constituting the index). Any form may be used as long as it is possible.

For example, the point index Q ^j is an index having centroid points of different color regions as shown in FIG. 6B, but other forms can be used as long as they can be identified by feature points having feature quantities observed in the image. It may be.

  A square index as shown in FIG. 5B has a pattern representing an identifier inside and can be uniquely identified. When such an index is detected, a binarization process is performed on the captured image and then a labeling process is performed, and an area formed by four straight lines is extracted as an index candidate from a region having a certain area or more. Further, it is determined whether or not the candidate area is an index area by determining whether or not there is a specific pattern in the candidate area, and the direction and identifier of the index are acquired by reading the internal pattern.

  An image captured by the imaging device 3010 is transmitted to the index detection unit 3030 via the image input unit 3020.

The index detection unit 3030 detects the image coordinates of each vertex p ^{ki and} the image coordinates of the point index Q ^j constituting the two-dimensional shape index P ^k photographed from the image acquired via the image input unit 3020.

Furthermore, the index detection unit 3030 identifies each detected two-dimensional shape index P ^kn . Then, 2-dimensional shape index identifier _{k n} and the image coordinates ^{u Pkni} of each vertex ^{p kni,} and the image coordinates ^{u QKA} identifier _{k a} and the point indicators point indicator, and outputs it to the data management unit 3060. Here, n (n = 1,..., N) is an index for each detected two-dimensional shape index, and N represents the total number of detected indices. Further, the total number of vertices of the N indices detected in the image is N _Total .

For example, the case of FIG. 1 shows a case where a square-shaped index having identifiers 1, 2, and 3 is captured, N = 3, identifiers k ₁ = 1, k ₂ = 2 and k _3. = 3 and the corresponding image coordinates u ^Pk1i , u ^Pk2i , u ^Pk3i , (i = 1, 2, 3, 4) are output. N _Total is 12 (= 3 × 4). Further, a (a = 1,..., A) is an index for each detected point index, and A represents the total number of detected point indices.

Here, if a three-dimensional vector representing the origin position of a three-dimensional coordinate system A with respect to a certain three-dimensional coordinate system B is t, and a 3 × 3 rotation matrix representing a posture is R, the position on _A is x _A (third order A coordinate x _B (three-dimensional vector) on the point B represented by (original vector) is represented by the following equation.

  In this embodiment, a three-dimensional vector t representing a position and a 3 × 3 rotation matrix R representing a posture are used as means for representing the position and posture of the coordinate system A with respect to the coordinate system B.

  The imaging device approximate position / orientation input unit 3040 obtains approximate positions and orientations t and R of the imaging device 3010 in the reference coordinate system at the time of image capture, and outputs these to the data management unit 3060.

  Here, a method for obtaining the approximate position and orientation of the imaging apparatus will be briefly described. The approximate position and orientation of the imaging device are obtained using an index whose position defining the reference coordinate system is known and image information of an index whose approximate position and orientation are known. When the marker detected in the image is not on the same plane, the position and orientation of the imaging apparatus are obtained by a DLT (Direct Linear Transformation) method, and when the marker is on the same plane, the method by plane homography is used. A method for obtaining the position and orientation of the imaging device from the correspondence between the image coordinates of a plurality of points and the reference coordinates (generally referred to as the PnP problem) is widely known in the field of photogrammetry and computer vision, and will not be described in detail. .

  Note that the method for acquiring the approximate position and orientation is not limited to this. For example, a 6-DOF position and orientation sensor may be attached to the imaging apparatus and the output value may be used as the approximate position and orientation of the imaging apparatus. You may obtain | require using the image information of the attitude | position of an imaging device and an index | index obtained by attaching a freedom degree attitude | position sensor to an imaging device. Alternatively, the approximate position and orientation of the imaging apparatus may be obtained using other techniques.

The index approximate position and orientation input unit 3050, the approximate position and orientation _R s approximate position _{t pn} and two-dimensional shape index of the point indicators, determine the _{t s,} and outputs it to the data management unit 3060. Coarse position and orientation R _s approximate position t _pn and two-dimensional shape index of the point _indicators, t _s, it is possible to use the value of the outline measured manually with a tape measure and protractor, a surveying instrument, the present embodiment A value estimated once by the above method may be used. The position of the point index and the position and orientation of the two-dimensional shape index input by the index approximate position and orientation input unit 3050 are merely initial values and are optimized by the method of the present embodiment. Therefore, unlike the prior art, there is no need to obtain the position of the point index and the position and orientation of the two-dimensional shape index with high accuracy. Therefore, even if it measures manually, a user's load can be reduced compared with the past.

The position / orientation calculation unit 3070 receives, from the data management unit 3060, the approximate position / orientation of the imaging device in all captured image frames, the identifier of each two-dimensional shape index detected in the same frame, the image coordinates of each vertex, and the reference The approximate position in the coordinate system, the identifier of each point index, and the approximate position in the reference coordinate system are input. Then, the type of the unknown parameter is determined from the input information. Based on this information, an observation equation is created, and the index position / orientation calculation process is performed. As a result, the position / orientation information of the two-dimensional shape index P ^{kn and} the position information of the point index Q ^ka are output.

  Next, details of an index arrangement information measuring method in the position and orientation calculation unit 3070 will be described. Based on the detection and identification result of the index, the position and orientation of the index in the reference coordinate system are calculated.

First, perspective projection conversion will be described. FIG. 8 is a diagram for explaining the camera coordinate system and the screen coordinate system. The intersection of the visual axis and the image plane is the origin o _i in the screen coordinate system, x _i axis in the horizontal direction of the image, the vertical direction and y _i axes. The distance between the origin o _c and the image plane of the camera coordinate system (focal length) is f, the opposite direction of the z _c axis of the camera coordinate system is the line of sight vector, x _c-axis parallel to the horizontal direction of the image, y _{The c} axis is parallel to the vertical direction of the image.

By perspective projection transformation, a point x _c = [x _c y _c z _c ] ^t on the camera coordinate system is projected to a point whose screen coordinates are u = [u _x u _y ] ^t as shown in the following equation.

  Here, it is assumed that the lens distortion does not exist or is corrected, and the imaging apparatus is assumed to be a pinhole camera. As shown in FIG. 9, the expression (2) indicates that a point in space, a projection point on the image of the point, and the imaging device position (viewpoint) exist on the same straight line. Also called a formula.

The position of the imaging apparatus in the reference coordinate system _{t = [t x t y t} z] t, ( in fact, the attitude of the reference coordinate system relative to the camera coordinate system) and orientation of the imaging apparatus _{ω = [ω x ω y ω} z ]. ω is a posture expression method with three degrees of freedom, and the posture is expressed by a rotation axis vector and a rotation angle. If the rotation angle is r _a , r _a is expressed by ω as follows:

When the rotation axis vector is r _axis = [r _x r _y r _z ] ^t , the relationship between r _axis and ω is expressed as the following equation.
_{[Ω x ω y ω z]} = [r a r x r a r y r a r z] ··· (4)
The relationship between ω (rotation angle r _a , rotation axis vector r _axis ) and 3 × 3 rotation transformation matrix R is expressed by the following equation.

_{X w = [x w y w} z w] camera coordinate _{x c} in ^t points on the reference coordinate system is expressed as follows by t and R.

From the expressions (2) and (6), the point x _w = [x _w y _w z _w ] ^t on the reference coordinate is converted to the point u = [u _x u _y ] on the image by the perspective projection transformation as in the following expression. projected onto ^t .

Ideally, t, ω, projection position, which is calculated from equation (7) on the basis of the x _w (calculated position) and actually observed the position (observation position) is consistent. Therefore, if the horizontal displacement between the calculation position and the observation position image is F, the vertical displacement is G, and the observation position is u _o = [u _ox u _oy ] ^t , then F and G are 0.

F, G is a function related to the position x _w on the reference coordinates of the imaging apparatus position t, the imaging device orientation omega, viewed point.

_Assuming that the position of the point index in the reference coordinate system is t _pn = [t _pnx t _pny t _pnz ] ^t , x _w and t _pn are the same as shown in the following equation.

The square index is a 3 × 3 rotation transformation matrix corresponding to a position t _s = [t _sx t _sy t _sz ] ^{t in the} reference coordinate system and an attitude ω _s = [ω _sx ω _sy ω _sz ] (ω _s ) with respect to the reference coordinate system. It is represented by a and _{R s).} The position in the square marker coordinate system of the vertices of the square indices and _{x s = [x s y s} 0] t, and each vertex of the square marker is on _x s -y _s plan in square marker coordinate system.

Position x _w in the reference coordinate system of the vertices of the square indices becomes t _s and ω _{s (R s)} about functions as follows.

Therefore, as shown in the following equation, F, G, the imaging device location t, the imaging device orientation omega, the position t _p in the reference coordinate system of the point _indicators, position t _s and square markers square markers posture omega _s It becomes a function.

  Expression (11) is a nonlinear equation regarding the position and orientation of the imaging apparatus and the position and orientation of the index. Therefore, using Taylor expansion up to the first order term, linearization is performed in the vicinity of the approximate values of the position and orientation of the imaging apparatus and the index, and the position and orientation of the imaging apparatus and the position and orientation of the index are obtained by repeated calculation.

Equation (12) is a linearization of equation (11). Δt _x , Δt _y , Δt _z are the positions of the imaging device, Δω _x , Δω _y , Δω _z are the postures of the imaging device, Δt _px , Δt _py , Δt _pz are the positions of the point indicators , Δt _sx , Δt _sy , and Δt _sz represent the position of the square marker, and Δω _sx , Δω _sy , and Δω _sz represent the correction amount for the approximate value of the posture of the square marker.

Here, F ⁰ and G ⁰ in Expression (12) are constants, and the projected position (calculated position) of the index and the observation position u obtained from the approximate values of the position and orientation of the imaging device, the position of the point index, or the position and orientation of the square index. _It is the difference from _o .

  Equation (12) is an observation equation for one vertex or one vertex of a square marker observed on a certain image. Actually, a plurality of point indices and square indices are observed on a plurality of images, and thus a plurality of expressions (12) are obtained. Therefore, by solving a plurality of equations (12) as simultaneous equations for correction values for the approximate values of the position and orientation of the imaging device, the position of the point index, and the position and orientation of the square index, and correcting the approximate value, the imaging device is repeated. Position, orientation of the point index, and position and orientation of the square index. FIG. 2 is a flowchart illustrating a procedure for obtaining the position and orientation of the imaging apparatus, the position of the point index, and the position and orientation of the square index in the present embodiment. Note that it is assumed that shooting of a scene including the index and extraction and identification of the index from the captured image have been completed.

  Step S5010 sets the approximate values of the position and orientation of the imaging device that captured each image, the position of the point index, and the position and orientation of the square index. In step S5020 and subsequent steps, a correction value for correcting the approximate value set in step S5010 is obtained. In step S5020, a projection error and a partial differential coefficient (image Jacobian) for each unknown parameter are obtained from the approximate values of the position and orientation of the imaging apparatus, the position of the point index, and the position and orientation of the square index. In step S5030, a coefficient matrix necessary for obtaining the arrangement information parameter is calculated. In step S5040, backward substitution is performed using the calculated coefficient matrix, and a correction value for each unknown parameter is obtained. In step S5050, the approximate value is corrected by the correction value obtained in step S5040 to obtain a new approximate value. In step S5060, it is determined whether the position and orientation of the imaging apparatus corrected in step S5050, the position of the point index, and the position and orientation of the square index have converged. In step S5070, index arrangement information is output.

  Hereinafter, each step will be described in detail.

In step S5010, the approximate values of the position and orientation of the imaging device that captured each image, the position of the point index, and the position and orientation of the square index are input. Here, the number of the captured images is N sheets, the position of the _t i ₌ the imaging device taken each image ^{_{[t ix t iy t iz]}} t (i = 1, ···, N), posture Ω _i = [ω _ix ω _iy ω _iz ] (i = 1,..., N). Further, the number of points index and _p1 pieces _K, the position of the point indicator _{t pi = [t pix t piy} t piz] t (i = 1, ···, K p1) and. Further, the number of square indices is K _s1, the positions of the square indices are t _si = [t _six t _siy t _siz ] ^t (i = 1,..., K _s1 ), and the posture is ω _si = _{[ω six ω siy ω siz]} (i = 1, ···, K s1) and.

  Next, in step S5020, partial differential coefficients and residuals for unknown parameters of index image coordinates are obtained. Further, the residual is obtained from the difference between the projected position of each index calculated from the approximate values of the position and orientation of the imaging device, the position of the point index, and the position and orientation of the square index and the actual observation position on the image.

The observation equation of Expression (12) holds as many as the number of indices observed on the image (the number of vertices in the case of a square index). Expression (12) is an observation equation that includes a position and orientation correction value of one imaging apparatus and a position index correction value of one point index or a position and orientation correction value of one square index as unknown parameters. Here, as shown in Expression (13), the observation equations for the position and orientation of the N imaging devices, the position of K _p1 point indices, and the position and orientation of K _s1 square indices are established. In this case, the number of correction values to be unknown is (6 × N + 3 × K _p1 + 6 × K _s1 ).

When the number of point indices detected from the image i (i = 1,..., N) is d _{pi and the} number of square indices is d _si , the number D of point indices detected from N images. _p and Ds of the square index are as follows.

When the number of point indices detected from N images is D _p and the number of square indices is D _s , the observation equation (13) is (D _p + 4 × D _s ), that is, 2 × ( D _p + 4 × D _s ) This observation equation stands. When the constant terms F ⁰ and G ⁰ on the left side of the observation equation of Equation (13) are moved to the right side and a simultaneous equation is established, this simultaneous equation can be written in matrix form as the following equation.
J · △ = E (15)
J is called a Jacobian matrix, and is an array of partial differential coefficients for F and G imaging device positions and orientations, point index positions, and square index positions and orientations. The number of rows of the Jacobian matrix J is the same as the number of observation equations (2 × (D _p + 4 × D _s )), and the number of columns is the same as the number of unknowns (6 × N + 3 × K _p1 + 6 × K _s1 ). is there. Δ is a correction value vector. The number of elements of the correction value vector is the same as the number of unknowns (6 × N + 3 × K _p1 + 6 × K _s1 ). E is an error vector (residual), and has the difference −F ₀ , −G ₀ between the calculation position of the projection position based on the approximate value and the observation position u _o . The number of elements of E is the same as the number of observation equations (2 × (D _p + 4 × D _s )).

  Note that the origin, scale, and orientation of the reference coordinate system can be explicitly specified by simultaneously photographing a point index whose position in the reference coordinate system is known or a square index whose position and orientation are known. In the equation (13) for these indices, the positions of the indices and the partial differential coefficients for the indices are all zero. In order to explicitly specify the origin, scale, and orientation of the reference coordinate system, it is sufficient to use three point indicators whose positions are known if they are point indicators, and square indicators whose positions and orientations are known if they are square indicators. One may be used.

When the Jacobian matrix J in the equation (15) is not a square matrix, the transpose matrix of the Jacobian matrix J is applied to both sides as shown in the equation (16), and the correction value vector Δ is obtained using the least square method. it can.
(J ^t · J) Δ = J ^t · E (16)
That is, a correction value can be obtained by creating a square matrix J ^T J by applying the transposed matrix of the Jacobian matrix J, and obtaining the inverse matrix (J ^T J) ⁻¹ and applying it to both sides.

Since the matrix J ^T J increases in size according to the number of unknown parameters, the calculation of the inverse matrix generally requires a large calculation load. On the other hand, the Jacobian matrix J is a sparse matrix including many zeros. Therefore, rather than obtaining the inverse matrix (J ^T J) ⁻¹ and directly obtaining the correction value vector Δ, the calculation is efficiently performed by dividing the unknown parameter using the structure of this sparse matrix. be able to.

  FIG. 14 shows the structure of the Jacobian matrix. FIG. 14 shows the structure of the Jacobian matrix when three images are taken so that three point indicators and two square indicators are taken. The white portion of the figure indicates that 0 is always included, and the black portion indicates a portion where 0 is not necessarily included.

FIG. 15 shows the structure of a matrix (J ^T J) obtained by transposing the Jacobian matrix. In FIG. 15A, the white part always contains 0, and the sub-matrix shown in black is a square matrix located at the diagonal of the matrix (J ^T J). The sizes are equal (represented by (D _i × D _i )). Here, it is not necessary to calculate the white portion, that is, the portion where the element is always 0. The shaded area is an area where an element other than 0 may enter.

In step S5030, a coefficient matrix for obtaining unknown parameters step by step is obtained. Here, the arrangement information parameters are collected for each type, and the position and orientation of the N imaging devices, the position of K _p1 point indices, and the position and orientation of K _s1 square indices are obtained separately. The correction value vector Δ is divided into a position / orientation correction value vector Δ _c , a point index position correction value vector Δ _p , and a square index position / orientation correction value vector Δ _{s as} in Expression (17). Is done. When the matrix J ^t J of Expression (16) is divided for each arrangement information parameter and the right side is collected, the following expression is obtained.

  Here, the divided coefficient matrix will be described.

U = α ^T α, which is a matrix obtained by multiplying the Jacobian matrix α related to the position and orientation of the imaging apparatus and its transposition α ^T. V = β ^T β, which is a matrix obtained by multiplying the Jacobian matrix β related to the position of the point index and its transposition β ^T. T = γ ^T γ, which is a matrix obtained by multiplying the Jacobian matrix γ related to the position and orientation of the square index and its transposition γ ^T. Also, W ₁ = α ^T β and W ₂ = α ^T γ.

Since the position of the point index and the position and orientation of the plane index are independent from each other, X = β ^T γ = 0. Furthermore, α ^T α, β ^T β, and γ ^T γ can be obtained by calculating only the sub-matrix components of α, β, and γ.

  Therefore, the method of this embodiment can reduce the memory secured when performing calculations in the computer, as compared with the case of performing calculations as usual.

That is, when sub-matrices on the diagonal of U, V, and T are U _i , V _j , and T _k , respectively, the following equations are obtained.
U _i = α _ii ^T α _ii (18)

The α, β, and γ sub-matrices are represented by α _ii , β _ij , and γ _jk , respectively. N represents the number of captured images.

Accordingly, when obtaining the J ^T J matrix that is the coefficient matrix of the unknown parameter, only U, V, T, W ₁ , and W ₂ need be obtained. That is, the following equation is obtained.

The J ^t · J matrix shown in FIG. 15A can be expressed as shown in FIG.

  In step S5040, correction values are calculated stepwise by backward substitution.

Here, the matrix size of U is (6N × 6N), the matrix size of V is (3D _p × 3D _p ), and the matrix size of T is (6D _s × 6D _s ). In order to obtain Δ, an inverse matrix is calculated, but when all Δ are solved simultaneously without performing the above division, the size of ((6N + 3D _p + 6D _s ) × (6N + 3D _p + 6D _s )) It is necessary to find the inverse matrix of. However, by solving divided in inverse matrix calculation for obtaining the correction value, △ _S is _{(6D s} × 6D _s), the _{△ P (3D p × 3D p} ), △ C is (6N × 6N), The inverse matrix reduced to the size of

Also, U, V, such as T, then the sub-matrix is a square matrix is in diagonal, and shows the shape of a matrix of N ₁₁ the following equation of equal size matrix of each sub-matrix as N _11.

An inverse matrix operation of a matrix in which a sub-matrix such as N ₁₁ is diagonal and the size of each sub-matrix is equal to (D × D) is the inverse of (Dn × Dn) as in the following equation: The problem of obtaining a matrix operation can be reduced to a problem of obtaining n (D × D) inverse matrix operations.

  In this way, it is possible to speed up the inverse matrix operation of a matrix in which the sub-matrix that is a square matrix is diagonal and the size of each sub-matrix is equal (D × D). As described above, according to the present embodiment, the calculation can be speeded up using the structure of the sparse matrix.

  In step S5050, the correction value Δ for each approximate value obtained in step S5040, that is, the correction value for the approximate value of the position and orientation of the imaging device, the position of the point index, and the position and orientation of the square index is added to the approximate value. Is a new approximate value.

  Next, in step S5060, a convergence determination is performed. In the convergence judgment, the absolute value of the correction value is below the specified threshold, the sum of the difference between the calculated position of the projected position and the observation position or the average value is below the specified threshold, the calculated position of the projected position and the observed position This is performed by determining a condition such that the sum of differences is minimized. If it is determined in this step that convergence has been achieved, the marker placement information measurement is terminated. On the other hand, if it is determined that it has not converged, the process returns to step S5020, and the simultaneous equations are formulated again based on the approximate values of the corrected position and orientation of the imaging device, the position of the point index, and the position and orientation of the square index. To do.

  As described above, in the present embodiment, a large number of scene images in which indices are arranged are captured, and the indices are detected and identified from the captured images. Then, from the index observation position on the image, the correction value for the position and orientation of the imaged image pickup device and the approximate value of the index placement information is divided into different types of placement information parameters and obtained step by step. By repeatedly performing the correction operation, index placement information is obtained.

(Modification 1-1)
In the first embodiment, the positions and orientations of the square indices are obtained on the assumption that the sizes of the two-dimensional shape indices such as the square indices are all known, but a square index of unknown size and a square index of known size are mixed. Even in this case, the calculation of the unknown parameter can be speeded up.

Size unknown position of the square marker correction value _{△ t usx, △ t usy,} △ t usz, the correction value of the orientation of the size unknown square index _{△ ω usx, △ ω usy,} △ ω usz, size unknown square Assuming that the correction value of the index size is Δs, the observation equation regarding one vertex of the square index of unknown size is as follows.

  As in the first embodiment, when the correction value Δ is obtained, it is divided according to the type of arrangement information parameter. Here, the image can be divided according to the position and orientation parameters of the imaging device, the position and orientation parameters (position and orientation) of a square index with a known size, and the position and orientation parameters (position, orientation and size) of a square index with an unknown size. Similar to the first embodiment, the correction information is obtained step by step for each divided arrangement information parameter, and the operation for correcting the approximate value is repeatedly performed, whereby the index arrangement information can be obtained.

[Second Embodiment]
In the marker arrangement information measuring apparatus according to the present embodiment, the bundle adjustment calculation using the constraint condition that the markers are arranged on the same plane is accelerated. Hereinafter, the index arrangement information measuring apparatus according to the present embodiment will be described in detail.

  FIG. 3 shows a schematic configuration of an index arrangement information measuring apparatus 2000 according to the present embodiment. As shown in FIG. 3, the index arrangement information measuring apparatus 2000 according to the present embodiment includes an image input unit 2020, an index detection unit 2030, a constraint condition setting unit 2040, an imaging device approximate position / posture input unit 2050, and an approximate index position input. A unit 2060, a data management unit 2070, and a position / orientation calculation unit 2080. An imaging apparatus 2010 is connected to the arrangement information measuring apparatus 2000.

On the environment or the object, the index Q ^j as shown in FIG. 3 to be photographed in the imaging apparatus 2010 are arranged. These indexes are assumed to be the same as the point indexes described in the first embodiment.

  In the following, a coordinate system used as a reference for measurement (here, a coordinate system in which one point in the environment 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) is referred to as a reference coordinate system. The position of each index in the reference coordinate system is the measurement target.

  An image captured by the imaging apparatus 2010 is input to the arrangement information measuring apparatus 2000 via the image input unit 2020 and transmitted to the index detection unit 2030.

Index detection unit 2030 inputs an image via the image input unit 2020, detects the image coordinates of the point indicators Q ^j which is taken in the input image.

Furthermore, the index detection unit 2030, the image coordinates ^{u QKA} identifier _{k a} and the point indicators point indicator, and outputs it to the data management unit 2070. Here, a (a = 1,..., A) is an index for each detected point index, and A represents the total number of detected point indices.

  The constraint condition setting unit 2040 sets a constraint condition related to the marker placement. In the present embodiment, a constraint condition that a plurality of indexes exist on the same plane is given as a constraint condition related to the marker arrangement. It is assumed that the plane is an unknown plane, and there are a plurality of unknown planes.

  FIG. 7 is a diagram for explaining a GUI (graphical user interface) for setting a plurality of indices existing on the same plane. On the GUI, there is a captured image display portion, and an image captured by the imaging device is displayed. The display of the captured image can be switched by a button in the captured image switching portion. Clicking the Previous button with the mouse displays the previous captured image, and clicking the Next button displays the next captured image. The user sets an index on the same plane while viewing the captured image. On the photographed image, a name unique to each index is displayed around the identified index.

  FIG. 7 shows a case where a captured image in which two point indicators (POINT 1 and POINT 2 in the figure) are captured is displayed in the captured image display portion. It is assumed that the point index POINT1 and the point index POINT2 are on the same plane. The user clicks around the index of either POINT1 or POINT2 in the photographed image with the mouse to select it. The name display of the selected indicator changes to bold. Next, the markers on the same plane are grouped by left-clicking the mouse while pressing the shift key around the markers existing on the same plane as the selected index. If you click on an index that is already grouped as being on the same plane, the name display of all the indices in the same group changes to bold. In this state, another index can be added to the group by left-clicking while holding down the shift key. In addition, when the mouse is right-clicked while holding down the shift key around the grouped index, the group is removed. When the right-click is performed without pressing the shift key, the entire group is released.

In this way, the constraint condition is set for the index, the plane parameter r = [r θ φ] representing the plane in polar coordinate representation, and the coordinate system on the set plane (hereinafter referred to as the plane coordinate system) The point index position x _p = [x _p y _p 0] ^t is output to the data management unit 2070. Z _p axis of the plane coordinate system is parallel to the normal direction of the plane, passing through the origin in the reference coordinate system. Further, the details of the fact that the index on the plane is always z = 0 in the plane coordinate system will be described later.

Here, if a three-dimensional vector representing the origin position of a three-dimensional coordinate system A with respect to a certain three-dimensional coordinate system B is t, and a 3 × 3 rotation matrix representing a posture is R, the position on _A is x _A (third order A coordinate x _B (three-dimensional vector) on a point B represented by (original vector) is expressed as shown in Expression (26).

  As in the first embodiment, a three-dimensional vector t representing a position and a 3 × 3 rotation matrix R representing a posture are used as means for representing the position and posture of the coordinate system A with respect to the coordinate system B. The imaging device approximate position and orientation input unit 2050 obtains approximate positions and orientations t and R of the imaging device 2010 in the reference coordinate system, and outputs them to the data management unit 2070. The approximate position and orientation of the imaging device are obtained in the same manner as in the first embodiment.

The index approximate position / posture input unit 2060 obtains the approximate position t _pn of the point index and outputs it to the data management unit 2070. As the approximate position t _pn of the point index, an approximate value manually measured using a tape measure, a protractor, a surveying instrument, or the like may be used, or a value once estimated by the method of the present embodiment or the like may be used. . If the indices on the same plane are all point indices, the approximate values of the plane parameters are obtained by obtaining the regression plane from the approximate values of the position of the point indices. Furthermore, the user may directly specify the plane parameters without obtaining the approximate position of the index. The approximate value of the position of the index constrained on the plane is obtained by obtaining the plane parameter described above and then converting it to the position in the plane coordinate system.

The position / orientation calculation unit 2080 receives, from the data management unit 2070, the approximate position and orientation of the imaging apparatus in a certain frame, the identifier of each two-dimensional shape index detected in the same frame, the image coordinates of each vertex, and the approximate position in the reference coordinate system. , And the identifier of each point index and the approximate position in the reference coordinate system are input, the observation equation is created based on this, the position and orientation calculation processing of the index is performed, and the resulting two-dimensional shape index P ^kn And the position and orientation information of the point index Q ^ka are output. The position / orientation calculation unit 2080 obtains each unknown parameter in order by dividing the unknown orientation information parameter in the observation equation.

  Next, the details of the index arrangement information measuring method in the position and orientation calculation unit 2080 will be described.

The position of the index in the reference coordinate system is calculated based on the detection and identification result of the index and the constraint conditions regarding the layout of the index. First, perspective projection conversion will be described. FIG. 8 is a diagram for explaining the camera coordinate system and the screen coordinate system. Origin o _i in the screen coordinate system is taken at the intersection of the visual axis and the image plane, take the horizontal direction of the image x _i axis, a direction perpendicular to the y _i axis. The distance between the origin o _c and the image plane of the camera coordinate system (focal length) is f, the opposite direction of the z _c axis of the camera coordinate system is the line of sight vector, x _c-axis parallel to the horizontal direction of the image, y _{The c} axis is parallel to the vertical direction of the image.

By perspective projection transformation, a point x _c = [x _c y _c z _c ] ^t on the camera coordinate system is projected to a point whose screen coordinates are u = [u _x u _y ] ^t as shown in the following equation.

  In this embodiment, it is assumed that the lens distortion does not exist or is corrected, and the imaging apparatus is assumed to be a pinhole camera. As shown in FIG. 9, equation (27) indicates that a point in space, a projection point on the image of the point, and the position of the imaging device (viewpoint) exist on the same straight line. Also called a formula.

The position of the imaging apparatus in the reference coordinate system _{t = [t x t y t} z] t, ( in fact, the attitude of the reference coordinate system relative to the camera coordinate system) and orientation of the imaging apparatus _{ω = [ω x ω y ω} z ]. ω is a posture expression method with three degrees of freedom, and the posture is expressed by a rotation axis vector and a rotation angle. If the rotation angle is r _a , r _a is expressed by ω as follows:

When the rotation axis vector is r _axis = [r _x r _y r _z ] ^t , the relationship between r _axis and ω is expressed as the following equation.
_{[Ω x ω y ω z]} = [r a r x r a r y r a r z] ··· (29)
The relationship between ω (rotation angle r _a , rotation axis vector r _axis ) and 3 × 3 rotation transformation matrix R is expressed by the following equation.

The camera coordinate x _c of the point x _w = [x _w y _w z _w ] ^t on the reference coordinate system is expressed by t and R as follows.

From the equations (27) and (31), the point x _w = [x _w y _w z _w ] ^t on the reference coordinates is converted to the point u = [u _x u _y ] on the image by the perspective projection transformation as in the following equation. projected onto ^t .

Ideally, t, omega, projection position calculated from equation (32) based on x _w (calculated position) actually observed is a position (observation position) coincides. Therefore, if the horizontal displacement between the calculation position and the observation position image is F, the vertical displacement is G, and the observation position is u _o = [u _ox u _oy ] ^t , then F and G are 0.

F, G is a function related to the position x _w on the reference coordinates of the imaging apparatus position t, the imaging device orientation omega, viewed point. Here, the position x _w of a point on the reference coordinates, the type and the index is a function of different variables depending on whether bound on the plane. Therefore, F and G are functions of different variables depending on the type of the index and whether or not the constraint is made on the plane. Below, the case where an index | index is restrained on a plane is demonstrated.

(When constraining the indicator on a plane)
First, the plane definition method will be described. As shown in FIG. 10, the plane is expressed by polar coordinate expression r = [r θ φ]. Z _p axis of the plane coordinate system is parallel to the normal direction of the plane, passing through the origin in the reference coordinate system. The orientation of the plane is defined by the rotation θ around the y axis and the rotation φ around the x axis. Further, r is a signed distance between the origin o _w plane and the reference coordinate system. A rotation matrix R _p representing the orientation of the plane with respect to the reference coordinate system is expressed as follows.

The position _{t pl = [t plx t ply} t plz] t in the reference coordinate system of the origin _{o p} of the plane coordinate system, the intersection of the _{z p} axis and the plane is calculated as follows. However,

Represents the normal vector of the plane.

Position _{x w} in the reference coordinate system of the above-described polar representation r = [r θ φ] points on a plane coordinate system is expressed by _{x p = [x p y p} 0] t is expressed as follows. From the following equation, _{x w} it can be seen that is a function of _x p, r.

A point index constrained on the plane is represented by a position t ^pl _p = [t ^pl _px t ^pl _py 0] ^t on the plane coordinate system. As shown in the following equation, x _p becomes the same as t ^pl _p .

Therefore, _xw is a function of t ^pl _p , r, and F and G are imaging device position t, imaging device attitude ω, position t ^pl _p on the plane of the point index, and plane parameters as shown in the following equations: It becomes a function of r.

  Expression (38) is a nonlinear equation regarding the position and orientation of the imaging apparatus, the position of the index, and the plane parameters. Therefore, the Taylor expansion up to the first term is used to linearize in the vicinity of the approximate values of the position and orientation of the imaging device, the position of the index, and the plane parameters, and the position and orientation of the imaging apparatus, the position of the index, and the plane Find the parameters.

Equation (39) is a linearization of equation (38). _{△ t x, △ t y,} △ t z _{position, △ ω x, △ ω y} , △ ω z is the posture of the imaging ^{_{device, △ t pl px, Δt pl}} py indicators point to be bound to the plane of the imaging device The positions on the plane, Δr, Δθ, and Δφ represent correction amounts for the approximate values of the plane parameters.

Here, F ⁰ and G ⁰ in Expression (39) are constants, and the projection position (calculation position) of the index obtained from the position and orientation of the imaging device, the position of the point index, and the approximate value of the plane parameter, and the observation position u _o It is a difference.

  Expression (39) is an observation equation for one point index observed on a certain image. Actually, a plurality of point indices and vertices of square indices are observed on a plurality of images, and thus a plurality of expressions (39) are obtained. Accordingly, the plurality of equations (39) are solved as simultaneous equations for correction values for the position and orientation of the imaging apparatus, the position of the point index constrained on the plane, and the approximate value of the plane parameter, and the correction of the approximate value is repeated. Thus, the position and orientation of the imaging device, the position of the point index constrained on the plane, and the plane parameters are obtained.

  FIG. 4 is a flowchart showing a procedure for obtaining the position and orientation of the imaging apparatus and the position of the point index constrained by the plane in the present embodiment. Note that it is assumed that shooting of a scene including an index and index extraction and identification from the captured image have been completed. In step S4010, the position and orientation of the imaging device that captured each image, the position of the point index, and the approximate values of the plane parameters are set. In step S4020 and subsequent steps, a correction value for correcting the approximate value set in step S4010 is obtained. In step S4020, a projection error and a partial differential coefficient (image Jacobian) for each unknown parameter are obtained from the position and orientation of the imaging device, the position of the point index, and the approximate values of the plane parameters. In step S4030, a coefficient matrix necessary for obtaining the arrangement information parameter is calculated. In step S4040, backward substitution is performed using the calculated coefficient matrix to obtain correction values for the unknown parameters. In step S4050, the approximate value is corrected by the correction value obtained in step S4040 to obtain a new approximate value. In step S4060, it is determined whether or not the position and orientation of the imaging apparatus corrected in step S4050, the position of the point index, and the plane parameters have converged. Hereinafter, each step will be described in detail.

In step S4010, the position and orientation of the imaging device that captured each image, the position of the point index, and approximate values of the plane parameters are input. Here, the number of the captured images is N sheets, the position of the _t i ₌ the imaging device taken each image ^{_{[t ix t iy t iz]}} t (i = 1, ···, N), posture Ω _i = [ω _ix ω _iy ω _iz ] (i = 1,..., N). Further, the number of point indexes constrained on the plane is K _p2 , and t ^pl _pi = [t ^pl _pix t ^pl _pi ] ^t (i = 1,..., K _p2 ).

  Next, in step S4020, a partial differential coefficient and a residual for an unknown parameter of the index image coordinates are obtained. Further, the residual is obtained from the difference between the projected position of each index calculated from the position and orientation of the imaging apparatus, the position of the point index, and the approximate value of the plane parameter, and the observation position on the actual image.

The observation equation of Equation (39) holds as many as the number of indices observed on the image. Expression (39) is an observation equation for the position and orientation correction value of one imaging apparatus, the correction value of the position of one point index, and the correction value of one plane parameter. Here, as shown in the following equation, an observation equation is established for the position and orientation of the N imaging devices, the position of the point index constrained on the K _p2 planes, and the parameters of the P planes. In this case, the number of correction values to be unknown is (6 × N + 2 × K _p2 + 3 × P).

If the number of point indices detected from the image i (i = 1,..., N) is d _pi , the number D _{p of} point indices detected from the N images is as follows.

When the number of point indices detected from N images is D _p , the observation equation (37) has D _p sets, that is, 2 × D _p observation equations. When the constant terms F ⁰ and G ⁰ on the left side of the observation equation of Equation (39) are transferred to the right side and a simultaneous equation is established, this simultaneous equation can be written in matrix form as the following equation.
J · △ = E (42)
J is called a Jacobian matrix, and is an array of F and G imaging device positions and orientations, point index positions, and partial differential coefficients for plane parameters. The number of rows of the Jacobian matrix J is the number of observation equations 2 × D _p , and the number of columns is the same as the number of unknowns (6 × N + 2 × K _p2 + 3 × P). Δ is a correction value vector. The number of elements of the correction value vector is the same as the number of unknowns (6 × N + 2 × K _p2 + 3 × P). E is an error vector, and has the difference −F ₀ , −G ₀ between the calculation position of the projection position based on the approximate value and the observation position u _o as an element. The number of elements of E is the same as the number 2 × D _p of the observation equation.

  It should be noted that the origin, scale, and orientation of the reference coordinate system can be explicitly specified by simultaneously photographing point indices whose positions in the reference coordinate system are known. In the equation (40) for these indices, the values of the partial differential coefficients for the positions of the indices are all zero. In order to explicitly specify the origin, scale, and orientation of the reference coordinate system, it is sufficient to use three point indices whose positions are known as point indices.

When the Jacobian matrix J in the equation (42) is not a square matrix, the correction value vector Δ can be obtained by the least square method by multiplying both sides by the transposed matrix of the Jacobian matrix J as shown in the following equation.
(J ^t · J) Δ = J ^t · E (43)
That is, a square matrix J ^T J is formed by applying a transposed matrix of the Jacobian matrix J, and an inverse matrix (J ^T J) ⁻¹ is obtained and applied to both sides to obtain a correction value. However, since the size of the matrix J ^T J increases with the number of unknown parameters, the calculation of the inverse matrix generally has a heavy calculation load. On the other hand, since the Jacobian matrix J is a sparse matrix containing many 0s, the structure of this sparse matrix is used rather than obtaining the inverse matrix (J ^T J) ⁻¹ and obtaining the correction value vector Δ directly. The calculation can be performed efficiently by dividing and obtaining the unknown parameters.

In step S4030, a coefficient matrix for obtaining unknown parameters is obtained step by step. Here, it is considered that the arrangement information parameters are collected for each type, and the position and orientation of the N imaging devices, the positions of the K _p2 point indicators, and the P plane parameters are obtained separately. The correction value vector Δ is divided into a position / orientation correction value vector Δ _c , a point index position correction value vector Δ _p , and a planar correction value vector Δ _{PL as} in the following equation. When the matrix J ^t · J of Expression (43) is divided for each placement information parameter and the right side is collected, the following expression is obtained.

Here, the divided coefficient matrix will be described. A matrix obtained by multiplying the Jacobian matrix α related to the position and orientation of the imaging apparatus and its transposition α ^T is U = α ^T α, a Jacobian matrix β related to the position of the point index and a matrix multiplied by the transposition β ^T is V = β ^T β, and a plane A matrix obtained by multiplying the Jacobian matrix γ related to the parameter and its transposition γ ^T is T = γ ^T γ, W ₁ = α ^T β, and W ₂ = α ^T γ. Since the position of the point index and the plane parameter are related to each other, X = β ^T γ ≠ 0. Further, as in the first embodiment, α ^T α, β ^T β, and γ ^T γ are obtained by calculating only α, β, and γ sub-matrix components, so that the memory that is secured when calculating in the computer is calculated as usual. Less than you do. That is, if the sub-matrices on the diagonal of U, V, and T are U _i , V _j , and T _k , respectively, the following equations are obtained.
U _i = α _ii ^T α _ii (18)

The α, β, and γ sub-matrices are represented by α _ii , β _ij , and γ _ik , respectively. N represents the number of captured images.

Therefore, U, V, T, W ₁ , W ₂ , and X may be obtained when obtaining a J ^t · J matrix that is a coefficient matrix of unknown parameters.

In step S5040, an upper right triangular matrix is first created, and then correction values are calculated stepwise by backward substitution. When the divided ^Jt · J matrix and the error vector are calculated side by side, the calculation is as follows.

  In the following, the divided block is transformed for each row to create an upper right triangular matrix.

^{_{Here, P 22 = V-W 1}} T U -1 W, P 23 = X-W 1 T U -1 W 2, F 2 = E 2 -W 1 T U -1 E 1, P 32 = X T ^{_{-W 2 T U -1 W, P}} 33 = T-W 2 T U -1 W 2, F 3 = E 3 -W 2 T U -1 E 1, and putting,

^{_{Here, L 33 = P 33 -P 32}} P 22 -1 P 23, G 3 = F 3 -P 32 P 22 -1 F 2, and put the

Equation (48) can be transformed into an upper right triangular matrix as shown in Equation (52).

In step S5050, when backward substitution is performed for the correction value vector of Expression (53), a correction value vector of the approximate value of the placement information parameter is obtained as in the following expression.
△ _PL = _L ³³ -1 _{G 3
^{△ P = P 22 -1 F 2}} - (P 22 -1 P 23) △ 3 ··· (54)
^{_{△ C = U -1 E 1 -}} (U -1 W 1) △ 2 - (U -1 W 2) △ 3
Here, the matrix size of U is (6N × 6N), the matrix size of P ₂₂ is (2K _p2 × 2K _p2 ), and the matrix size of L ₁₁ is (3P × 3P). Δ In order to obtain _C , the inverse matrix of U is calculated. However, if all Δ are solved simultaneously without performing the above division, ((6N + 2K _p2 + 3P) × (6N + 2K _p2 + 3P)) It is necessary to obtain an inverse matrix of size. However, by solving divided in inverse matrix calculation for obtaining the correction value, △ _C is (6N × 6N), △ _P is _{(2K p2 × 2K p2),} △ PL is (3P × 3P), the size of the What is necessary is just to obtain the inverse matrix reduced to.

  In addition, a sub-matrix that is a square matrix such as the following equation is diagonal, and the size of each sub-matrix is equal to D, an inverse matrix operation of (Dn × Dn) is obtained as in equation (56). The first embodiment described that the problem can be reduced to the problem of obtaining n (D × D) inverse matrix operations, and the speed can be increased.

  In the process of calculating the correction value Δ described above, only the calculation for U can be performed as in Expression (56). However, in order to perform the calculation most efficiently, the calculation order of the expression (56) is applied to the matrix having the largest size among U, V, and T in consideration of the calculation order of the arrangement information parameters. To solve the simultaneous equations.

  In step S5060, the correction value Δ of each approximate value from the result obtained by backward substitution in step S5050, that is, the position and orientation of the imaging device, the position of the point index, the position and orientation of the square index, and the approximate value of the plane parameter here. The correction value for is obtained. In step S5070, the obtained correction value is added to the approximate value, and is set as a new approximate value.

  In step S5080, convergence determination is performed. Convergence determination is such that the absolute value of the correction value falls below a specified threshold, the difference between the calculated position of the projected position and the observed position falls below the specified threshold, the difference between the calculated position of the projected position and the observed position becomes minimal, etc. This is done by determining the conditions. If it is determined in this step that it has converged, the marker placement information measurement ends. If it has not converged, the process returns to step S5020, and the corrected position and orientation of the imaging device, the position of the point index, the plane Formulate simultaneous equations again based on the approximate values of the parameters.

  As described above, in the present embodiment, a large number of images of a scene in which indices are arranged are captured, the indices are detected and identified from the captured images, and a plurality of indices existing on the same plane are designated, From the index observation position on the image and the constraint condition that the index exists on the same plane, the position and orientation of the imaged imaging device, the arrangement information of the index constrained on the plane, and the correction value for the approximate value of the plane parameter Is obtained stepwise by dividing it into arrangement information parameters satisfying the observation equation, and the operation for correcting the approximate value is repeatedly performed to obtain the arrangement information of the index at high speed.

(Modification 2-1)
In the second embodiment, even when a constraint condition that a plurality of indexes exist on the same plane is given as a constraint condition related to the marker placement, the marker placement information measurement is speeded up. The condition may be a constraint condition that the directions of the normal vectors of a plurality of plane indices are the same.

FIG. 12 is a diagram showing an index arrangement to which this embodiment can be applied. In the figure, it is assumed that four square markers are arranged, and the normal vectors of the four square markers, that is, the directions of the z _s axes of the square markers are the same.

The direction of the normal vector of the square index is represented by θ and φ. The normal vector is a vector obtained by rotating the z-axis unit vector of the reference coordinate system by θ around the x axis and then rotating by φ around the y axis. θ and φ have a common value for a plurality of square indices whose normal vectors are in the same direction. When rotation about the z-axis of each square index is ψ, the posture R _s of the square index with respect to the reference coordinate system is expressed by the following equation.

Assuming that the position of each square index in the reference coordinate system is t _s = [t _sx t _sy t _sz ] ^t , the position x _w of the point x _s = [x _s y _s ] ^t on the square index is It is expressed by the following formula.

Position in the reference coordinate system of each vertex of the square marker, the orientation θ of the normal vector, phi, orientation around the z axis of the square indices [psi, a function of the position t _s in the reference coordinate system of the square marker. Therefore, the observation equation of the square index when the normal vector is constrained is as follows.

  When obtaining the index placement information, a simultaneous equation is established based on a plurality of observation equations (68), and corrections are made to the approximate values of the position and orientation of the imaging apparatus, the placement information of the index, and the normal vector orientations θ and φ. Find the value and optimize the approximate value by iterative calculation. As in the second embodiment, unknown parameters are obtained in stages. FIG. 16 shows a case where three images of a scene are taken when two square markers having a common normal vector and two square markers having another normal vector are present in the scene. It represents the structure of the Jacobian matrix. The white portion of the figure indicates that 0 is always included, and the black portion indicates a portion where 0 is not necessarily included. By using the structure of the sparse matrix as shown in FIG. 16, the unknown parameters can be obtained by dividing into the camera position, the square index position and orientation, and the normal vector parameters as in the second embodiment. Therefore, the bundle adjustment calculation is speeded up. Similarly to the second embodiment, when obtaining a correction value by backward substitution, the inverse matrix calculation can be made more efficient by obtaining the parameter having the largest number of unknown coefficients last.

  As described above, as a constraint condition regarding index placement, even when the normal vectors of multiple indicators have the same orientation, they are divided into placement information parameters and obtained step by step, and index placement information can be obtained at high speed. Can be requested.

(Modification 2-2)
In the second embodiment, the arrangement information of the index is obtained with one kind of constraint condition regarding the arrangement of the index. However, the arrangement information of the index can be obtained even when there are a plurality of constraint conditions. .

  FIG. 13 is a diagram showing an index arrangement to which the present embodiment can be applied. In the figure, the point index 1 and the square index 1 exist on the same plane (plane 1), the directions of the normal vectors of the square index 2 and the square index 3 are common, and the point index 2 and the square index 4 are related to the arrangement. The case where there is no constraint condition is shown.

When the scene shown in FIG. 13 is photographed by the imaging device, the observation equation (62) for the point index 1, the observation equation (63) for the square index 1, the observation equation (68) for the square indices 2 and 3, With respect to the point index 2, the observation equation (69) holds when the position of the point index not restricted by the plane is Δt _px , Δt _py , Δt _pz, and for the square index 4 the position of the square index not restricted by the plane is Δ If t _sx , Δt _sy , Δt _sz , and the posture of the square index not constrained by the plane are Δω _sx , Δω _sy , Δω _sz , the observation equation (70) is established.

Expressions (62), (63), (68) to (70) are the position and orientation correction values of one imaging apparatus, the arrangement information of one index, the parameters of one plane, and the direction of one normal vector. It is an observation equation about the correction value. As shown in the following equation, the position and orientation of the N imaging devices, the position of the point index not constrained on the K _p1 planes, the position of the point index constrained on the K _p2 planes, the K _s1 plane The position and orientation of a square index not constrained above, the position and orientation of a square index constrained on K _s2 planes, the position and orientation of a square index where K _s3 normal vectors are constrained, the parameters of P planes, Establish an observation equation for the correction value of the direction of V normal vectors.

The observation equation of Expression (71) is set up and solved as simultaneous equations for each point index detected on the captured image and each vertex of the square index. As a result, unknown parameters (imaging device position t _i , imaging device orientation ω _i , position index position t _pi without constraint on arrangement, square index position t _si without orientation constraint, orientation ω _si , constraint on plane Point index position t ^pl _pi , square index position t ^pl _si constrained on the plane, orientation θ ^pl _i , square index position t ^v _si , normal vector constrained, orientation ψ _i , plane A correction value for the approximate value of the parameter r _i and the direction of the normal vector θ ^v _i , φ ^v _i ) is obtained. Then, the correction of the approximate value is optimized by iterative calculation.

  When obtaining the correction value of each unknown parameter, the unknown parameter is obtained stepwise as in the second embodiment. In FIG. 17, the point index 1 and the square index 1 exist on the same plane (plane 1), the directions of the normal vectors of the square index 2 and the square index 3 are common, and the point index 2 and the square index 4 are arranged. This shows the structure of the Jacobian matrix when two images are taken so that all of these indices are taken when there is no constraint condition for. The white portion of the figure indicates that 0 is always included, and the black portion indicates a portion where 0 is not necessarily included. By using a sparse matrix structure as shown in FIG. 17, the unknown parameters can be divided and obtained. Similarly to the second embodiment, when obtaining a correction value by backward substitution, it is possible to improve the efficiency of inverse matrix calculation by obtaining a parameter having the largest number of unknown parameters last.

  As described above, in the present embodiment, even when there are a plurality of constraint conditions related to marker placement, marker placement information can be obtained at high speed.

[Third embodiment]
In the second embodiment, the case where there is a constraint on the placement information of the index has been described in detail, but the placement information of the index and the imaging device can be calculated at high speed even when the placement of the imaging device is restrained. Can do. In the first embodiment and the second embodiment, the portion of the Jacobian matrix relating to the position and orientation of the imaging device always has a sub-matrix that is a square matrix diagonally and the size of each sub-matrix is 6 × 6. It was. However, in the present embodiment, the portion of the Jacobian matrix relating to the position and orientation of the imaging device is divided according to the type and presence / absence of constraint conditions.

For example, when the imaging apparatus is constrained to a plane, the plane parameter for expressing the plane in polar coordinates is set to r = [r θ φ], and the position of the imaging apparatus in the set plane coordinate system, as in the second embodiment. T ^pl = [t _x ^pl t _y ^pl 0] ^t , posture is ω = [ω _x ω _y ω _z ], and the correction value of the position on the plane of the imaging device constrained by the plane is Δt _x ^pl , If Δt _y ^pl and the position of the point index not constrained by a plane is t _p = [t _px t _py t _pz ] ^t , the observation equation is as follows.

  As in the second embodiment, unknown parameters are obtained in stages. FIG. 18 shows that when there are one imaging device that is not constrained to a plane, two imaging devices that are constrained to a plane, and two unconstrained point indexes in a scene, the image of the scene is 1 for each imaging device. This shows the structure of the Jacobian matrix when taking a picture. The white portion of the figure indicates that 0 is always included, and the black portion indicates a portion where 0 is not necessarily included. By using the structure of the sparse matrix as shown in FIG. 18, as in the second embodiment, the position and orientation of the imaging device in which the unknown parameters are not constrained by the plane, the position and orientation of the imaging device constrained by the plane, and the plane parameters Therefore, the bundle adjustment calculation is speeded up. Similarly to the second embodiment, when calculating a correction value by backward substitution, it is possible to improve the efficiency of inverse matrix calculation by determining the type of parameter having the most unknown parameters last.

  As described above, according to the present embodiment, it is possible to obtain the index and the arrangement information of the imaging device at high speed even when the constraint condition regarding the arrangement of the imaging device exists. In addition, even when the imaging apparatus is constrained by a constraint condition other than being constrained on a plane, the constraint condition of the imaging apparatus is handled in the same manner as when there are other constraint conditions described as the index constraint conditions. It goes without saying that it is possible.

[Other Embodiments]
(Other embodiment-1)
In each of the above embodiments, a specific processing method for each of the types of typical unknown parameters has been described. An arrangement information measuring apparatus capable of adaptively executing the processing methods of the plurality of embodiments according to the types of unknown parameters by determining the types of unknown processing parameters and using a processing method according to the determination result. Can be provided.

  For example, in the first embodiment, the type of the unknown parameter can be determined based on the index information detected by the index detection unit 3030. That is, the position of the point index detected by the index detection unit 3030, the position and orientation of the square index detected by the index detection unit 3030, and the position and orientation of the imaging device that captures these indexes are the types of unknown parameters. Is determined.

  In the second embodiment, the type of the unknown parameter can be determined based on the index information detected by the index detection unit 2030 and the constraint condition setting unit 2040. In other words, the position of the point index detected by the index detection unit 2030, the plane parameters in the plane constraint conditions set by the constraint condition setting unit 2040, and the position and orientation of the imaging device that captures these indexes are the types of unknown parameters. Judge that there is.

  In this way, the type of unknown parameter can be determined from information input to the data management unit.

(Other embodiment-2)
In the above embodiment, when there is a constraint condition, the observation equation is solved so as to satisfy the constraint condition. Then, a correction value for the approximate value of the arrangement information parameter was obtained stepwise by dividing the arrangement information parameter satisfying the observation equation. Then, the index and the arrangement information of the imaging device were obtained by repeatedly performing the operation of correcting the approximate value. However, any method may be used for dividing the observation equation as long as it uses a difference in the type of arrangement information parameter appearing in the observation equation.

The simultaneous equations of observation equations are as follows,
(J ^t · J) Δ = J ^t · E (73)
If the arrangement information parameter satisfies the observation equation, the J ^t · J matrix that is the coefficient matrix of the correction value vector Δ can be divided. The J ^t · J matrix is divided into blocks for each arrangement information parameter, and the right side is summarized as follows.

  Here, m exists as many as the types of arrangement information parameters. When forward erasure is performed for each divided block, an upper right triangular matrix in which one parameter is a block as shown in the following equation is formed.

  Backward substitution is performed for each divided block, and a correction value Δ for each approximate value is obtained from the result obtained by backward substitution.

  Similar to the first embodiment and the second embodiment, the bundle adjustment calculation is speeded up by using the structure of the sparse matrix by dividing according to the types of unknown parameters. In addition, as in the first embodiment and the second embodiment, when calculating the correction value by backward substitution, it is possible to improve the efficiency of the inverse matrix calculation by obtaining the parameter with the most unknown parameters last. is there.

  As described above, the correction value for the approximate value of the placement information parameter is divided according to the type of the placement information parameter that satisfies the observation equation, is obtained in stages, and the operation for correcting the approximate value is repeatedly performed, thereby providing an index. In addition, the calculation for obtaining the arrangement information of the imaging device can be speeded up.

(Other embodiment-3)
In the above embodiment, as a method for correcting the index arrangement information, a method has been described in which the observation equation is Taylor-expanded and repeatedly corrected using up to the first order term. This is a method of linearly approximating the observation equation locally in one iteration and estimating the correction amount of the unknown parameter when it is assumed that the error is zero. It is an equivalent method. The Newton method is a typical method for solving a nonlinear equation by numerical calculation, but the iterative calculation method applicable to the above embodiment is not limited to this method. For example, even if the correction amount is dynamically changed based on the variation in the correction amount of the unknown parameter estimated in one process, such as the Levenberg-Marquardt method, which is known as a well-known technique as in the Newton method. Good. Further, it may be a method in which the result of Taylor expansion is taken into consideration up to higher order terms and repeated calculation is performed. The feature of the above embodiment is that even when a plurality of types of indicators having different arrangement information amounts exist together, an optimal solution is obtained by effectively using each type of indicator.

(Other embodiment-4)
An object of the present invention is to supply a storage medium (or recording medium) in which a program code of software for realizing the functions of the above-described embodiments is recorded to a system or apparatus, and a computer (or CPU or MPU) of the system or apparatus Needless to say, this can also be achieved by reading and executing the program code stored in the storage medium. In this case, the program code itself read from the storage medium realizes the functions of the above-described embodiment, 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 showing the schematic structure of the position and orientation calculation device concerning a first embodiment. It is a flowchart which shows the schematic process of the position and orientation calculation method which concerns on 1st embodiment. It is a figure which shows schematic structure of the position and orientation calculation apparatus which concerns on 2nd embodiment. It is a flowchart which shows the schematic process of the position and orientation calculation method which concerns on 2nd embodiment. It is a figure which shows schematic structure of the parameter | index which concerns on 1st and 2nd embodiment. It is a figure which shows schematic structure of the parameter | index which concerns on 1st and 2nd embodiment. It is a figure explaining GUI which sets the some parameter | index which exists on the same plane. It is a figure explaining a camera coordinate system and a screen coordinate system. It is a figure explaining a collinear conditional expression. It is a figure explaining the definition method of a plane. It is a figure which shows the case where two planes to which an index is constrained exist. It is a figure which shows the parameter | index arrangement | sequence which can apply the modification 2-3. It is a figure which shows the parameter | index arrangement | sequence which can apply the modification 2-4. It is a figure which shows the shape of the Jacobian matrix in 1st embodiment. Is a view showing the shape of a J ^{T J} matrix in the first embodiment. It is a figure which shows the shape of the Jacobian matrix in 2nd embodiment and the modification 2-1. It is a figure which shows the shape of the Jacobian matrix in 2nd embodiment and modification 2-2. It is a figure which shows the shape of the Jacobian matrix in 3rd embodiment.

Claims (14)

An information processing method for measuring index placement information,
An acquisition step of acquiring information related to the image coordinates of the index detected from the image obtained by capturing the index;
A setting step for setting an initial value of the placement information of the index;
A calculation step of calculating the placement information of the index by solving an observation equation for information on the image coordinate of the index, the position and orientation of the imaging device at the time of capturing the image, and the initial value of the placement information of the index And
The information processing method characterized in that the arrangement information calculation step divides the unknown parameter in the observation equation into three or more and calculates the divided unknown parameter stepwise. The indicators include first and second indicators of different types,
The unknown parameters include parameters related to the position and orientation of the imaging device, parameters related to the placement information of the first index, and parameters related to the placement information of the second index,
2. The divided unknown parameter includes a parameter related to a position and orientation of the imaging apparatus, a parameter related to arrangement information of the first index, and a parameter related to arrangement information of the second index. The information processing method described.   The information processing method according to claim 1, wherein the placement information of the index is information indicating a position, a position / posture, a position / posture, and a size. Furthermore, it has an index constraint condition acquisition step of acquiring a constraint condition related to the index,
The information processing method according to claim 1, wherein the unknown parameter includes a parameter related to a constraint condition. Furthermore, based on a limited geometric constraint in which the imaging device exists, an imaging device constraint condition acquisition step of acquiring a constraint condition that the imaging device may exist in the space,
The information processing method according to claim 1, wherein the unknown parameter includes a parameter related to a constraint condition.   The constraint condition is a constraint that an index is arranged on a plane, a variable representing the constraint is an equation of a plane, and the arrangement information under the constraint is a position on a plane or a position on a plane and a plane. 6. The information processing method according to claim 4, wherein the rotation is about a normal vector.   The constraint condition is that a plurality of planar shape indexes share a normal vector, the variable representing the constraint is the direction of the normal vector of the index, and the arrangement information under the constraint is cubic 6. The arrangement information measuring apparatus according to claim 4 or 5, wherein the arrangement information is a rotation around a normal vector of a position and an index in the original space.   2. The information processing method according to claim 1, wherein among the divided unknown parameters, an unknown parameter having the largest number of unknown coefficients is calculated last. An acquisition step of acquiring an image of the index;
An index image coordinate acquisition step of detecting the index from the image and acquiring information relating to image coordinates of the detected index;
An index information acquisition step of acquiring index information related to the index;
A setting step for setting an observation equation based on the image information of the index and the index information;
A division method setting step of setting a division method of unknown parameters in the observation equation based on the index information;
An information processing method comprising: dividing an unknown parameter of the observation equation based on the set division method and calculating the divided unknown parameter stepwise. The indicators include first and second indicators of different types,
The unknown parameters include parameters related to the position and orientation of the imaging device, parameters related to the placement information of the first index, and parameters related to the placement information of the second index,
The unknown parameter in the observation equation is divided into a parameter relating to a position and orientation of the imaging device, a parameter relating to arrangement information of the first indicator, and a parameter relating to arrangement information of the second indicator. The information processing method described. The index information includes arrangement information of the index and constraint conditions of the index,
10. The information processing method according to claim 9, wherein the divided unknown parameters include a parameter related to index arrangement information and a parameter related to index constraint conditions.   The program for implement | achieving the information processing method of Claim 1 thru | or 11 using a computer. An information processing apparatus for measuring index arrangement information,
Obtaining means for obtaining information on image coordinates of an index detected from an image obtained by photographing the index;
Setting means for setting an initial value of the placement information of the index;
Calculation means for calculating the placement information of the index by solving an observation equation for information on the image coordinate of the index, the position and orientation of the imaging device at the time of capturing the image, and the initial value of the placement information of the index And
The arrangement information calculating means divides an unknown parameter in the observation equation into three or more, and calculates the divided unknown parameter stepwise. An acquisition means for acquiring an image of the index;
Index image coordinate acquisition means for detecting the index from the image and acquiring information relating to image coordinates of the detected index;
Index information acquisition means for acquiring index information related to the index;
Setting means for setting an observation equation based on the image information of the index and the index information;
A division method setting means for setting a division method of unknown parameters in the observation equation based on the index information;
An information processing apparatus comprising: a calculation unit that divides unknown parameters of the observation equation based on the set division method and calculates the divided unknown parameters stepwise.

Images