WO2014034035A1

WO2014034035A1 - Location attitude estimation device, location attitude estimation method, and location attitude estimation program

Info

Publication number: WO2014034035A1
Application number: PCT/JP2013/004849
Authority: WO
Inventors: 中野　学
Original assignee: 日本電気株式会社
Priority date: 2012-08-31
Filing date: 2013-08-13
Publication date: 2014-03-06
Also published as: JPWO2014034035A1; JP6260533B2

Abstract

A location attitude estimation device comprises: an unknown attitude candidate calculation unit (1) which receives input of three or more combinations of correspondence points which are included in two images, and known attitudes which are two known attitude parameters among three attitude parameters which represent a relative attitude of a camera which has photographed the two images, and outputs as unknown attitude candidates all unknown attitude solutions which satisfy a prescribed function; a location candidate calculation unit (2) which calculates a candidate of a relative location of the camera with respect to each of the unknown attitude candidates; and an error minimum candidate extraction unit (3) which, from among the unknown attitude candidates and the location candidates, extracts one or a plurality of unknown attitudes and relative locations of the camera whereat a prescribed error function which represents a geometrical relation between the correspondence points, the relative locations of the camera, and the unknown attitudes, reaches a minimum.

Description

Position / orientation estimation apparatus, position / orientation estimation method, and position / orientation estimation program

The present invention relates to a technique for estimating the relative position and orientation of a camera that has captured a plurality of images.

In order to restore the 3D shape of a subject and generate a panoramic image of a background from a plurality of images taken with a camera, it is necessary to accurately estimate the position and orientation of the camera that has taken the image.

最低 At least two images are required to estimate the camera position and orientation. When there are three or more images, it is possible to cope with posture estimation by combining two images. A method for estimating the relative position and orientation between two images will be described below. The reason for estimating the relative position and orientation is that when the input is only an image, the absolute change amount of the position and orientation between the two images cannot be estimated. In order to estimate the relative position and orientation, one of the two images is assumed to be the origin of the three-dimensional space. If the origin and distance indices are known, appropriate coordinate transformation may be applied to the estimated relative position and orientation. In the following, the first image is the coordinate origin, the orientation of the second image is a three-dimensional rotation matrix represented by three degrees of freedom, and the indefinite position is a three-dimensional vector represented by two degrees of freedom. .

One method for estimating the camera position and orientation when internal parameters such as focal length, optical center coordinates, and lens distortion are known is a method using an epipolar equation. The epipolar equation is an equation representing the geometric relationship between the image coordinates (hereinafter referred to as corresponding points) representing the same three-dimensional coordinates on the two images and the camera position and orientation.

For example, Non-Patent Document 1 describes a method of estimating a camera position and orientation by solving an epipolar equation using eight or more pairs of corresponding points on two images. In the method described in Non-Patent Document 1, first, the corresponding point is converted into a parameter called a three-dimensional E matrix (Essential Matrix) represented by the camera position and orientation. Next, by ignoring the constraint condition of the E matrix and making it an independent variable with 8 degrees of freedom, a linear epipolar equation satisfied by 8 or more pairs of corresponding points is solved, and the E matrix is calculated. The constraint condition is that one of three singular values obtained by singular value decomposition of the E matrix is zero and two are equal. Finally, the E matrix is corrected after the fact so as to satisfy the constraint condition, and the corrected E matrix is decomposed to estimate the position and orientation. The correction is to perform singular value decomposition on the estimated E matrix and change the singular value as described above.

Since the estimation accuracy of the camera position and orientation by the method described in Non-Patent Document 1 depends on the extraction accuracy of the corresponding point set, such as when the movement of the camera is minute or when the movement direction of the camera is close to the optical axis direction, It is known that when the difference between the image coordinates of the corresponding point set is small, the accuracy is greatly reduced.

In such a case, as a method for estimating the camera position and orientation with high accuracy, there is a method in which a corresponding point set and sensor information other than the camera are used in combination.

For example, Non-Patent Document 2 describes an epipolar equation that satisfies two sets of degrees of freedom 3 representing a posture from an acceleration sensor and one vanishing point (hereinafter, referred to as a known posture) and is satisfied by three sets of corresponding points. A method for directly estimating the position and orientation by solving the above is described. Since the parameters to be estimated are reduced to a total of 3 degrees of freedom including 2 degrees of freedom representing the position and 1 degree of freedom representing the remaining posture (hereinafter referred to as an unknown posture), it is more accurate than Non-Patent Document 1. .

However, in the technique described in Non-Patent Document 2, the epipolar equation considering the constraint condition between parameters becomes a complex nonlinear simultaneous equation, so the input of corresponding points is limited to three sets, and the least squares of four sets or more are used. I cannot find a solution.

As a method for obtaining the least squares solution, Non-Patent Document 3 describes a method for estimating the position and orientation by dividing the corresponding points into 3, 4, 5, or more cases. In any case, as in Non-Patent Document 1, an E matrix is obtained from a linear epipolar equation, the E matrix is corrected so as to satisfy the constraint conditions, and the corrected E matrix is decomposed to estimate the position and orientation.

It is known that by estimating the camera position and orientation using each of the above methods, it is possible to restore the three-dimensional shape of the corresponding points and generate a panoramic image of the background. Methods for restoring the three-dimensional coordinates of corresponding points based on the relative position and orientation of the camera are described in Non-Patent Document 4 and Non-Patent Document 5, for example.

