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

1. A position/orientation estimation apparatus configured to estimate a position and an orientation of an object, the position/orientation estimation apparatus comprising:

a processor configured with a program to perform operations comprising:

operation as an image acquisition unit configured to acquire a two-dimensional image of the object;

operation as a 3D model acquisition unit configured to acquire a three-dimensional model expressing a three-dimensional shape of the object; and

operation as a pre-update position/orientation acquisition unit configured to acquire position/orientation information expressing a pre-update position and orientation of the object, which is estimated;

operation as a second position/orientation estimation unit configured to update the position/orientation information with regard to a translation in an X-axis and a translation in a Y-axis of the two-dimensional image, and a rotation about a Z-axis that is orthogonal to the X-axis and the Y-axis, using the three-dimensional model and the two-dimensional image; and

operation as an output unit configured to output the position and the orientation expressed by the updated position/orientation information as the position and the orientation of the object, wherein

the processor is configured with the program such that operation as the second position/orientation estimation unit comprises:

operation as a two-dimensional model generation unit configured to generate a two-dimensional model projected on a same plane as that of the two-dimensional image, based on the position/orientation information;

operation as an optimization unit configured to optimize a translation parameter in the X-axis, a translation parameter in the Y-axis, and a rotation parameter about the Z-axis by matching the positions of the two-dimensional image and the two-dimensional model; and

operation as an updating unit configured to update the position/orientation information using the optimized three parameters.

2. The position/orientation estimation apparatus according to claim 1 , wherein

the processor is configured with the program to perform operations such that operation as the first position/orientation estimation unit comprises:

receiving the position/orientation information acquired by the pre-update position/orientation acquisition unit; and

transmitting the position/orientation information updated by the first position/orientation estimation unit to the second position/orientation estimation unit.

3. The position/orientation estimation apparatus according to claim 2 ,

wherein the processor is configured with the program to perform operations such that operation as the first position/orientation estimation unit comprises optimizing the translation parameter in the X-axis, the translation parameter in the Y-axis, the translation parameter in the Z-axis, a rotation parameter about the X-axis, a rotation parameter about the Y-axis, and a rotation parameter about the Z-axis so as to bring points of the three-dimensional model corresponding to points of the three-dimensional coordinate data closer to the corresponding points of the three-dimensional coordinate data, and updates the position/orientation information using the optimized six parameters.

4. The position/orientation estimation apparatus according to claim 2 ,

wherein the processor is configured with the program to perform operation such that operation as the first position/orientation estimation unit comprises optimizing the translation parameter in the Z-axis, a rotation parameter about the X-axis, and a rotation parameter about the Y-axis so as to bring points of the three-dimensional model corresponding to points of the three-dimensional coordinate data closer to the corresponding points of the three-dimensional coordinate data, and updates the position/orientation information using the optimized three parameters.

5. The position/orientation estimation apparatus according to claim 1 , wherein

the processor is configured with the program to perform operations such that operation as the first position/orientation estimation unit comprises:

receiving the position/orientation information updated by the second position/orientation estimation unit; and

transmitting the position/orientation information updated by the first position/orientation estimation unit to the output unit.

6. The position/orientation estimation apparatus according to claim 5 ,

wherein the processor is configured with the program to perform operation such that operation as the first position/orientation estimation unit comprises optimizing the translation parameter in the X-axis, the translation parameter in the Y-axis, the translation parameter in the Z-axis, a rotation parameter about the X-axis, a rotation parameter about the Y-axis, and a rotation parameter about the Z-axis so as to bring points of the three-dimensional model corresponding to points of the three-dimensional coordinate data closer to the corresponding points of the three-dimensional coordinate data, and updates the position/orientation information using the optimized six parameters.

7. The position/orientation estimation apparatus according to claim 5 ,

wherein the processor is configured with the program to perform operation such that operation as the first position/orientation estimation unit comprises optimizing the translation parameter in the Z-axis, a rotation parameter about the X-axis, and a rotation parameter about the Y-axis so as to bring points of the three-dimensional model corresponding to points of the three-dimensional coordinate data closer to the corresponding points of the three-dimensional coordinate data, and updates the position/orientation information using the optimized three parameters.

8. The position/orientation estimation apparatus according to claim 1 , wherein the processor is configured with the program such that:

the position/orientation information acquired by the pre-update position/orientation acquisition unit is transmitted to the first position/orientation estimation unit and the second position/orientation estimation unit;

the position/orientation information that is updated by the first position/orientation estimation unit and the position/orientation information that is updated by the second position/orientation estimation unit are transmitted to the output unit;

operation as the output unit comprises outputting the position and the orientation of the object based on the position/orientation information that is updated by the first position/orientation estimation unit and the position/orientation information that is updated by the second position/orientation estimation unit.

9. The position/orientation estimation apparatus according to claim 8 ,

wherein the processor is configured with the program to perform operation such that operation as the first position/orientation estimation unit comprises optimizing the translation parameter in the X-axis, the translation parameter in the Y-axis, the translation parameter in the Z-axis, a rotation parameter about the X-axis, a rotation parameter about the Y-axis, and a rotation parameter about the Z-axis so as to bring points of the three-dimensional model corresponding to points of the three-dimensional coordinate data closer to the corresponding points of the three-dimensional coordinate data, and updates the position/orientation information using the optimized six parameters.

10. The position/orientation estimation apparatus according to claim 8 ,

wherein the processor is configured with the program to perform operation such that operation as the first position/orientation estimation unit comprises optimizing the translation parameter in the Z-axis, a rotation parameter about the X-axis, and a rotation parameter about the Y-axis so as to bring points of the three-dimensional model corresponding to points of the three-dimensional coordinate data closer to the corresponding points of the three-dimensional coordinate data, and updates the position/orientation information using the optimized three parameters.

11. A method for estimating a position and an orientation of an object, the method comprising:

acquiring a two-dimensional image of the object;

acquiring three-dimensional data expressing measured three-dimensional coordinates of a surface of the object;

acquiring a three-dimensional model expressing a three-dimensional shape of the object; and

acquiring position/orientation information expressing a pre-update position and orientation of the object, which is estimated;

updating the position/orientation information based on the three-dimensional model and the three-dimensional data;

updating the position/orientation information with regard to a translation in an X-axis and a translation in a Y-axis of the two-dimensional image, and a rotation about a Z-axis that is orthogonal to the X-axis and the Y-axis, using the three-dimensional model and the two-dimensional image;

outputting the position and the orientation expressed by the updated position/orientation information as the position and the orientation of the object;

generating a two-dimensional model projected on a same plane as that of the two-dimensional image, based on the position/orientation information;

optimizing a translation parameter in the X-axis, a translation parameter in the Y-axis, and a rotation parameter about the Z-axis by matching the positions of the two-dimensional image and the two-dimensional model; and

updating the position/orientation information using the optimized three parameters.

12. The method according to claim 11 , wherein

updating the position/orientation information based on the three-dimensional model occurs prior to updating the position/orientation information with regard to the translation in the X-axis, the translation in the Y-axis, and the rotation about the Z-axis.

13. The method according to claim 11 , wherein

updating the position/orientation information based on the three-dimensional model occurs after updating the position/orientation information with regard to the translation in the X-axis, the translation in the Y-axis, and the rotation about the Z-axis.

14. A non-transitory computer-readable storage medium storing a program for causing a computer to estimate a position and an orientation of an object, the program, when read and executed, causing the computer to perform operations comprising:

acquiring a two-dimensional image of the object;

acquiring three-dimensional data expressing measured three-dimensional coordinates of a surface of the object;

acquiring a three-dimensional model expressing a three-dimensional shape of the object;

acquiring position/orientation information expressing pre-update position and orientation of the object, which is estimated;

updating the position/orientation information based on the three-dimensional model and the three-dimensional data;

updating the position/orientation information with regard to a translation in an X-axis and a translation in a Y-axis of the two-dimensional image, and a rotation about a Z-axis that is orthogonal to the X-axis and the Y-axis, using the three-dimensional model and the two-dimensional image;

outputting the position and the orientation expressed by the updated position/orientation information as the position and the orientation of the object;

generating a two-dimensional model projected on a same plane as that of the two-dimensional image, based on the position/orientation information;

optimizing a translation parameter in the X-axis, a translation parameter in the Y-axis, and a rotation parameter about the Z-axis by matching the positions of the two-dimensional image and the two-dimensional model; and

updating the position/orientation information using the optimized three parameters.