As described in Non-Patent Document 2, if two of the postures are known, the camera position and posture can be estimated from a minimum of three sets of corresponding points. However, since observation errors due to quantization and sensor noise inside the camera occur at the corresponding points, the camera position and orientation cannot be estimated with high accuracy using only three sets. In general, a plurality of camera positions and orientations are estimated in three sets, and it is impossible to mathematically distinguish which camera position and orientation should be selected. In order to estimate the only camera position and orientation with high accuracy, it is necessary to use four or more sets of corresponding points. However, the position and orientation estimation method described in Non-Patent Document 2 is a case where there are only three sets of corresponding points, and cannot be applied to four or more sets.

The position / orientation estimation method described in Non-Patent Document 3 calculates the position and orientation at the same time, and therefore requires a correction means, which requires a large amount of calculation. Specifically, it is necessary to perform matrix operations such as singular value decomposition for correction.

SUMMARY OF THE INVENTION An object of the present invention is to provide a position / orientation estimation apparatus, a position / orientation estimation method, and a position / orientation estimation program that can estimate the relative position and orientation of a camera that has taken two images with high accuracy and high speed. To do.

The position / orientation estimation apparatus according to the present invention includes three or more pairs of corresponding points included in two images and two known orientation parameters representing the relative orientation of the camera that has captured the two images. A known posture, which is a posture parameter, is input, and the relative position between the unknown posture, which is one of the posture parameters and is an unknown posture parameter, is used by using the input corresponding point and the known posture. An unknown posture candidate calculation unit that outputs, as candidates for unknown postures, solutions of all the unknown postures that satisfy a predetermined function represented by using the unknown posture and the corresponding point as a variable. A position candidate calculation unit that calculates a position candidate of the camera relative to each of the posture candidates, and all of the unknown posture candidates and the camera relative to the unknown posture candidates. A predetermined position representing a geometrical relationship between the relative position of the corresponding point, the camera, and the unknown posture among the unknown posture candidate and the position candidate. One or a plurality of the unknown postures having a minimum error function and a minimum error candidate extraction unit that extracts a relative position of the camera are provided.

The position / orientation estimation method according to the present invention uses three or more sets of corresponding points included in two images and two known orientation parameters representing the relative orientation of the camera that captured the two images. A known posture, which is a posture parameter, is input, and the relative position between the unknown posture, which is one of the posture parameters and is an unknown posture parameter, is used by using the input corresponding point and the known posture. Output the solutions of all the unknown poses satisfying a predetermined function expressed using the unknown pose and the corresponding point as a variable, as unknown pose candidates, and for each of the unknown pose candidates, Calculates the relative position candidates of the camera, and inputs all the unknown posture candidates, the relative position candidates of the camera for the unknown posture candidates, and the corresponding points. Among the unknown posture candidates and the position candidates, one or more of the unknowns that minimize a predetermined error function representing a geometric relationship between the relative position of the corresponding point, the camera, and the unknown posture. The posture and the relative position of the camera are extracted.

The position / orientation estimation program according to the present invention is known to a computer among three or more sets of corresponding points included in two images and three orientation parameters representing the relative orientations of the cameras that photographed the two images. And the known posture, which is one of the posture parameters and is an unknown posture parameter, and the known posture, which are two posture parameters of An unknown posture candidate calculation process for outputting all the unknown posture solutions satisfying a predetermined function represented by using the unknown posture and the corresponding points as relative variables of a camera as candidates for unknown posture; A position candidate calculation process for calculating a position candidate of the camera relative to each of the unknown posture candidates, and all the unknown posture candidates and the unknown posture candidates, respectively. The relative position candidate of the camera and the corresponding point are input, and the relative position of the corresponding point, the camera, and the unknown posture are selected from the unknown posture candidate and the position candidate. One or a plurality of unknown poses that minimize a predetermined error function representing a geometric relationship and a minimum error candidate extraction process that extracts a relative position of the camera are executed.

According to the present invention, the relative position and orientation of a camera that has captured two images can be estimated with high accuracy and high speed.

It is a block diagram which shows the structural example of 1st Embodiment of the position and orientation estimation apparatus by this invention. It is a flowchart which shows an example of operation | movement of 1st Embodiment of the position and orientation estimation apparatus by this invention. It is a block diagram which shows the structural example of 2nd Embodiment of the position and orientation estimation apparatus by this invention. It is a flowchart which shows an example of operation | movement of 2nd Embodiment of the position and orientation estimation apparatus by this invention. It is a block diagram which shows the structural example of 3rd Embodiment of the position / orientation estimation apparatus by this invention. It is a flowchart which shows an example of operation | movement of 3rd Embodiment of the position and orientation estimation apparatus by this invention. It is a block diagram at the time of mounting the position and orientation estimation apparatus by this invention in an information processing system. It is a block diagram which shows the structure of the principal part of the position and orientation estimation apparatus by this invention.

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

Embodiment 1. FIG.
FIG. 1 is a block diagram showing a configuration example of a first embodiment (Embodiment 1) of a position and orientation estimation apparatus according to the present invention. The position and orientation estimation apparatus shown in FIG. 1 includes an unknown orientation candidate calculation unit 1, a position candidate calculation unit 2, and a minimum error candidate extraction unit 3. The unknown posture candidate calculation unit 1 includes a coefficient calculation unit 11, a simultaneous polynomial solution unit 12, a real solution extraction unit 13, and an unknown posture candidate conversion unit 14.

The unknown posture candidate calculation unit 1 represents three or more sets of corresponding points of two images and the relative posture of the camera that captured the two images (hereinafter, sometimes referred to as “posture”) 3 Two known posture parameters among the two posture parameters are input as known postures. Then, the unknown posture candidate calculation unit 1 is one of the three posture parameters and is an unknown posture parameter and the relative position of the camera (hereinafter, may be referred to as “position”). Are used as variables, and all unknown poses satisfying a predetermined function expressed using corresponding points and unknown poses are calculated. In the following description, a predetermined function represented by a corresponding point and an unknown posture is simply referred to as a simultaneous polynomial.

The coefficient calculation unit 11 inputs three or more sets of corresponding points and two known postures, calculates the coefficient of the simultaneous polynomial, and outputs it to the simultaneous polynomial solving unit 12.

The simultaneous polynomial solving unit 12 receives the coefficients of the simultaneous polynomial calculated by the coefficient calculating unit 11, solves the simultaneous polynomial, and outputs all the solutions.

The real number solution extraction unit 13 inputs all the solutions of the simultaneous polynomials calculated by the simultaneous polynomial solution unit 12, extracts all the real number solutions from the solutions, and outputs them. If no real number solution can be extracted, the real number solution extraction unit 13 stops the subsequent processing and outputs a no solution flag.

The unknown posture candidate conversion unit 14 receives all the real solutions extracted by the real number solution extraction unit 13 and calculates and outputs unknown posture candidates. The unknown posture candidate conversion unit 14 converts each real solution into a value that is treated as one unknown posture candidate in subsequent calculations.

The position candidate calculation unit 2 is a processing unit that receives the unknown posture candidate calculated by the unknown posture candidate calculation unit 1, calculates a relative position candidate of the camera, and outputs the calculated candidate.

The minimum error candidate extraction unit 3 inputs an unknown posture candidate, a position candidate, and a corresponding point, and one or a plurality of unknowns that minimize a predetermined error function representing a geometric relationship between the corresponding point, the unknown posture, and the position. Calculate and output posture and position. Further, the minimum error candidate extraction unit 3 may input the known posture when replacing the corresponding points using the known posture as described later.

In the present embodiment, unknown posture candidate calculation unit 1 (more specifically, coefficient calculation unit 11, simultaneous polynomial solution unit 12, real solution extraction unit 13, unknown posture candidate conversion unit 14), position candidate calculation unit 2, error The minimum candidate extraction unit 3 is realized by, for example, an information processing device such as hardware designed to perform specific arithmetic processing or the like, or a CPU (Central Processing Unit) that operates according to a program.

Next, the operation of the position / orientation estimation apparatus of this embodiment will be described. FIG. 2 is a flowchart showing an example of the operation of the first embodiment of the position / orientation estimation apparatus according to the present invention.

First, when three or more sets of corresponding points and two known postures are input, the coefficient calculation unit 11 uses a unknown point and a relative position of the camera as variables, and is represented by a predetermined point represented using the corresponding point and the unknown posture. The coefficient of the function (hereinafter referred to as simultaneous polynomial) is calculated and output to the simultaneous polynomial solving unit 12 (step S11).

Here, the posture of the camera is represented by a three-degree-of-freedom rotation matrix or three variables necessary to express the rotation matrix. The degree of freedom of the posture is an index indicating how many variables each of the nine elements of the 3 × 3 matrix representing the posture of the camera are represented. The two known poses are two known variables out of the three variables or a rotation matrix expressed by two degrees of freedom. The unknown pose is a rotation matrix expressed with one remaining variable or one degree of freedom.

In the present embodiment, the simultaneous polynomials for which the coefficient calculation unit 11 calculates coefficients are uniquely determined by how to express the degree of freedom of the camera posture. Therefore, the coefficient calculation unit 11 calculates the coefficient in the simultaneous polynomial using, for example, three or more pairs of corresponding points and two known postures in accordance with a predefined expression method of the degree of freedom of the camera posture or the type of the simultaneous polynomial. What is necessary is just to calculate. In addition, the coefficient calculation unit 11 predefines a plurality of simultaneous polynomial types corresponding to, for example, a method for expressing the degree of freedom of the camera posture, and uses the simultaneous polynomials that are used according to the values of the setting parameters read at the time of activation or the like It is also possible to select the type.

Next, the simultaneous polynomial solving unit 12 inputs the coefficient of the simultaneous polynomial calculated by the coefficient calculating unit 11, and solves the simultaneous polynomial using the corresponding point and the known posture input in step S11 (step S12). Further, the simultaneous polynomial solving unit 12 outputs all the solutions satisfying the simultaneous polynomial to the real number solution extracting unit 13. The solution of the simultaneous polynomial here includes an inappropriate value as an unknown posture. For example, some or all of them may be complex numbers.

When all the solutions of the simultaneous polynomials solved by the simultaneous polynomial solving unit 12 are input to the real number solution extracting unit 13, when there are real number solutions in those solutions (Yes in step S13), all the real number solutions are obtained. Extracted and output to the unknown posture candidate conversion unit 14. When no real number solution can be extracted from all the solutions of the simultaneous polynomials, the real number solution extraction unit 13 outputs a “no solution” flag as the position and orientation estimation result and ends the operation (No in step S13). Step S17). For example, if all the solutions are complex numbers, the real number solution extraction unit 13 outputs a “no solution” flag and ends the operation. The “no solution” flag may be a true / false value, for example, or may be a position / orientation value indicating no solution determined in advance.

When there is even one real number solution in the simultaneous polynomial, the unknown posture candidate conversion unit 14 inputs all the real number solutions and converts them into unknown posture candidates (step S14). The unknown posture candidates obtained as a result of the calculation are output to the position candidate calculation unit 2 and the minimum error candidate extraction unit 3. The unknown posture candidate conversion unit 14 may perform processing for converting the obtained real solution into values (one numerical value representing the unknown posture or a 3 × 3 rotation matrix with one degree of freedom) that are candidates for unknown postures. .

The position candidate calculation unit 2 receives all the unknown posture candidates obtained in step S14, calculates the relative positions of the cameras corresponding to the unknown postures, and extracts the minimum error candidate extraction unit 3 as the position candidates. (Step S15).

The minimum error candidate extraction unit 3 inputs an unknown posture candidate, a position candidate, and a corresponding point, and one or a plurality of unknowns that minimize a predetermined error function representing a geometric relationship between the corresponding point, the unknown posture, and the position. The posture and position are calculated and output (step S16). Further, the minimum error candidate extraction unit 3 does not need to input a known posture when replacing a corresponding point using a known posture as described later, and may input a known posture when not replacing.

The minimum error candidate extraction unit 3 outputs all of the input unknown posture candidates and position candidates as unknown postures and positions when the number of input corresponding points is three sets. When the number of input corresponding points is four or more, the minimum error candidate extraction unit 3 minimizes a predetermined error function from among all the input unknown posture candidates and position candidates. Calculate and output the position and unknown posture. The reason why this calculation is not performed when the number of corresponding points is three is that the error in all unknown posture candidates is 0, and it is impossible to determine which candidate is closest to the correct answer. On the other hand, when the number of corresponding points is four or more, since the error in each candidate is different, only one candidate with the smallest error can be selected.

(Example 1)
Hereinafter, the first embodiment will be described using a specific example. First, the epipolar equation with the unknown posture and the relative position of the camera as variables will be described. In the following, I is a unit matrix, det is a determinant, ∥∥ is an L2 norm of a vector, and [] _× represents a matrix representation of a cross product of three-dimensional vectors. Also, the number of input corresponding points is n sets, the i-th corresponding point coordinate of the first image is v _i , the i-th corresponding point coordinate of the second image is v ′ _i , and the camera such as focal length and optical center It is assumed that the internal parameters of are already calibrated and v _i and v ′ _i are homogenized. Further, it is assumed that the number of v _i and v ′ _i is equal. If the two known postures are α and β, and the unknown posture is θ, the relative posture of the camera is expressed by Equation (1) as a rotation matrix.

In this embodiment, the posture is expressed as in Expression (1), but can be arbitrarily expressed depending on how to define the axial direction, the rotational direction, and the order of multiplication. In this embodiment, the known posture is expressed by two parameters α and β. However, the known posture may be expressed using a unit quaternion, and as described in Non-Patent Document 2, a rotation axis and It may be expressed using a rotation amount.

When the relative position of the camera is represented by a three-dimensional vector t with two degrees of freedom, the corresponding points v _i , v ′ _i , the position t, and the posture R are represented as an equation (2) called an epipolar equation.

The degree of freedom of t is set to 2 due to the indefiniteness of the scale. Since α and β are already known, the epipolar equation is expressed as shown in Equation (3) when deformed by replacing v _i with R _α R _β v _i .

Since there are two unknowns of t in Equation (3) and three in θ, Equation (3) can be solved if there are a minimum of three corresponding points, and a least squares solution can be calculated if there are four or more pairs. First, the case of three sets will be described. When three sets of corresponding points are input, Expression (3) is expressed as Expression (4).

Here, A is a 3 × 3 matrix including θ as a variable. Equation (4) indicates that t is a zero vector or a null space of A. Since t is not a zero vector, t is represented as an eigenvector corresponding to the eigenvalue zero of A. That is, θ is a solution of a function in which θ is a variable and one eigenvalue of A is zero.

For example, since the eigenvalue of A is a solution of a cubic equation, the simultaneous polynomial solving unit 12 may use an equation in which the eigenvalue is written in a closed form with θ as a variable. In this case, the simultaneous polynomial solution unit 12 obtains θ by an iterative solution method such as Newton's method with an appropriate initial value. Since the eigenvalue is zero, the simultaneous polynomial solving unit 12 may obtain a solution with det (A) = 0. In order to obtain θ satisfying det (A) = 0, the simultaneous polynomial solving unit 12 replaces cos θ and sin θ of det (A) with two independent variables c and s, for example, as a new conditional expression c the ^² + ^s ² = 1 may be solved equation (5) was added.

Further, as described in Non-Patent Document 2, cos θ and sin θ can be expressed by one variable using a half-width formula. In this case, Equation (5) is not a simultaneous polynomial but a univariate polynomial. The simultaneous polynomial solving unit 12 substitutes the obtained θ for A, and calculates t as an eigenvector corresponding to the eigenvalue zero of A. At this time, since the absolute scale of t is unknown and has 2 degrees of freedom, the norm of t is normalized to 1.

Next, the case where four or more pairs of corresponding points are input will be described. When four or more sets of corresponding points are input, the expression (3) is expanded to be expressed as the expression (6).

Here, B is an n × 3 matrix that is expressed as in Expression (7) and includes θ as a variable.

As known from Non-Patent Document 1 and Non-Patent Document 3, the error represented by Equation (6) is the minimum eigenvalue of B ^T B, and t corresponds to the minimum eigenvalue of B ^T B, not the zero vector. It is an eigenvector. That is, θ is a solution of a function that minimizes one eigenvalue of B ^T B with θ as a variable. For example, since B ^T B is a 3 × 3 matrix, the eigenvalue of B ^T B can be written down with θ as a variable, and can be obtained by an iterative solution as in Equation (4). In addition, since one eigenvalue is the minimum, the simultaneous polynomial solving unit 12 may obtain a solution of the following equation (8).

In order to obtain θ satisfying Expression (8), the simultaneous polynomial solving unit 12 replaces cos θ and sin θ in Expression (8) with two independent variables c and s, for example, and c ² + s ² as a new conditional expression. You may solve the following formula | equation (9) which added = 1.

Similarly to equation (5), as described in Non-Patent Document 2, cos θ and sin θ can be expressed by one variable using a half-angle formula. In this case, Equation (9) is not a simultaneous polynomial but a univariate polynomial. The simultaneous polynomial solving unit 12 substitutes the obtained θ for B ^T B, and calculates t as an eigenvector corresponding to the minimum eigenvalue of B ^T B. As in the case of three sets, the norm of t is normalized to 1.

In the present embodiment, the case of 3 sets and 4 sets or more has been described separately, but also in the case of 3 sets, a solution similar to equation (5) can be obtained by solving equation (9). This is because the following equation (10) has a relationship, and therefore the solution of equation (9) includes the solution of equation (5).

Next, the operation of each part in the present embodiment will be specifically described. In step S11, the coefficient calculation unit 11 inputs corresponding points v _i and v _i ′ (3 ≦ i ≦ n) and two known postures (α, β), where n = In the case of 3, the coefficient of the simultaneous polynomial or the one-variable polynomial represented by the expression (5) or (9), and in the case of n ≧ 4, the expression (9) is calculated and output to the simultaneous polynomial solving unit 12.

The two known postures can be acquired using, for example, an acceleration sensor or a vanishing point. When using an acceleration sensor, the direction of gravity is measured with the camera stationary. Although the direction of gravity is represented by a three-dimensional vector, the magnitude of the norm is ignored, so the degree of freedom is 2. Therefore, the difference in the direction of gravity when two images are taken can be expressed by two parameters α and β. A vanishing point is a point where two or more parallel lines in a three-dimensional space intersect on an image by projective transformation. Non-patent document 2 describes a method for calculating a known posture from one vanishing point.

In step S12, the simultaneous polynomial solving unit 12 solves the simultaneous polynomial using the coefficient output from the coefficient calculating unit 11, and outputs the solution of the equation (5) or the equation (9) to the real solution extracting unit 13. In order to solve the simultaneous polynomial of Equation (5) or Equation (9), for example, a method of reducing to a single variable polynomial using a Sylvester matrix or a method of simultaneously calculating a solution of two variables using a Gröbner basis, etc. Various methods can be used.

In step S13, the real number solution extraction unit 13 inputs the solution of the simultaneous polynomial expressed by the equation (5) or the equation (9), calculates all the real number solutions from the solution, and determines the presence or absence of the real number solution. . If there is a real number solution, the real number solution extraction unit 13 outputs it to the unknown posture candidate conversion unit 14. If all the solutions are complex numbers, the real number solution extraction unit 13 outputs a no solution flag and ends the operation (step S17). The no solution flag may be, for example, a true / false value or a position / orientation value indicating no solution determined in advance.

In step S <b> 14, the unknown posture candidate conversion unit 14 inputs all real number solutions, calculates unknown posture candidates, and outputs them to the position candidate calculation unit 2 and the minimum error candidate extraction unit 3. Candidate unknown posture, for example, is given by R _theta of formula (1). For example, if there are k real solutions, it means that there are k θ, and the unknown posture candidate conversion unit 14 substitutes k θ one by one into the equation (1), and k unknown postures. Can be obtained (3 × 3 matrix).

In step S <b> 15, the position candidate calculation unit 2 calculates position candidates for all input unknown posture candidates and outputs the position candidates to the minimum error candidate extraction unit 3. The position candidate is an eigenvector corresponding to the eigenvalue zero of A or an eigenvector corresponding to the smallest eigenvalue of B ^T B. The eigenvector can be obtained by eigenvalue decomposition or singular value decomposition.

In step S16, first, the minimum error candidate extraction unit 3 inputs a corresponding point, an unknown posture candidate, and a position candidate. The minimum error candidate extraction unit 3 performs different operations according to the input number of corresponding points. In the case of n = 3, the minimum error candidate extraction unit 3 outputs all the unknown posture candidates and position candidates that have been input as unknown postures and positions. This is because det (A) = det (B ^T B) = 0, so ∥Bt∥ ² = 0, and it is impossible to determine which candidate is closest to the correct answer. When n ≧ 4 and one or more unknown posture candidates and position candidates are input, the minimum error candidate extraction unit 3 extracts the unknown posture candidate and the position candidate that minimize the minimum eigenvalue of B ^TB. Are output as unknown posture and position.

In the present embodiment, the position candidate calculation unit 2 narrows down the candidates for the optimal solution by batch processing for all real solutions, but it is also possible to sequentially process the solutions of simultaneous polynomials one by one. For example, each time the simultaneous polynomial solving unit 12 obtains one solution, the solution is output to the real number solution extracting unit 13. If the real solution extraction unit 13 determines that the solution is a real number, the conversion processing to the posture candidate by the unknown posture candidate conversion unit 14 and the minimum error candidate determination processing by the minimum error candidate extraction unit 3 are executed by loop processing. Then, the error minimum candidate extraction unit 3 stores, as the error minimum candidate determination process, a candidate that minimizes the error each time, and every time a real solution is extracted, error calculation, error minimum value comparison process, Overwriting processing of the minimum candidate and the minimum value may be performed. Here, the branching process based on the number of posture candidates and the number of input points may be collectively performed by the minimum error candidate extraction unit 3.

In this embodiment, the coefficient calculation unit 11 and the minimum error candidate extraction unit 3 each switch the processing by counting the number of corresponding points. However, depending on the determination result of the number of corresponding points by the coefficient calculation unit 11, the subsequent processing is performed. May be divided into two cases, n = 3 and n ≧ 4. In this case, if n = 3, the unknown posture candidate calculation unit 1 and the position candidate calculation unit 2 calculate Expression (5). If n ≧ 4, the unknown posture candidate calculation unit 1 and the position candidate calculation unit. 2 and the minimum error candidate extraction unit 3 calculate Expression (9).

In this embodiment, the minimum error candidate extraction unit 3 uses the minimum eigenvalue of B ^T B as the error, but may use det (B ^T B). In this case, next to the unknown posture candidate conversion unit 14, the minimum error candidate extraction unit 3 calculates det (B ^T B), selects only one candidate of the smallest unknown posture, and outputs it to the position candidate calculation unit 2. To do. As other errors, a Euclidean distance between the epipolar line and the corresponding point, or a Sampson error that is an approximation of the Euclidean distance may be used.

As described above, the position / orientation estimation apparatus according to the present embodiment first calculates all candidates having the smallest error based on the geometric relationship between the position and the unknown attitude, and extracts the candidate having the smallest error from the candidates. Therefore, it is guaranteed that the output unknown posture and position are the global minimum solution. In other words, unlike the method of performing correction after ignoring the constraint conditions as in Non-Patent Document 3, since the unknown posture and position candidates are calculated separately, no error increase due to the correction occurs.

Also, the position / orientation estimation apparatus according to the present embodiment does not require correction means and can reduce the amount of calculation. That is, it is not necessary to perform matrix operations such as singular value decomposition for correction as in Non-Patent Document 3.

Therefore, according to the position / orientation estimation apparatus of the present embodiment, the relative position / orientation of the camera can be estimated with high accuracy and stability at a high speed.

Embodiment 2. FIG.
A position / orientation estimation apparatus according to a second embodiment (Embodiment 2) will be described with reference to the drawings. FIG. 3 is a block diagram showing a configuration example of the second embodiment of the position / orientation estimation apparatus according to the present invention. The configuration of the position / orientation estimation apparatus of the present embodiment is different from the configuration of the first embodiment shown in FIG. 1 in that the unknown orientation candidate calculation unit 1 further includes a secondary optimality verification unit 4. Since the configuration other than the secondary optimality verification unit 4 is the same as that of the first embodiment, the description thereof is omitted.

The second-order optimality verification unit 4 receives the real number solution output from the real number solution extraction unit 13, verifies the positive / negative of the function obtained by second-order differentiation of the equation having the unknown pose as a variable with the unknown pose, and is a positive real number The solution is output to the unknown posture candidate conversion unit 14 as a candidate that satisfies the second optimality sufficient condition. In addition, when the verification result shows that there is no positive real number solution, the secondary optimality verification unit 4 stops the subsequent processing and outputs a no solution flag.

In the present embodiment, the secondary optimality verification unit 4 is realized by, for example, hardware designed to perform specific arithmetic processing or the like, or an information processing device such as a CPU that operates according to a program.

Next, the operation of the position / orientation estimation apparatus of this embodiment will be described. FIG. 4 is a flowchart showing an example of the operation of the position / orientation estimation apparatus of this embodiment. Since the operations other than step S21 are the same as those in the first embodiment, description thereof will be omitted.

The quadratic optimality verification unit 4 verifies the sign of the function obtained by second-order differentiation of the equation having the unknown pose as a variable with respect to the real solution of the simultaneous polynomial extracted by the real number solution extraction unit 21 (step) S21), a positive real number solution is output to the unknown posture candidate conversion unit 14 as a solution satisfying the second-order optimality sufficient condition. If all real solutions do not satisfy the secondary optimality sufficient condition, the secondary optimality verification unit 4 outputs a no-solution flag and ends the operation (No in step S21, step S17). The no solution flag may be a true / false value, for example, or may be a posture and position value representing no solution determined in advance.

(Example 2)
Next, the operation of each unit in the second embodiment will be specifically described. The operations other than the second optimality verification unit 4 are the same as those in the first embodiment. The second-order optimality verification unit 4 inputs the real number solution output from the real number solution extraction unit 13, and verifies the positive / negative by substituting the real number solution into the function obtained by second-order differentiation of the equation with the unknown posture as a variable with the unknown posture. To do. Here, the equation using the unknown posture as a variable may be, for example, an eigenvalue of (B ^T B) written with θ as a variable, or may be det (B ^T B).

If the function into which the real solution is substituted is positive, the secondary optimality verification unit 4 outputs the solution to the unknown posture candidate conversion unit 14 as a solution satisfying the secondary optimality sufficient condition (Yes in step S21). If none is positive, the second-order optimality verification unit 4 outputs a no-solution flag in the same manner as the real solution extraction unit 13, and ends (No in step S21, step S17).

In the position / orientation estimation apparatus of this embodiment, the second-order optimality verification unit 4 eliminates inappropriate solutions called saddle points that satisfy simultaneous polynomials but are not locally optimal solutions. When the saddle point is deleted, the real number solution obtained by the second-order optimality verification unit 4 is always a local optimum solution. Therefore, it is guaranteed that the solution having the smallest error among them is a global optimum solution. In addition, when the real number solution obtained by the secondary optimality verification unit 4 does not exist, the calculation cost can be reduced by terminating the subsequent processing.

Therefore, according to the position and orientation estimation apparatus of the present embodiment, the reliability of the position and orientation output as the estimation result can be further improved, and the relative position and orientation of the camera can be estimated at a higher speed.

Embodiment 3. FIG.
A position / orientation estimation apparatus according to a third embodiment of the present invention will be described with reference to the drawings. FIG. 5 is a block diagram showing a configuration example of the third embodiment of the position / orientation estimation apparatus according to the present invention. The position / orientation estimation apparatus of the present embodiment further includes a three-dimensional shape restoration unit 5 in addition to the configuration of the first embodiment shown in FIG. 1 or the second embodiment shown in FIG. Since the configuration other than the three-dimensional shape restoration unit 5 is the same as that of the first embodiment or the second embodiment, the description thereof is omitted.

The 3D shape restoration unit 5 inputs the corresponding point and the known posture, and the position and unknown posture output by the minimum error candidate extraction unit 3, and restores and outputs the three-dimensional coordinates of the corresponding point. The three-dimensional shape restoration unit 5 outputs the position and orientation together with the restored three-dimensional coordinates. Here, the posture may be, for example, three parameters or a rotation matrix.

In the present embodiment, the three-dimensional shape restoration unit 5 is realized by, for example, hardware designed to perform specific arithmetic processing or the like, or an information processing apparatus such as a CPU that operates according to a program.

Next, the operation of the position / orientation estimation apparatus of this embodiment will be described. FIG. 6 is a flowchart illustrating an example of the operation of the position / orientation estimation apparatus according to this embodiment. Hereinafter, since operations other than step S31 are the same as those in the second embodiment, description thereof will be omitted. Note that when the configuration of the position / orientation estimation apparatus of the present embodiment is obtained by adding the three-dimensional shape restoration unit 5 to the configuration of the first embodiment, step S21 in FIG. 6 is unnecessary.

The three-dimensional shape restoration unit 5 receives the corresponding point, the known posture, the position and the unknown posture output by the minimum error candidate extraction unit 3, restores the three-dimensional coordinates of the corresponding point, and outputs the corresponding point and the posture. (Step S31).

(Example 3)
Next, the operation of each part in this embodiment will be specifically described. The operations other than the three-dimensional shape restoration unit 5 are the same as those in the first embodiment or the second embodiment.

The 3D shape restoration unit 5 receives the corresponding points, the known postures, and the positions and unknown postures output by the minimum error candidate extraction unit 3, and restores and outputs the three-dimensional coordinates of the corresponding points. The three-dimensional shape restoration unit 5 outputs the position and orientation together with the restored three-dimensional coordinates. Here, the posture may be, for example, three parameters or a rotation matrix. The three-dimensional shape restoration unit 5 may output only the three-dimensional coordinates without outputting the position and orientation.

A method for restoring the three-dimensional coordinates of corresponding points based on the relative position and orientation of the camera is described in Non-Patent Document 4 and Non-Patent Document 5, for example.

Note that the position / orientation estimation apparatus of each of the above embodiments can be realized by, for example, hardware corresponding to each unit, and can also be realized by an information processing system. FIG. 7 is a block diagram when the position / orientation estimation apparatus according to the present invention is implemented in an information processing system. The information processing system shown in FIG. 7 is a general information processing system including a processor 61, a program memory 62, and a storage medium 63. The storage medium 63 may be a storage area composed of separate storage media, or may be a storage area composed of the same storage medium. As the storage medium, a magnetic storage medium such as a RAM (Random Access Memory) or a hard disk can be used.

The program memory 62 includes the above-described unknown posture candidate calculation unit 1 (more specifically, the coefficient calculation unit 11, the simultaneous polynomial solution unit 12, the real number solution extraction unit 13, and the unknown posture candidate conversion unit 14), Stored is a program for causing the processor 61 to perform processing of each of the position candidate calculation unit 2, the minimum error candidate extraction unit 3, the secondary optimality verification unit 4, and the three-dimensional shape restoration unit 5, The processor 61 operates according to this program. The processor 61 may be a processor that operates according to a program such as a CPU, for example. Thus, the present invention can also be realized by a computer program. Note that it is not necessary to operate all the parts that can be operated by the program by the program, and a part may be configured by hardware. Moreover, you may implement | achieve as a separate unit, respectively.

As mentioned above, although this invention was demonstrated with reference to embodiment and an Example, this invention is not limited to the said embodiment and Example. Various changes that can be understood by those skilled in the art can be made to the configuration and details of the present invention within the scope of the present invention.

FIG. 8 is a block diagram showing a configuration of a main part of the position / orientation estimation apparatus according to the present invention. As shown in FIG. 8, the position / orientation estimation apparatus according to the present invention has, as main components, three or more sets of corresponding points included in two images and the relative orientation of the camera that captured the two images. The known posture, which is a known two posture parameter among the three posture parameters representing the input, is input, and the corresponding posture and the known posture are used to input one of the posture parameters and the unknown posture parameter. Unknown posture candidate calculation that outputs the solutions of all unknown postures satisfying a predetermined function expressed using the unknown posture and corresponding points using the unknown posture and the relative position of the camera as variables Unit 1, position candidate calculation unit 2 for calculating a candidate for the relative position of the camera with respect to each of the unknown posture candidates, and the relative position of the camera with respect to each of the unknown posture candidates and the unknown posture candidates. Complements and corresponding points are input, and one or a minimum of a predetermined error function representing a geometric relationship between the relative position of the corresponding point and the camera and the unknown posture is selected from the unknown posture candidates and the position candidates A plurality of unknown postures, and a minimum error candidate extraction unit 3 that extracts a relative position of the camera.

In the above-described embodiment, a position / orientation estimation apparatus as shown in the following (1) to (3) is also disclosed.

(1) In the position / orientation estimation apparatus, an unknown posture candidate calculation unit (for example, unknown posture candidate calculation unit 1) inputs a corresponding point and a known posture, and is represented using a predetermined posture represented by the unknown posture and the corresponding point. A coefficient calculation unit (for example, the coefficient calculation unit 11) that calculates the coefficient of the function, a simultaneous polynomial solution unit (for example, the simultaneous polynomial solution unit 12) that inputs all the coefficients and calculates all the solutions that satisfy the predetermined function; A real solution extraction unit (for example, a real solution extraction unit 13) that extracts all real solutions from the solutions obtained by the simultaneous polynomial solution unit and outputs a flag indicating all the extracted real solutions or no solution; It is configured to include an unknown posture candidate conversion unit (for example, unknown posture candidate conversion unit 14) that converts each real number solution of all unknown postures extracted by the real number solution extraction unit into one unknown posture candidate. May be.

(2) In the position / orientation estimation device, the unknown orientation candidate calculation unit calculates and outputs a real number solution satisfying the second-order optimality condition among all the real number solutions satisfying a predetermined function, and outputs the second-order optimality verification (For example, secondary optimality verification unit 4) may be included. According to such a position / orientation estimation apparatus, the reliability of the position / orientation output as the estimation result can be further improved, and the relative position / orientation of the camera can be estimated at a higher speed.

(3) The position / orientation estimation apparatus inputs the corresponding point, the known attitude, the relative position of the camera output by the minimum error candidate extraction unit, and the unknown attitude, and restores the three-dimensional coordinates of the corresponding point. The output three-dimensional shape restoration unit (for example, the three-dimensional shape restoration unit 5) may be provided.

This application claims priority based on Japanese Patent Application No. 2012-191262 filed on August 31, 2012, the entire disclosure of which is incorporated herein.

Although the present invention has been described with reference to the embodiments and examples, the present invention is not limited to the above embodiments and examples. Various changes that can be understood by those skilled in the art can be made to the configuration and details of the present invention within the scope of the present invention.

Industrial applicability

The present invention is applied to the reconstruction of the three-dimensional shape of a subject, the generation of a panoramic image of a background, and the like.

DESCRIPTION OF SYMBOLS 1 Unknown posture candidate calculation part 2 Position candidate calculation part 3 Error minimum candidate extraction part 4 Secondary optimality verification part 5 Three-dimensional shape restoration part 11 Coefficient calculation part 12 Simultaneous polynomial solution part 13 Real number solution extraction part 14 Unknown posture candidate conversion part 21 Real Solution Extractor 61 Processor 62 Program Memory 63 Storage Medium

Claims

Input three or more sets of corresponding points included in the two images and known postures that are two known posture parameters among the three posture parameters representing the relative posture of the camera that captured the two images. Then, using the input corresponding points and the known posture, the unknown posture, which is one of the posture parameters and is an unknown posture parameter, and the relative position of the camera are used as variables. An unknown posture candidate calculation unit that outputs, as candidates for unknown postures, solutions of all the unknown postures that satisfy a predetermined function expressed using posture and the corresponding points;
A position candidate calculation unit for calculating a candidate for a relative position of the camera with respect to each of the unknown posture candidates;
All of the unknown posture candidates, the relative position candidates of the camera with respect to each of the unknown posture candidates, and the corresponding points are input, and the corresponding points are selected from the unknown posture candidates and the position candidates. One or more unknown postures that minimize a predetermined error function representing a geometric relationship between the relative position of the camera and the unknown posture, and a minimum error candidate extraction that extracts the relative position of the camera And a position and orientation estimation device.
The unknown pose candidate calculation unit
A coefficient calculation unit that inputs a corresponding point and a known posture, and calculates a coefficient of a predetermined function expressed using the unknown posture and the corresponding point;
A simultaneous polynomial solver that inputs the coefficients and calculates all solutions that satisfy the predetermined function;
A real solution extraction unit that extracts all real solutions from the solutions obtained by the simultaneous polynomial solution unit and outputs a flag indicating all extracted real solutions or no solution;
The position and orientation estimation apparatus according to claim 1, further comprising: an unknown posture candidate conversion unit that converts all of the real solutions of the unknown posture extracted by the real number solution extraction unit into one unknown posture candidate.
The unknown pose candidate calculation unit
The position / orientation estimation according to claim 1, further comprising: a second-order optimality verification unit that calculates and outputs a real-number solution satisfying a second-order optimality condition among all real-number solutions satisfying a predetermined function. apparatus.
A corresponding point, a known posture, a relative position and an unknown posture of the camera output by the minimum error candidate extracting unit, a three-dimensional shape restoring unit for restoring and outputting the three-dimensional coordinates of the corresponding point; The position and orientation estimation apparatus according to any one of claims 1 to 3.
Input three or more sets of corresponding points included in the two images and known postures that are two known posture parameters among the three posture parameters representing the relative posture of the camera that captured the two images. Then, using the input corresponding points and the known posture, the unknown posture, which is one of the posture parameters and is an unknown posture parameter, and the relative position of the camera are used as variables. Output all the unknown posture solutions satisfying a predetermined function represented by the posture and the corresponding points as candidates for unknown posture,
Calculating a relative position candidate of the camera for each of the unknown pose candidates;
All of the unknown posture candidates, the relative position candidates of the camera with respect to each of the unknown posture candidates, and the corresponding points are input, and the corresponding points are selected from the unknown posture candidates and the position candidates. Extracting one or a plurality of unknown poses and a relative position of the cameras that minimize a predetermined error function representing a geometric relationship between the relative position of the camera and the unknown pose. Position and orientation estimation method.
On the computer,
Input three or more sets of corresponding points included in the two images and known postures that are two known posture parameters among the three posture parameters representing the relative posture of the camera that captured the two images. Then, using the input corresponding points and the known posture, the unknown posture, which is one of the posture parameters and is an unknown posture parameter, and the relative position of the camera are used as variables. An unknown posture candidate calculation process for outputting, as candidates for unknown postures, solutions of all the unknown postures that satisfy a predetermined function expressed using posture and the corresponding points;
A position candidate calculation process for calculating a candidate for a relative position of the camera with respect to each of the unknown posture candidates;
All of the unknown posture candidates, the relative position candidates of the camera with respect to each of the unknown posture candidates, and the corresponding points are input, and the corresponding points are selected from the unknown posture candidates and the position candidates. One or more unknown postures that minimize a predetermined error function representing a geometric relationship between the relative position of the camera and the unknown posture, and a minimum error candidate extraction that extracts the relative position of the camera A position and orientation estimation program for executing and processing.