# Patent

## Title
Method and apparatus for processing information

## Abstract

An objective viewpoint image of an object captured at an objective viewpoint position is obtained, a measurement value of a sensor is received, information about the image coordinates of calibrating indices is detected from an objective viewpoint image, and two kinds of arrangement information of the calibrating indices and the sensor both placed on and relative to the object are obtained on the basis of the measurement value and the information about the image coordinates of the calibrating indices. With this, the two kinds of arrangement information are easily and accurately obtained at the same time.

## Background
BACKGROUND OF THE INVENTION
1. Field of the Invention
The present invention relates to a method and an apparatus for obtaining arrangement information of indices and a sensor both placed on and relative to an object.
2. Description of the Related Art
By setting a plurality of indices on a measuring object; capturing the measuring object with a camera; and detecting the image coordinates of the projected figure of each index in the captured image, the position and the orientation of the measuring object relative to the camera or in the space having the camera installed therein are obtained (see, e.g., D. G. Lowe, “Fitting Parameterized three-dimensional models to images”, IEEE Transactions on PAMI, vol. 13, no. 5, pp. 441-450, 1991 (hereinafter “Lowe”), and A. I. Comport, E. Marchard, F. Chaumette: “A real-time tracker for markerless augmented reality”, Proc. Int'l Symp. On Mixed and Augmented Reality 2004, pp. 36-45, 2004 (hereinafter “Comport et al.”).
In K. Satoh, S. Uchiyama, and H. Yamamoto: “A head tracking method using bird's-eye view camera and gyroscope”, Proc. 3rd IEEE/ACM Int'l Symp. on Mixed and Augmented Reality (ISMAR 2004), PP. 202-211, 2004 (hereinafter “Satoh et al.”), a method for measuring the position and the orientation of an object is proposed. In this method, indices and an orientation sensor are placed on a measuring object, and an objective-viewpoint camera is placed to observe the measuring object at an objective (third-person) viewpoint. The position and the orientation of the object are measured on the basis of figures of the indices detected from an image captured by the objective-viewpoint camera, and an orientation-measurement value of an orientation sensor. As for the case where an imaging device is a measuring object, a method has been proposed for measuring the position and the orientation of an object (by a group including the inventors of the present invention and others). In this method, indices and an orientation sensor are placed on an imaging device, an objective-viewpoint camera is placed to observe the imaging device, and other indices are arranged in a scene to be observed by the imaging device. The position and the orientation of the imaging device are measured on the basis of figures of the indices on the imaging device detected from an image captured by the objective-viewpoint camera; figures of the indices in a scene detected from an image captured by the imaging device itself; and an orientation-measurement value of an orientation sensor.
Regarding the method in which the position and the orientation of a measuring object are computed by detecting figures of a plurality of indices set on the object, the positional relationship between the indices and the object must be previously known. In the case where an orientation sensor is used for measuring the position and the orientation of an object as set forth in Satoh et al., the orientation of the orientation sensor relative to the measuring object must be previously known.
Hitherto, works for obtaining these positional relationships, i.e., two kinds of arrangement information of the indices and the sensor relative to the object, are independently conducted (hereinafter, a work generally referred to as calibration). That is, two kinds of calibration of an index and an orientation sensor are conducted in mutually different steps. Unfortunately, mutually independent calibration of them causes troublesome works.
SUMMARY OF THE INVENTION
In view of the above problems, the present invention is directed to easily and accurately obtain two kinds of arrangement information of calibrating indices and a sensor both placed on and relative to an object at the same time.
According to one aspect of the present invention, an information processing method for obtaining two kinds of arrangement information of calibrating indices and a sensor both placed on and relative to an object so as to serve as calibration information includes steps of obtaining a captured image of the object; receiving a measurement value of the sensor; detecting information about the image coordinates of the respective calibrating indices, from the image; and obtaining calibration information based on the measurement value and the information about the image coordinates of the respective calibrating indices.
According to another aspect of the present invention, an information processing method for computing the positions of respective calibrating indices and the orientation of an orientation sensor both placed on and relative to an imaging device so as to serve as calibration information includes steps of obtaining a first captured image by the other imaging section; obtaining a second image captured by the imaging device; receiving a measurement value of the orientation sensor; detecting information about the image coordinates of the respective calibrating indices, from the first image; detecting information about the image coordinates of respective reference indices arranged in a scene, from the second image; and computing calibration information based on the measurement value and the information about the image coordinates of the respective calibrating indices and the respective reference indices.
Other features and aspects of the present invention will be apparent from the following description taken in conjunction with the accompanying drawings, in which like reference characters designate the same or similar parts throughout the figures thereof.



BRIEF DESCRIPTION OF THE DRAWINGS
The accompanying drawings, which are incorporated in and constitute a part of the specification, illustrate embodiments of the invention and, together with the description, serve to explain the principles of the invention.
FIG. 1 shows an exemplary structure of a calibration apparatus according to a first embodiment of the present invention.
FIG. 2 is a flowchart of an exemplary process of a calibration method according to the first embodiment.
FIG. 3 shows an exemplary structure of a calibration apparatus according to a second embodiment of the present invention.
FIG. 4 is a flowchart of an exemplary process of a calibration method according to the second embodiment.
FIG. 5 shows an exemplary structure of indices according to the second embodiment.
FIG. 6 shows an exemplary structure of indices according to a modification of the second embodiment.



DESCRIPTION OF THE EMBODIMENTS
Exemplary embodiments of the present invention will now be described in detail in accordance with the accompanying drawings.
First Embodiment
A calibration apparatus according to a first embodiment obtains two kinds of arrangement information of indices and an orientation sensor both placed on and relative to an imaging device. The calibration apparatus and a calibration method according to the present embodiment will be described below.
FIG. 1 shows an exemplary structure of a calibration apparatus 100 according to the present embodiment. As shown in FIG. 1, the calibration apparatus 100 according to the present embodiment includes a subjective-viewpoint-index detecting section 110, a sensor-measurement-value inputting section 120, an objective-viewpoint camera 140, an objective-viewpoint-index detecting section 150, an instruction section 160, a data-managing section 170, and a calibration-information computing section 180 and is connected to a calibrating imaging-device 130 and an orientation sensor 190, which will be described later.
It is presumed that the imaging device 130 has the calibrating orientation sensor 190 placed thereon and two calibrating indices P1 and P2 set thereon (hereinafter, called objective viewpoint indices). While the orientation of the orientation sensor 190 and the positions of the respective objective viewpoint indices are unknown in the subjective-viewpoint-camera coordinate system (the coordinate system of the imaging device 130, in which one point on the imaging device 130 is defined as an origin and three axes orthogonal to one another are defined as the X-, Y-, and Z-axes), the unknown orientation of the orientation sensor and positions of the respective objective viewpoint indices in the subjective-viewpoint-camera coordinate system serve as information to be calibrated by the calibration apparatus 100, that is, an output of the calibration apparatus 100 according to the present embodiment.
According to the present embodiment, since the imaging device 130 serving as a measuring object has the indices arranged thereon and the orientation sensor 190 placed thereon, calibration information is configured by two kinds of arrangement information of the indices and the orientation sensor.
A plurality of indices Qk (k=1 . . . Kn) serving as indices to be captured by the imaging device 130 (hereinafter, an index is referred to as a subjective viewpoint index (a reference index)) is arranged at a plurality of positions in the physical space, wherein the positions of the indices in the world coordinate system (the coordinate system in which one point in the physical space is defined as an origin and three axes orthogonal to one another are respectively defined as the X-, the Y-, and the Z-axes) are known. The subjective viewpoint indices Qk should be arranged such that at least three of them can be observed by the imaging device 130 upon obtaining calibrating data. In an example situation shown in FIG. 1, the four subjective viewpoint indices Q1, Q2, Q3 and Q4 are arranged, and three of them (i.e., Q1, Q3, and Q4) are present in the field of view of the imaging device.
The subjective viewpoint indices Qk may be configured by, for example, circular markers having respectively different colors or feature points such as natural features having respectively different texture features. Alternatively, quadrangular indices, each formed by a quadrangular region having a certain area, may be used as the indices. The indices may be of any type as long as the image coordinates of a projected figure on a captured image can be detected and the indices can be individually identified.
The subjective-viewpoint-index detecting section 110 receives an image output by and from the imaging device 130 (hereinafter, this image is referred to as a subjective viewpoint image) and detects the image coordinates of the subjective viewpoint indices Qk in the received captured image. For example, when the subjective viewpoint indices Qk are configured by markers having respectively different colors, regions corresponding to the respective colors of the markers are detected from the subjective viewpoint image, and the positions of the center of gravity of the regions are set as the detecting coordinates of the indices. When the subjective viewpoint indices Qk are configured by feature points having respectively different texture features, the positions of the indices are detected by applying template-matching on the subjective viewpoint image with the aid of template images of the respective indices previously stored as known information. When the quadrangular indices are used as reference indices, the captured image is binaries, labeling is then performed, and index candidates formed by four straight lines are detected among labeled regions having at least a predetermined area. In addition, determination whether a particular pattern is present in the candidate region allows fault detection to be eliminated. Furthermore, the direction and the identifier of the index are also obtained by the pattern inside the region. Finally, image coordinates of four vertices of the square are detected. Herein, the quadrangular indices detected as described above are considered as four indices formed by four apexes.
The subjective-viewpoint-index detecting section 110 outputs image coordinates uQkn and identifiers kn of the corresponding detected subjective viewpoint indices Qkn to the data-managing section 170 in response to its request, wherein n (=1 . . . N) stands for an index of the corresponding detected index, and N stands for the total number of the detected indices. For example, in the case shown in FIG. 1, while N=3, the identifiers: k1=1, k2=3, and k3=4, and the image coordinates: uQk1, uQk2, and uQk3 respectively corresponding to these identifiers are output. While the subjective-viewpoint-index detecting section 110 according to the present embodiment sequentially executes the above-described index-detecting process upon receipt of an image from the imaging device 130 as a trigger, the detecting section 110 may execute the process on the basis of a request of the data-managing section 170 (on the basis of a subjective viewpoint image received at that time).
The sensor-measurement-value inputting section 120 receives an orientation-measurement value from the orientation sensor 190 and outputs it to the data-managing section 170 in response to its request. While a value output by the orientation sensor 190 generally indicates the orientation of the sensor in the sensor coordinate system uniquely defined by the sensor, the sensor-measurement-value inputting section 120 transforms it into the orientation of the sensor in the world coordinate system by multiplying its known transformation matrix from the sensor coordinate system to the world coordinate system and outputs the transformed rotation matrix (hereinafter, written by Rws) to the data-managing section 170.
The objective-viewpoint camera 140 is arranged at a fixed (static) position such that the imaging device 130 is capturable. It is presumed that the position and the orientation of the objective-viewpoint camera 140 in the world coordinate system are previously stored in the calibration-information computing section 180 as known values.
The objective-viewpoint-index detecting section 150 receives an image captured by the objective-viewpoint camera 140 (hereinafter, the image is called as an objective viewpoint image); detects the image coordinates of the corresponding objective viewpoint indices in the captured image in a similar way as done by the subjective-viewpoint-index detecting section 110 (in accordance with kinds of objective viewpoint indices P1 and P2 to be calibrated); and outputs detected image coordinates uP1 and uP2 to the data-managing section 170 in response to its request. While the objective-viewpoint-index detecting section 150 according to the present embodiment sequentially executes the above-described index-detecting process upon receipt of an objective viewpoint image as a trigger, the objective-viewpoint-index detecting section 150 may execute the process in response to request of the data-managing section 170 (on the basis of an objective viewpoint image received at that time).
The instruction section 160 transmits an instruction of obtaining data to the data-managing section 170 upon receipt of a data-obtaining command input by an operator (not shown). The instruction section 160 also transmits another instruction of computing calibration information to the calibration-information computing section 180 upon receipt of a calibration-information-computing command input by the operator. A command can be input to the instruction section 160, for example, with the aid of a keyboard and pressing a key of the keyboard having a particular command allocated thereto, or by the aid of a graphic user interface (GUI) displayed on a monitor.
Upon receipt of the instruction of obtaining data from the instruction section 160, the data-managing section 170 receives (i) the image coordinates and the identifier of each of the subjective viewpoint indices from the subjective-viewpoint-index detecting section 110, (ii) a sensor-measurement value Rws from the sensor-measurement-value inputting section 120, and (iii) the image coordinates uP1 and uP2 of the objective viewpoint indices from the objective-viewpoint-index detecting section 150. Then, the data-managing section 170 creates a plurality of data sets, the number of which is that of the received subjective viewpoint indices, wherein each data set is configured by (a) detection information (the identifier and the image coordinates) of a subjective viewpoint index at a certain time, (b) the image coordinates of all objective viewpoint indices detected at the same time, and (c) a sensor-measurement value at the same time. Then, the data set is added to a data list for storage. Also, the data-managing section 170 outputs the stored data list to the calibration-information computing section 180 in response to its request.
Upon receipt of an instruction of computing calibration information from the instruction section 160, the calibration-information computing section 180 receives the data list from the data-managing section 170, performs a calibration process on the basis of the data list, and outputs resultantly obtained calibration information (i.e., the positions of each of the objective viewpoint indices in the subjective-viewpoint-camera coordinate system and the orientation of the orientation sensor 190 in the subjective-viewpoint-camera coordinate system.
FIG. 2 is a flowchart of a calibration process of the calibration apparatus according to the present embodiment performed for obtaining calibration information. Program code in accordance with the flowchart is stored in memories (not shown) such as a random access memory (RAM) and/or a read-only memory (ROM) in the calibration apparatus and read and executed by a central processing unit (CPU) (not shown).
In step S2005, the instruction section 160 determines whether a data-obtaining command is input by the operator. The operator arranges the imaging device 130 at a position for obtaining data and inputs the data-obtaining command. If a data-obtaining command is not input by the operator, processing proceeds to step S2050. Otherwise, if receiving the data-obtaining command, the instruction section 160 allows the process to move to step S2010.
In step S2010, the data-managing section 170 receives the image coordinates uQkn and their identifiers kn of the respective subjective viewpoint indices Qkn from the subjective-viewpoint-index detecting section 110 detecting the same.
In step S2020, the data-managing section 170 receives the image coordinates uP1 and uP2 of the objective viewpoint indices from the objective-viewpoint-index detecting section 150 detecting the same.
In step S2030, the data-managing section 170 receives the orientation-measurement value Rws from the sensor-measurement-value inputting section 120. With these steps S2010, S2020, and S2030, a sensor measurement value and the respective image coordinates of the objective viewpoint indices and the subjective viewpoint indices at the same time can be obtained.
In step S2040, the data-managing section 170 adds respective information of the received subjective viewpoint indices Qkn to a data list L so as to serve as data Di. To be more specific, the identifier kn received from the subjective-viewpoint-index detecting section 110 is written by ki; likewise, uQkn input from the subjective-viewpoint-index detecting section 110 is written by uQki; the image coordinates uP1 and uP2 of the objective viewpoint indices received from the objective-viewpoint-index detecting section 150 are written by uP1i and uP2i; the orientation-measurement value Rws input from the sensor-measurement-value inputting section 120 is written by Rwsi; and a set of Di=[ki, uiQki, uP1i, uP2i, Rwsi] is registered in the data list L as the i-th data, wherein i (=1 . . . I) stands for an index of the corresponding set of the registered data, and I stands for the total sets of the registered data. Data is obtained through the above-described step. Processing then proceeds to step S2050.
In step S2050, the data-managing section 170 determines whether the already obtained data list has sufficient information for computing calibration information. If the data list does not satisfy the condition, the process returns to step S2005 and the calibration apparatus waits for an input of a data-obtaining command. If the data list satisfies the condition for computing calibration information, the process moves to step S2060. An example condition of the data list of having information sufficient for computing calibration information is set such that data about three or more different subjective viewpoint indices Qk appears at least twice in the data list L. Meanwhile, since accuracy of the computed calibration information improves as the versatility of input data increases, the condition may be set so as to request a greater number of data.
In step S2060, the instruction section 160 determines whether calibration-information computing command is input by the operator. If the calibration-information computing command is input by the operator, the process moves to step S2070. If not, the process returns to step S2005, and the calibration apparatus waits for an input of a data-obtaining command.
The calibration-information computing section (hereinafter, sometimes simply referred to as computing section) 180 treats calibration information to be obtained as a nonary state vector s, that is, s=[xcP1 ycP1 zcP1 xcP2 ycP2 zcP2 ξcs φcs ζcs]T defined by the position: xcP1=(xcP1, ycP1, zcP1), of the objective viewpoint index P1 in the subjective-viewpoint-camera coordinate system; the position: xcP2=(xcP2, ycP2, zcP2), of the objective viewpoint index P2 in the subjective-viewpoint-camera coordinate system; and the orientation: ωcs=(ξcs, φcs, ζcs), of the orientation sensor 190 in the subjective-viewpoint-camera coordinate system. A variety of methods for expressing an orientation with a ternary value are provided, and it is presumed here that the orientation is expressed with a ternary vector defining a rotating angle and axis respectively with the magnitude and the orientation of the vector.
In step S2070, the computing section 180 provides appropriate initial values to the state vector s. For example, the initial values are previously provided through a setting operation by the operator, e.g., via a dialog.
In step S2080, the computing section 180 computes theoretical value: u′iQki=[u′xiQki, u′yiQki], of subjective-viewpoint image-coordinates of the subjective viewpoint index Qki on the basis of the respective data Di in the data list L and the state vector s with respect to the corresponding i from 1 through I. Meanwhile, the theoretical value u′iQki indicates the coordinates of the subjective viewpoint index to be visible in the subjective viewpoint image for a certain state vector s. The theoretical value u iQki is computed with expression (1) on the basis of the state vector s.

u′iQki=Fi(s)  (1)

To be more specific, a function Fi(s) is configured by the following expressions (2) and (3). With expression (2), the position xCiQki of the subjective viewpoint index Qki in the subjective-viewpoint-camera coordinates upon obtaining the i-th data is computed on the basis of the state vector s and the data Di, and with expression (3), the theoretical coordinates u′iQki on the subjective viewpoint image of the subjective viewpoint index Qki are computed from xCiQki.




  
    
      
        
          x
          
            C
            i
          
          
            Q
            
              k
              i
            
          
        
        =
        
          
            
              [
              
                
                  x
                  
                    C
                    i
                  
                  
                    Q
                    
                      k
                      i
                    
                  
                
                ⁢
                
                  
                
                ⁢
                
                  y
                  
                    C
                    i
                  
                  
                    Q
                    
                      k
                      i
                    
                  
                
                ⁢
                
                  
                
                ⁢
                
                  z
                  
                    C
                    i
                  
                  
                    Q
                    
                      k
                      i
                    
                  
                
              
              ]
            
            T
          
          =
          
            
              R
              
                WC
                i
              
              
                -
                1
              
            
            ·
            
              (
              
                
                  x
                  W
                  
                    Q
                    
                      k
                      i
                    
                  
                
                -
                
                  t
                  
                    WC
                    i
                  
                
              
              )
            
          
        
      
    
    
      
        (
        2
        )
      
    
  
  
    
      
        
          u
          i
          
            ′
            ⁢
            
              
            
            ⁢
            
              Q
              
                k
                i
              
            
          
        
        =
        
          
            
              [
              
                
                  u
                  xi
                  
                    ′
                    ⁢
                    
                      
                    
                    ⁢
                    
                      Q
                      
                        k
                        i
                      
                    
                  
                
                ⁢
                
                  
                
                ⁢
                
                  u
                  yi
                  
                    ′
                    ⁢
                    
                      
                    
                    ⁢
                    
                      Q
                      
                        k
                        i
                      
                    
                  
                
              
              ]
            
            T
          
          =
          
            
              [
              
                
                  
                    -
                    
                      f
                      x
                      C
                    
                  
                  ⁢
                  
                    
                      x
                      
                        C
                        i
                      
                      
                        Q
                        
                          k
                          i
                        
                      
                    
                    
                      
                        
                      
                      ⁢
                      
                        z
                        
                          C
                          i
                        
                        
                          Q
                          
                            k
                            i
                          
                        
                      
                    
                  
                
                ⁢
                
                  
                
                -
                
                  
                    f
                    y
                    C
                  
                  ⁢
                  
                    
                      y
                      
                        C
                        i
                      
                      
                        Q
                        
                          k
                          i
                        
                      
                    
                    
                      z
                      
                        C
                        i
                      
                      
                        Q
                        
                          k
                          i
                        
                      
                    
                  
                
              
              ]
            
            T
          
        
      
    
    
      
        (
        3
        )
      
    
  




where xWQki is a position of the subjective viewpoint index Qki in the world coordinate system; fcx and fcy are focal distances of the imaging device 130 respectively in the X- and Y-axes, and it is presumed that these are previously stored as known values.

In expression (2), Rwci stands for the orientation of the imaging device 130 in the world coordinate system upon obtaining the i-th data and is defined by expression (4) with the aid of a matrix Rcs decided by ωcs serving as an element of the state vector s and the orientation-measurement value Rwsi serving as an element of the data Di.

RWCi=RWSi·RCS−1  (4)

In expression (4), Rcs stands for a rotation matrix used for coordinate transformation from the sensor coordinate system to the subjective-viewpoint-camera coordinate system and is defined by the following expressions:




  
    
      
        
          R
          cs
        
        =
        
          [
          
            
              
                
                  
                    
                      
                        ξ
                        2
                      
                      
                        θ
                        2
                      
                    
                    ⁢
                    
                      (
                      
                        1
                        -
                        
                          cos
                          ⁢
                          
                            
                          
                          ⁢
                          θ
                        
                      
                      )
                    
                  
                  +
                  
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    θ
                  
                
              
              
                
                  
                    
                      
                        ξ
                        ⁢
                        
                          
                        
                        ⁢
                        ψ
                      
                      
                        θ
                        2
                      
                    
                    ⁢
                    
                      (
                      
                        1
                        -
                        
                          cos
                          ⁢
                          
                            
                          
                          ⁢
                          θ
                        
                      
                      )
                    
                  
                  -
                  
                    
                      ζ
                      θ
                    
                    ⁢
                    sin
                    ⁢
                    
                      
                    
                    ⁢
                    θ
                  
                
              
              
                
                  
                    
                      
                        ξ
                        ⁢
                        
                          
                        
                        ⁢
                        ζ
                      
                      
                        θ
                        2
                      
                    
                    ⁢
                    
                      (
                      
                        1
                        -
                        
                          cos
                          ⁢
                          
                            
                          
                          ⁢
                          θ
                        
                      
                      )
                    
                  
                  +
                  
                    
                      ψ
                      θ
                    
                    ⁢
                    sin
                    ⁢
                    
                      
                    
                    ⁢
                    θ
                  
                
              
            
            
              
                
                  
                    
                      
                        ψ
                        ⁢
                        
                          
                        
                        ⁢
                        ξ
                      
                      
                        θ
                        2
                      
                    
                    ⁢
                    
                      (
                      
                        1
                        -
                        
                          cos
                          ⁢
                          
                            
                          
                          ⁢
                          θ
                        
                      
                      )
                    
                  
                  +
                  
                    
                      ζ
                      θ
                    
                    ⁢
                    sin
                    ⁢
                    
                      
                    
                    ⁢
                    θ
                  
                
              
              
                
                  
                    
                      
                        ψ
                        2
                      
                      
                        θ
                        2
                      
                    
                    ⁢
                    
                      (
                      
                        1
                        -
                        
                          cos
                          ⁢
                          
                            
                          
                          ⁢
                          θ
                        
                      
                      )
                    
                  
                  +
                  
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    θ
                  
                
              
              
                
                  
                    
                      
                        ψ
                        ⁢
                        
                          
                        
                        ⁢
                        ζ
                      
                      
                        θ
                        2
                      
                    
                    ⁢
                    
                      (
                      
                        1
                        -
                        
                          cos
                          ⁢
                          
                            
                          
                          ⁢
                          θ
                        
                      
                      )
                    
                  
                  -
                  
                    
                      ξ
                      θ
                    
                    ⁢
                    sin
                    ⁢
                    
                      
                    
                    ⁢
                    θ
                  
                
              
            
            
              
                
                  
                    
                      
                        ζ
                        ⁢
                        
                          
                        
                        ⁢
                        ξ
                      
                      
                        θ
                        2
                      
                    
                    ⁢
                    
                      (
                      
                        1
                        -
                        
                          cos
                          ⁢
                          
                            
                          
                          ⁢
                          θ
                        
                      
                      )
                    
                  
                  -
                  
                    
                      ψ
                      θ
                    
                    ⁢
                    sin
                    ⁢
                    
                      
                    
                    ⁢
                    θ
                  
                
              
              
                
                  
                    
                      
                        ζ
                        ⁢
                        
                          
                        
                        ⁢
                        ψ
                      
                      
                        θ
                        2
                      
                    
                    ⁢
                    
                      (
                      
                        1
                        -
                        
                          cos
                          ⁢
                          
                            
                          
                          ⁢
                          θ
                        
                      
                      )
                    
                  
                  +
                  
                    
                      ξ
                      θ
                    
                    ⁢
                    sin
                    ⁢
                    
                      
                    
                    ⁢
                    θ
                  
                
              
              
                
                  
                    
                      
                        ζ
                        2
                      
                      
                        θ
                        2
                      
                    
                    ⁢
                    
                      (
                      
                        1
                        -
                        
                          cos
                          ⁢
                          
                            
                          
                          ⁢
                          θ
                        
                      
                      )
                    
                  
                  +
                  
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    θ
                  
                
              
            
          
          ]
        
      
    
    
      
        (
        5
        )
      
    
  
  
    
      
        θ
        =
        
          
            
              ξ
              2
            
            +
            
              ψ
              2
            
            +
            
              ζ
              2
            
          
        
      
    
    
      
        (
        6
        )
      
    
  




In expression (2), twci stands for the position of the imaging device 130 in the world coordinate system upon obtaining the i-th data and is given by expressions (7) to (9) with the aid of the state vector s and the data Di.




  
    
      
        
          t
          
            WC
            i
          
        
        =
        
          
            R
            WB
          
          ·
          
            
              [
              
                
                  
                    
                      f
                      x
                      B
                    
                  
                  
                    0
                  
                  
                    
                      u
                      
                        x
                        i
                      
                      
                        P
                        1
                      
                    
                  
                
                
                  
                    0
                  
                  
                    
                      f
                      y
                      B
                    
                  
                  
                    
                      u
                      
                        y
                        i
                      
                      
                        P
                        1
                      
                    
                  
                
                
                  
                    
                      f
                      x
                      B
                    
                  
                  
                    0
                  
                  
                    
                      u
                      
                        x
                        i
                      
                      
                        P
                        2
                      
                    
                  
                
                
                  
                    0
                  
                  
                    
                      f
                      y
                      B
                    
                  
                  
                    
                      u
                      
                        y
                        i
                      
                      
                        P
                        2
                      
                    
                  
                
              
              ]
            
            +
          
          ·
          
            [
            
              
                
                  
                    
                      -
                      
                        
                          u
                          
                            x
                            i
                          
                          
                            P
                            1
                          
                        
                        ⁡
                        
                          (
                          
                            
                              z
                              C
                              
                                ′
                                ⁢
                                
                                  
                                
                                ⁢
                                
                                  P
                                  1
                                
                              
                            
                            -
                            
                              z
                              WB
                              ′
                            
                          
                          )
                        
                      
                    
                    -
                    
                      
                        f
                        x
                        B
                      
                      ⁡
                      
                        (
                        
                          
                            x
                            C
                            
                              ′
                              ⁢
                              
                                
                              
                              ⁢
                              
                                P
                                1
                              
                            
                          
                          -
                          
                            x
                            WB
                            ′
                          
                        
                        )
                      
                    
                  
                
              
              
                
                  
                    
                      -
                      
                        
                          u
                          
                            y
                            i
                          
                          
                            P
                            1
                          
                        
                        ⁡
                        
                          (
                          
                            
                              z
                              C
                              
                                ′
                                ⁢
                                
                                  
                                
                                ⁢
                                
                                  P
                                  1
                                
                              
                            
                            -
                            
                              z
                              WB
                              ′
                            
                          
                          )
                        
                      
                    
                    -
                    
                      
                        f
                        y
                        B
                      
                      ⁡
                      
                        (
                        
                          
                            y
                            C
                            
                              ′
                              ⁢
                              
                                
                              
                              ⁢
                              
                                P
                                1
                              
                            
                          
                          -
                          
                            y
                            WB
                            ′
                          
                        
                        )
                      
                    
                  
                
              
              
                
                  
                    
                      -
                      
                        
                          u
                          
                            x
                            i
                          
                          
                            P
                            2
                          
                        
                        ⁡
                        
                          (
                          
                            
                              z
                              C
                              
                                ′
                                ⁢
                                
                                  
                                
                                ⁢
                                
                                  P
                                  2
                                
                              
                            
                            -
                            
                              z
                              WB
                              ′
                            
                          
                          )
                        
                      
                    
                    -
                    
                      
                        f
                        x
                        B
                      
                      ⁡
                      
                        (
                        
                          
                            x
                            C
                            
                              ′
                              ⁢
                              
                                
                              
                              ⁢
                              
                                P
                                2
                              
                            
                          
                          -
                          
                            x
                            WB
                            ′
                          
                        
                        )
                      
                    
                  
                
              
              
                
                  
                    
                      -
                      
                        
                          u
                          
                            y
                            i
                          
                          
                            P
                            2
                          
                        
                        ⁡
                        
                          (
                          
                            
                              z
                              C
                              
                                ′
                                ⁢
                                
                                  
                                
                                ⁢
                                
                                  P
                                  2
                                
                              
                            
                            -
                            
                              z
                              WB
                              ′
                            
                          
                          )
                        
                      
                    
                    -
                    
                      
                        f
                        y
                        B
                      
                      ⁡
                      
                        (
                        
                          
                            y
                            C
                            
                              ′
                              ⁢
                              
                                
                              
                              ⁢
                              
                                P
                                2
                              
                            
                          
                          -
                          
                            y
                            WB
                            ′
                          
                        
                        )
                      
                    
                  
                
              
            
            ]
          
        
      
    
    
      
        (
        7
        )
      
    
  
  
    
      
        
          [
          
            
              
                
                  x
                  C
                  
                    ′
                    ⁢
                    
                      
                    
                    ⁢
                    
                      P
                      j
                    
                  
                
              
            
            
              
                
                  y
                  C
                  
                    ′
                    ⁢
                    
                      
                    
                    ⁢
                    
                      P
                      j
                    
                  
                
              
            
            
              
                
                  z
                  C
                  
                    ′
                    ⁢
                    
                      
                    
                    ⁢
                    
                      P
                      j
                    
                  
                
              
            
          
          ]
        
        =
        
          
            R
            WB
            
              -
              1
            
          
          ·
          
            R
            
              WC
              i
            
          
          ·
          
            x
            C
            
              P
              j
            
          
        
      
    
    
      
        (
        8
        )
      
    
  
  
    
      
        
          [
          
            
              
                
                  x
                  WB
                  ′
                
              
            
            
              
                
                  y
                  WB
                  ′
                
              
            
            
              
                
                  z
                  WB
                  ′
                
              
            
          
          ]
        
        =
        
          
            R
            WB
            
              -
              1
            
          
          ·
          
            t
            WB
          
        
      
    
    
      
        (
        9
        )
      
    
  




In the above expressions, RWB and tWB are respectively a rotation matrix standing for the orientation of the objective-viewpoint camera 140 and a translation vector standing for the position of the objective-viewpoint camera 140 both in the world coordinate system, and it is presumed that these are previously computed on the basis of the position and the orientation previously stored as known values, of the objective-viewpoint camera 140 in the world coordinate system. Also, fBx and fBy are focal distances of the objective-viewpoint camera 140 respectively in the x- and y-axis directions, and it is presumed that these are previously stored as known values.
The relationship among expressions (7) to (9) can be obtained by solving expressions (12) and (13) obtained by formulating a geometrical transformation upon observing the objective viewpoint indices Pj (j=1, 2) with the objective viewpoint camera into expressions (10) and (11); and expanding these expressions.




  
    
      
        
          x
          
            B
            i
          
          
            P
            j
          
        
        =
        
          
            
              [
              
                
                  x
                  
                    B
                    i
                  
                  
                    P
                    j
                  
                
                ⁢
                
                  
                
                ⁢
                
                  y
                  
                    B
                    i
                  
                  
                    P
                    j
                  
                
                ⁢
                
                  
                
                ⁢
                
                  z
                  
                    B
                    i
                  
                  
                    P
                    j
                  
                
              
              ]
            
            T
          
          =
          
            
              R
              WB
              
                -
                1
              
            
            ·
            
              (
              
                
                  
                    R
                    
                      WC
                      i
                    
                  
                  ·
                  
                    x
                    C
                    
                      P
                      j
                    
                  
                
                +
                
                  t
                  
                    WC
                    i
                  
                
                -
                
                  t
                  WB
                
              
              )
            
          
        
      
    
    
      
        (
        10
        )
      
    
  
  
    
      
        
          u
          i
          
            P
            j
          
        
        =
        
          
            
              [
              
                
                  u
                  
                    x
                    i
                  
                  
                    P
                    j
                  
                
                ⁢
                
                  
                
                ⁢
                
                  u
                  
                    y
                    i
                  
                  
                    P
                    j
                  
                
              
              ]
            
            T
          
          =
          
            [
            
              
                
                  -
                  
                    f
                    x
                    B
                  
                
                ⁢
                
                  
                    x
                    
                      B
                      i
                    
                    
                      P
                      j
                    
                  
                  
                    z
                    
                      B
                      i
                    
                    
                      P
                      j
                    
                  
                
              
              -
              
                
                  f
                  y
                  B
                
                ⁢
                
                  
                    y
                    
                      B
                      i
                    
                    
                      P
                      j
                    
                  
                  
                    z
                    
                      B
                      i
                    
                    
                      P
                      j
                    
                  
                
              
            
            ]
          
        
      
    
    
      
        (
        11
        )
      
    
  
  
    
      
        
          u
          
            x
            i
          
          
            P
            j
          
        
        =
        
          
            -
            
              f
              x
              B
            
          
          ⁢
          
            
              
                x
                C
                
                  ′
                  ⁢
                  
                    
                  
                  ⁢
                  
                    P
                    j
                  
                
              
              +
              
                x
                
                  WC
                  i
                
                ′
              
              -
              
                x
                WB
                ′
              
            
            
              
                z
                C
                
                  ′
                  ⁢
                  
                    
                  
                  ⁢
                  
                    P
                    j
                  
                
              
              +
              
                z
                
                  WC
                  i
                
                ′
              
              -
              
                z
                WB
                ′
              
            
          
        
      
    
    
      
        (
        12
        )
      
    
  
  
    
      
        
          u
          
            y
            i
          
          
            P
            j
          
        
        =
        
          
            -
            
              f
              y
              B
            
          
          ⁢
          
            
              
                y
                C
                
                  ′
                  ⁢
                  
                    
                  
                  ⁢
                  
                    P
                    j
                  
                
              
              +
              
                y
                
                  WC
                  i
                
                ′
              
              -
              
                y
                WB
                ′
              
            
            
              
                z
                C
                
                  ′
                  ⁢
                  
                    
                  
                  ⁢
                  
                    P
                    j
                  
                
              
              +
              
                z
                
                  WC
                  i
                
                ′
              
              -
              
                z
                WB
                ′
              
            
          
        
      
    
    
      
        (
        13
        )
      
    
  




Symbols x′ci, y′wci, and z′wci in expressions (12) and (13) have the relationship shown by expression below:




  
    
      
        
          [
          
            
              
                
                  x
                  
                    WC
                    i
                  
                  ′
                
              
            
            
              
                
                  y
                  
                    WC
                    i
                  
                  ′
                
              
            
            
              
                
                  z
                  
                    WC
                    i
                  
                  ′
                
              
            
          
          ]
        
        =
        
          
            R
            WB
            
              -
              1
            
          
          ·
          
            t
            
              WC
              i
            
          
        
      
    
    
      
        (
        14
        )
      
    
  




In step S2090, with respect to the corresponding i from 1 through I, the computing section 180 computes an error ΔuiQki between actual image coordinates uiQki of the subjective viewpoint index Qki included in each data in the data list L and theoretical values u′iQki of the corresponding image coordinates by expression below:

ΔuiQki=uiQki−u′iQki  (15)

In step S2100, with respect to the corresponding i from 1 through I, the computing section 180 computes an image Jacobian JuisQki (=∂uiQki/∂s) including the state vector s as a parameter, i.e., a Jacobian matrix of two rows and nine columns, having solutions as its respective elements obtained by partially differentiating the function Fi(s) given by expression (1) with the respective elements of the state vector s. To be more specific, the computing section 180 computes a Jacobian matrix JuixCiQki (=∂uiQki/∂xCiQki) of two rows and three columns, having solutions as its elements obtained by partially differentiating the right side of expression (3) with the respective elements of the position vector xCiQki in the subjective-viewpoint-camera coordinate system and a Jacobian matrix JxCisQki (=∂xCiQki/∂s) of three rows and nine columns, having solutions as its elements obtained by partially differentiating the right side of expression (2) with the respective elements of the state vector s, and the image Jacobian JuisQki with the aid of expression below:

JuisQki=JuixCiQki·JxCisQki  (16)

In step S2110, the computing section 180 computes a compensation value Δs of the vector s, on the basis of the error ΔuiQki and the Jacobian matrix JuisQki computed through the above-described steps with respect to the corresponding i from 1 through I. To be more specific, the computing section 180 prepares a 2I-dimension error vector configured by vertically aligning the error ΔuiQki for each subjective viewpoint index Qki; and a matrix of 2I-rows and nine columns configured by vertically aligning the Jacobian JuisQki for each subjective viewpoint index Qki, given by expressions (17) and (18), respectively. Then the computing section 180 computes the compensation value Δs with the aid of expression (19) by using the pseudo inverse matrix φ+ of the matrix φ given by expression (18).




  
    
      
        U
        =
        
          [
          
            
              
                
                  Δ
                  ⁢
                  
                    
                  
                  ⁢
                  
                    u
                    1
                    
                      Q
                      
                        k
                        ⁢
                        
                          
                        
                        ⁢
                        1
                      
                    
                  
                
              
            
            
              
                
                  Δ
                  ⁢
                  
                    
                  
                  ⁢
                  
                    u
                    2
                    
                      Q
                      
                        k
                        ⁢
                        
                          
                        
                        ⁢
                        2
                      
                    
                  
                
              
            
            
              
                ⋮
              
            
            
              
                
                  Δ
                  ⁢
                  
                    
                  
                  ⁢
                  
                    u
                    I
                    
                      Q
                      
                        k
                        ⁢
                        
                          
                        
                        ⁢
                        1
                      
                    
                  
                
              
            
          
          ]
        
      
    
    
      
        (
        17
        )
      
    
  
  
    
      
        Φ
        =
        
          [
          
            
              
                
                  J
                  
                    
                      u
                      1
                    
                    ⁢
                    s
                  
                  
                    Q
                    
                      k
                      ⁢
                      
                        
                      
                      ⁢
                      1
                    
                  
                
              
            
            
              
                
                  J
                  
                    
                      u
                      2
                    
                    ⁢
                    s
                  
                  
                    Q
                    
                      k
                      ⁢
                      
                        
                      
                      ⁢
                      2
                    
                  
                
              
            
            
              
                ⋮
              
            
            
              
                
                  J
                  
                    
                      u
                      1
                    
                    ⁢
                    s
                  
                  
                    Q
                    
                      k
                      ⁢
                      
                        
                      
                      ⁢
                      1
                    
                  
                
              
            
          
          ]
        
      
    
    
      
        (
        18
        )
      
    
  
  
    
      
        
          Δ
          ⁢
          
            
          
          ⁢
          
            s
            Pk
          
        
        =
        
          
            φ
            +
          
          ⁢
          U
        
      
    
    
      
        (
        19
        )
      
    
  




While being obtained, for example, by an equation: φ+=(φTφ)−1φT, the pseudo inverse matrix φ+ may be obtained differently.
In step S2120, the computing section 180 compensates for the vector s on the basis of the compensation value Δs computed in step S2110 and sets the obtained value as a new vector s in accordance with expression below:

s=s+Δs  (20)

In step S2130, the computing section 180 determines whether calculation of these values converges by using a certain determination criterion such as the magnitude of the error vector U in expressions (19) whether being smaller than a predetermined threshold or the magnitude of the compensation value Δs whether being smaller than a predetermined threshold. If the determination is non-convergence of the calculation, the computing section 180 performs the steps from step S2080 again on the basis of the compensated state vector s.
If the determination is convergence of the calculation, the computing section 180 outputs the obtained state vector s in step S2140.
Finally, the computing section 180 determines in step S2150 whether the calibration process is to be finished. When the operator instructs the calibration apparatus 100 to finish the calibration process, the process ends. When the operator instructs the calibration apparatus to continue the calibration process (to perform recalibration), the process returns to step S2005 and the calibration apparatus waits for an input of a data-obtaining command.
With the above-described process, two kinds of arrangement information of the indices and the sensor both displaced on and relative to the imaging device can be calibrated easily and accurately.
Modification of the First Embodiment
While a compensating value of the state vector is computed by the Newton-Raphson method given by expression (19) in the present embodiment, the method for computing the compensation value is not always limited to the Newton-Raphson. For example, the compensation value may be computed by the Levenberg-Marquard method (LM method) serving as the known method of iterative solution of a non-linear equation or by a combined method of the LM method with a statistical method such as an M-estimation serving as the known robust estimation method. One skilled in the art will appreciate that application of any numerical calculation method does not deteriorate the spirit of the present invention.
Also, while the orientation sensor is placed on the imaging device in the present embodiment, another sensor such as a position-orientation sensor may be used.
In addition, while the indices, each having the two dimensional coordinates on an image, are used as the subjective viewpoint indices in the present embodiment, a line feature or a geometric feature respectively disclosed in Lowe and Comport et al. may be used as a reference index for evaluation. For example, in the case of using the line feature, by defining a distance of the line from its origin as an error evaluation reference; configuring the error vector U of an error Δd computed from a detection value d from the image and an evaluation value d′ from the state vector s; and configuring the matrix φ of the Jacobian matrix Jds (=∂d/∂s) of a single row and nine columns having solutions as its elements obtained by partially differentiating the computing expression of d′ with the respective elements of the state vector s, a compensation value can be computed with the same framework as described above. Since the computing expression of d′ is disclosed in Lowe and Comport et al. as a function of the position and the orientation of the imaging device, and also the position and the orientation of the imaging device are obtained as a function of the vector s (by expressions (4) and (7)), the compensation value is easily obtained. Also, by accumulating errors and image Jacobians obtained from the line features, point features, and other indices, these features can be used at the same time.
Second Embodiment
In a calibration apparatus according to a second embodiment, a plurality of indices and a six-degree-of-freedom position-orientation sensor both placed on a measuring object are calibrated by capturing the indices placed on the measuring object with a movable camera having another six-degree-of-freedom position-orientation sensor placed thereon. While the positions of indices and the orientation of the orientation sensor both placed on and relative to the camera are treated as known parameters in the first embodiment, the respective positions & orientations of the respective indices and the six-degree-of-freedom position-orientation sensor both relative to the object are treated as unknown parameters in the second embodiment. The calibration apparatus and a calibration method according to the second embodiment will be described below.
FIG. 3 shows the general structure of a calibration apparatus 1100 according to the present embodiment. As shown in FIG. 3, the calibration apparatus 1100 according to the present embodiment includes an imaging section (hereinafter, sometimes referred to as a camera) 1010, an index-detecting section 1020, a position-orientation computing section 1030, a data-managing section 1040, an instruction section 1050, and a calibration-information computing section 1060 and is connected to an object 1000 having indices placed thereon.
The object 1000 has a six-degree-of-freedom position-orientation sensor A2 placed thereon as a target of the calibration. With this, the position & orientation of the six-degree-of-freedom position-orientation sensor A2 can be measured with respect to the world coordinate system. The six-degree-of-freedom position-orientation sensor A2 is connected to the position-orientation computing section 1030. The imaging section 1010 has a six-degree-of-freedom position-orientation sensor A1 placed thereon. Meanwhile, it is presumed that the position & orientation of the six-degree-of-freedom position-orientation sensor A1 relative to the imaging section 1010 are known, and this allows the position & orientation of the imaging section 1010 with respect to the world coordinate system to be measured. The six-degree-of-freedom position-orientation sensor A1 is connected to the position-orientation computing section 1030.
The object 1000 has a plurality of indices placed thereon as shown in FIG. 5A as a target of the calibration. The indices placed on the object are written by Pk (k=1 . . . Ko), wherein Ko stands for the number of the calibrating indices placed on the object. Each index Pk is configured by points Pki (i=1 . . . Nk) as shown in FIG. 5B, wherein Nk stands for the total number of points constituting the index Pk. For example, in case of using quadrangular indices, an index consists of four vertices of the square and Nk=4. Here, it is presumed that the positions of the points constituting the corresponding index in the index-coordinate system are known. The position & orientation of each index in the object-coordinate system (the coordinate system in which one point on the object is defined as an origin and three axes orthogonal to one another are respectively defined as the x-, y-, and z-axes) are unknown. The position & orientation of the six-degree-of-freedom position-orientation sensor placed on the object in the object-coordinate system are also unknown. It is presumed that the object has reference indices Pk (k=Ko+1 . . . Ko+K1) with at least three points displaced thereon, to define the object-coordinate system. Here, K1 stands for the number of the reference indices. Thus, the total number of the indices placed on the object is Ko+K1. The position & orientation of the reference indices in the object-coordinate system are known. All reference indices must be observed by the imaging section 1010 at least once in the same image together with another calibrating index. The indices Pk placed on the object may be of any type as long as the image coordinates of a projection figure on a captured image can be detected, and each index and each of points constituting the index can be identified.
The imaging section 1010 captures the indices placed on the object 1000 from a variety of positions and directions. The captured image is input into the index-detecting section 1020.
The index-detecting section 1020 receives the image from the imaging section 1010 and detects the image coordinates of the respective points constituting the index Pk (hereinafter, sometimes simply referred to as the image coordinates of the index) captured in the received image.
Also, the index-detecting section 1020 outputs the image coordinates upkni of each of points pkni constituting the detected index pkn and the corresponding identifier kn to the data-managing section 1040, wherein n (=1 . . . M) stands for an index of the corresponding one of the detected indices and M stands for the total number of the detected indices. For example, when the quadrangular indices are used as reference indices, the image coordinates of the four vertices are detected and output as the image coordinates of each index. For example, FIG. 3 shows the case where quadrangular indices having identifiers 1, 2, and 3 are placed on the object. Here, P1 and P2 are calibrating indices, P3 is reference index, Ko=2, and K1=1. At a certain camera position, three indices are to be captured, and M=3. With this arrangement, the identifiers k1=1, k2=2, and k3=3, and the corresponding image coordinates upkli, upk2i and upk3i (i=1, 2, 3, 4) are output.
The position-orientation computing section 1030 will be described. Presuming that t stand for the position written by a ternary vector and R stand for the orientation written by a 3×3 rotation matrix, of a certain coordinate system A relative to another coordinate system B, coordinate transformation from a point xA in the coordinate system A to a point xB in the coordinate system B is written by expression (22) with the aid of a 4×4 matrix MBA given by expression (21).




  
    
      
        
          M
          BA
        
        =
        
          [
          
            
              
                R
              
              
                t
              
            
            
              
                0
              
              
                1
              
            
          
          ]
        
      
    
    
      
        (
        21
        )
      
    
  
  
    
      
        
          [
          
            
              
                
                  x
                  B
                
              
            
            
              
                1
              
            
          
          ]
        
        =
        
          
            M
            BA
          
          ⁡
          
            [
            
              
                
                  
                    x
                    A
                  
                
              
              
                
                  1
                
              
            
            ]
          
        
      
    
    
      
        (
        22
        )
      
    
  




In the present embodiment, the matrix MBA serves so as to express the position & orientation of the coordinate system A relative to the coordinate system B.
On the basis of the position & orientation Mwc of the camera 1010 in the world coordinate system, obtained from a sensor measurement value of the six-degree-of-freedom position-orientation sensor A1 (hereinafter, sometimes referred to as the first six-degree-of-freedom position-orientation sensor) and the position & orientation MWA2 of the six-degree-of-freedom position-orientation sensor A2 (hereinafter, sometimes referred to as the second six-degree-of-freedom position-orientation sensor) in the world coordinate system, obtained from a measurement value of the six-degree-of-freedom position-orientation sensor A2, the position-orientation computing section 1030 computes the position & orientation MA2C of the camera 1010 in the coordinate system of the second six-degree-of-freedom position-orientation sensor with expression below:

MA2C=MWA2−1·MWC  (23)

Also, in the present embodiment, a position and an orientation are internally written by ternary vectors x=[x y z]T and ω=[ξ ψ ζ]T, respectively. While a variety of methods for expressing an orientation by a ternary value are proposed, it is presumed here that the orientation is written by a ternary vector defining its rotating angle and rotating direction with the magnitude and orientation of the vector, respectively. The position x and the orientation ω can be combined into a six dimensional vector: a=[x y z ξ ψ ζ]T.
The 4×4 matrix MA2C obtained from the sensor output values, expressing the position & orientation of the camera 1010 in the coordinate system of the second six-degree-of-freedom position-orientation sensor can be transformed into a six dimensional vector: aA2C=[xA2C yA2C zA2C ξA2C ψA2C ζA2C]T.
The position-orientation computing section 1030 outputs the position & orientation of the camera 1010 in the coordinate system of the second six-degree-of-freedom position-orientation sensor to the data-managing section 1040 in response to its request.
The instruction section 1050 transmits an instruction of obtaining data to the data-managing section 1040 upon receipt of a data-obtaining command from an operator (not shown). The instruction section 1050 also transmits another instruction of computing calibration information to the calibration-information computing section 1060 upon receipt of a calibration-information computing command from the operator.
Upon receipt of the instruction of obtaining data from the instruction section 1050, the data-managing section 1040 receives (i) the position & orientation of the camera 1010 in the coordinate system of the second six-degree-of-freedom position-orientation sensor from the position-orientation computing section 1030, (ii) the image coordinates of indices, and (iii) the corresponding identifiers of the indices from the index-detecting section 1020. Then, the data-managing section 1040 creates a plurality of data sets and adds them to a data list for storage. Here, each data set is configured by (a) the position & orientation of the camera 1010 in the coordinate system of the second six-degree-of-freedom position-orientation sensor at a certain time, (b) the image coordinates of an index detected at the same time, and (c) the identifier of the detected index. Also, the data-managing section 1040 outputs the created data list to the calibration-information computing section 1060 in response to its request.
Upon receipt of the instruction of computing calibration information from the instruction section 1050, the calibration-information computing section 1060 receives the data list from the data-managing section 1040, performs a calibration process on the basis of the data list, and outputs the calibration information (i.e., the position & orientations of each index and the second six-degree-of-freedom position-orientation sensor, both in the object coordinate system) obtained as a result of the calibration process.
FIG. 4 is a flowchart of the process of the calibration apparatus according to the present embodiment, for obtaining calibration information. Program code in accordance with the flowchart is stored in memories (not shown) such as a RAM and/or a ROM in the calibration apparatus and read and executed by a CPU (not shown).
In step S6010, the instruction section 1050 determines whether a data-obtaining command is input by the operator. The operator inputs the data-obtaining command upon arranging the object 1000 or the camera 1010 at a position for obtaining calibrating data. If it is determined in step 6010 that a data-obtaining command is not input by the operator, processing proceeds to step S6050. Otherwise, if receiving the data-obtaining command, the instruction section 1050 allows the process to move to step S6020.
In step S6020, from the position-orientation computing section 1030, the data-managing section 1040 receives the position & orientation of the camera 1010: aA2C=[xA2C yA2C zA2C ξA2C ψA2C ζ2C]T, in the coordinate system of the second six-degree-of-freedom position-orientation sensor.
In step S6030, from the index-detecting section 1020, the data-managing section 1040 receives an identifier kn and image coordinates upkni of each index Pkn detected by the index-detecting section 1020. The index-detecting section 1020 detects indices in an inputted image. Hence, with this step, the position & orientation of the camera 1010 in the coordinate system of the second six-degree-of-freedom position-orientation sensor and the image coordinates and the identifier of the indices at the same time can be obtained. Meanwhile, information input from the index-detecting section 1020 is not needed to always cover all indices on the object and is sufficient as long as it covers indices detected in the image.
In step S6040, the data-managing section 1040 adds received sets of data about all detected indices Pkn to a data list DL as data Dj. To be more specific, when the position & orientation aA2C of the camera 1010 received from the position-orientation computing section 1030 is written by aA2Cj=[xA2Cj yA2Cj zA2Cj ξA2Cj ψA2Cj ζA2Cj]T, the identifier kn received from the index-detecting section 1020 is written by kj, and the set of image coordinates upkni received from the same is written by upkji, a set of Dj=[aA2Cj upkji kj] is registered in the data list DL as the j-th data, wherein j (=1 . . . Nj) stands for an index of the corresponding set of data registered in the data list DL, and Nj stands for the total number of sets of the registered data. With the above-described step, data is obtained.
In step S6050, the data-managing section 1040 determines whether the already obtained data list has sufficient information for computing calibration information. If the data list does not satisfy the condition, the process returns to step S6010 and the calibration apparatus waits for an input of a data-obtaining command. If the data list satisfies the condition for computing calibration information, the process moves to step S6060. An example condition of the data list of having information sufficient for computing calibration information is set such that data about all indices appears twice or more in the data list DL. Meanwhile, since accuracy of the computed calibration information improves as the versatility of input data increases, the condition may be set so as to request a greater number of data.
In step S6060, it is determined whether a calibration-information computing command is input by the operator. If the calibration-information computing command is input by the operator, the process moves to step S6070, and if not, the process returns to step S6010 and the calibration apparatus waits for an input of a data-obtaining command.
The calibration-information computing section 1060 (hereinafter, sometimes shortly referred to as the computing section 1060) treats calibration information to be obtained, in other words, the positions & orientations of the respective calibrating indices and the second six-degree-of-freedom position-orientation sensor both in the object coordinate system, as unknown parameters. In the following descriptions, the positions & orientations of the respective calibrating indices and the second six-degree-of-freedom position-orientation sensor both in the object coordinate system are respectively written by state vectors smk=[xmk ymk zmk ξmk ψmk ζmk]T (k=1 . . . Ko) and sOA2=[xOA2 yOA2 zOA2 ξOA2 ψOA2 ζOA2]T.
In step S6070, the computing-section 1060 provides appropriate initial values to the respective state vectors smk (k=1 . . . Ko) and sOA2. As the respective initial values, the operator may manually input approximate values with the aid of the instruction section 1050. When three transformation matrices expressing the respective positions & orientations of the camera 1010 in the coordinate system of the second six-degree-of-freedom position-orientation sensor, the second six-degree-of-freedom position-orientation sensor in the object coordinate system, and the index in the object coordinate system are respectively written as MA2C, MOA2, and MOM, a transformation matrix MCM from the index-coordinate system to the camera-coordinate system can be obtained from these matrices as given by the following expression:

MCM=MA2C−1·MOA2−1·MOM  (24)

In step S6080, the computing-section 1060 computes a calculation value upkji,=[uxpkji, uypkji,] of the image coordinates of an index Pkj on the basis of the corresponding data Dj=[aA2Cj upkji kj] (j=1, 2 . . . Nj) in the data list DL and the state vectors smkj and SOA2 with respect to the corresponding j from 1 through Nj and i from 1 through Nk. Meanwhile, the calculation value of the image coordinates of the index Pkj indicates the position (the coordinates) of point pkji of the index Pkj to be observed in the received image for certain state vectors smkj and SOA2. The value upkji, is computed on the basis of a function of a state vector: s=[sm1T sm2T . . . smKoT sOA2T]T, providing the position & orientation of the respective calibrating indices and the second six-degree-of-freedom position-orientation sensor in the object-coordinate system, as written by the following expression:

uPkji′=Fj(s)  (25)

To be more specific, when the j-th data is obtained and the data is about a calibrating index, the function Fj(s) is configured by expressions (26) for obtaining a position vector xcpkji of the i-th point of the index Pkj in the camera coordinates from smkj and sOA2; and expression (27) for obtaining the coordinates upkji, of the i-th point of the index Pkj in the image from xcpkji.




  
    
      
        
          x
          C
          
            
              p
              
                k
                j
              
            
            ⁢
            i
          
        
        =
        
          
            
              [
              
                
                  x
                  C
                  
                    
                      p
                      
                        k
                        j
                      
                    
                    ⁢
                    i
                  
                
                ⁢
                
                  
                
                ⁢
                
                  y
                  C
                  
                    
                      p
                      
                        k
                        j
                      
                    
                    ⁢
                    i
                  
                
                ⁢
                
                  
                
                ⁢
                
                  z
                  C
                  
                    
                      p
                      
                        k
                        j
                      
                    
                    ⁢
                    i
                  
                
                ⁢
                
                  
                
                ⁢
                1
              
              ]
            
            T
          
          =
          
            
              M
              
                A
                ⁢
                
                  
                
                ⁢
                2
                ⁢
                C
              
              
                -
                1
              
            
            ·
            
              
                M
                
                  OA
                  ⁢
                  
                    
                  
                  ⁢
                  2
                
                
                  -
                  1
                
              
              ⁡
              
                (
                
                  s
                  
                    OA
                    ⁢
                    
                      
                    
                    ⁢
                    2
                  
                
                )
              
            
            ·
            
              
                M
                OM
              
              ⁡
              
                (
                
                  s
                  
                    mk
                    j
                  
                
                )
              
            
            ·
            
              x
              M
              
                
                  p
                  
                    k
                    j
                  
                
                ⁢
                i
              
            
          
        
      
    
    
      
        (
        26
        )
      
    
  
  
    
      
        
          u
          
            
              P
              
                k
                j
              
            
            ⁢
            i′
          
        
        =
        
          
            
              [
              
                
                  u
                  x
                  
                    
                      P
                      
                        k
                        j
                      
                    
                    ⁢
                    i′
                  
                
                ⁢
                
                  
                
                ⁢
                
                  u
                  y
                  
                    
                      P
                      
                        k
                        j
                      
                    
                    ⁢
                    i′
                  
                
              
              ]
            
            T
          
          =
          
            
              [
              
                
                  
                    -
                    
                      f
                      x
                      B
                    
                  
                  ⁢
                  
                    
                      x
                      C
                      
                        
                          p
                          
                            k
                            j
                          
                        
                        ⁢
                        i
                      
                    
                    
                      z
                      C
                      
                        
                          p
                          
                            k
                            j
                          
                        
                        ⁢
                        i
                      
                    
                  
                
                -
                
                  
                    f
                    y
                    B
                  
                  ⁢
                  
                    
                      y
                      C
                      
                        
                          p
                          
                            k
                            j
                          
                        
                        ⁢
                        i
                      
                    
                    
                      z
                      C
                      
                        
                          p
                          
                            k
                            j
                          
                        
                        ⁢
                        i
                      
                    
                  
                
              
              ]
            
            T
          
        
      
    
    
      
        (
        27
        )
      
    
  




In the above expressions, xMpkji is the position of the i-th point of the index Pkj in the index-coordinate system; fBx and fBy are the focal distances of the camera 1010 respectively in the X- and Y-axes directions, and it is presumed that these are previously stored as known values. Also, MA2C is a transformation matrix decided by aA2Cj, MOM(smkj) is a transformation matrix (i.e., a matrix for transformation from the kj-th index-coordinate system to the object-coordinate system) decided by smkj, and MOA2(sOA2) is a transformation matrix decided by sOA2.
The transformation matrix MOM(smk) (k=1 . . . Ko) is defined by the following expressions:




  
    
      
        
          
            M
            OM
          
          ⁡
          
            (
            
              s
              mk
            
            )
          
        
        =
        
           
          
            [
            
              
                
                  
                    
                      
                        
                          ξ
                          mk
                          2
                        
                        
                          θ
                          2
                        
                      
                      ⁢
                      
                        (
                        
                          1
                          -
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            θ
                          
                        
                        )
                      
                    
                    +
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                  
                
                
                  
                    
                      
                        
                          
                            ξ
                            mk
                          
                          ⁢
                          ψ
                        
                        
                          
                            θ
                            ⁢
                            
                              
                            
                          
                          2
                        
                      
                      ⁢
                      
                        (
                        
                          1
                          -
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            θ
                          
                        
                        )
                      
                    
                    -
                    
                      
                        
                          ζ
                          mk
                        
                        θ
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                  
                
                
                  
                    
                      
                        
                          
                            ξ
                            mk
                          
                          ⁢
                          
                            ζ
                            mk
                          
                        
                        
                          θ
                          2
                        
                      
                      ⁢
                      
                        (
                        
                          1
                          -
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            θ
                          
                        
                        )
                      
                    
                    +
                    
                      
                        
                          ψ
                          mk
                        
                        θ
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                  
                
                
                  
                    x
                    mk
                  
                
              
              
                
                  
                    
                      
                        
                          
                            ψ
                            mk
                          
                          ⁢
                          
                            ξ
                            mk
                          
                        
                        
                          θ
                          2
                        
                      
                      ⁢
                      
                        (
                        
                          1
                          -
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            θ
                          
                        
                        )
                      
                    
                    +
                    
                      
                        
                          ζ
                          mk
                        
                        θ
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                  
                
                
                  
                    
                      
                        
                          ψ
                          mk
                          2
                        
                        
                          θ
                          2
                        
                      
                      ⁢
                      
                        (
                        
                          1
                          -
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            θ
                          
                        
                        )
                      
                    
                    +
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                  
                
                
                  
                    
                      
                        
                          
                            ψ
                            mk
                          
                          ⁢
                          
                            ζ
                            mk
                          
                        
                        
                          θ
                          2
                        
                      
                      ⁢
                      
                        (
                        
                          1
                          -
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            θ
                          
                        
                        )
                      
                    
                    -
                    
                      
                        
                          ξ
                          mk
                        
                        θ
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                  
                
                
                  
                    y
                    mk
                  
                
              
              
                
                  
                    
                      
                        
                          
                            ζ
                            mk
                          
                          ⁢
                          
                            ξ
                            mk
                          
                        
                        
                          θ
                          2
                        
                      
                      ⁢
                      
                        (
                        
                          1
                          -
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            θ
                          
                        
                        )
                      
                    
                    -
                    
                      
                        
                          ψ
                          mk
                        
                        θ
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                  
                
                
                  
                    
                      
                        
                          
                            ζ
                            mk
                          
                          ⁢
                          
                            ψ
                            mk
                          
                        
                        
                          
                            θ
                            ⁢
                            
                              
                            
                          
                          2
                        
                      
                      ⁢
                      
                        (
                        
                          1
                          -
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            θ
                          
                        
                        )
                      
                    
                    +
                    
                      
                        
                          ξ
                          mk
                        
                        θ
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                  
                
                
                  
                    
                      
                        
                          ζ
                          mk
                          2
                        
                        
                          θ
                          2
                        
                      
                      ⁢
                      
                        (
                        
                          1
                          -
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            θ
                          
                        
                        )
                      
                    
                    +
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                  
                
                
                  
                    z
                    mk
                  
                
              
              
                
                  0
                
                
                  0
                
                
                  0
                
                
                  1
                
              
            
            ]
          
        
      
    
    
      
        (
        28
        )
      
    
  
  
    
      
        θ
        =
        
          
            
              ξ
              mk
              2
            
            +
            
              ψ
              mk
              2
            
            +
            
              ζ
              mk
              2
            
          
        
      
    
    
      
        (
        29
        )
      
    
  




The transformation matrix MOA2(sOA2) is defined by the following expressions:




  
    
      
        
          
            M
            
              OA
              ⁢
              
                
              
              ⁢
              2
            
          
          ⁡
          
            (
            
              s
              
                OA
                ⁢
                
                  
                
                ⁢
                2
              
            
            )
          
        
        =
        
           
          
            [
            
              
                
                  
                    
                      
                        
                          ξ
                          
                            OA
                            ⁢
                            
                              
                            
                            ⁢
                            2
                          
                          2
                        
                        
                          θ
                          2
                        
                      
                      ⁢
                      
                        (
                        
                          1
                          -
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            θ
                          
                        
                        )
                      
                    
                    +
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                  
                
                
                  
                    
                      
                        
                          
                            ξ
                            
                              OA
                              ⁢
                              
                                
                              
                              ⁢
                              2
                            
                          
                          ⁢
                          
                            ψ
                            
                              OA
                              ⁢
                              
                                
                              
                              ⁢
                              2
                            
                          
                        
                        
                          
                            θ
                            ⁢
                            
                              
                            
                          
                          2
                        
                      
                      ⁢
                      
                        (
                        
                          1
                          -
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            θ
                          
                        
                        )
                      
                    
                    -
                    
                      
                        
                          ζ
                          
                            OA
                            ⁢
                            
                              
                            
                            ⁢
                            2
                          
                        
                        θ
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                  
                
                
                  
                    
                      
                        
                          
                            ξ
                            
                              OA
                              ⁢
                              
                                
                              
                              ⁢
                              2
                            
                          
                          ⁢
                          
                            ζ
                            
                              OA
                              ⁢
                              
                                
                              
                              ⁢
                              2
                            
                          
                        
                        
                          θ
                          2
                        
                      
                      ⁢
                      
                        (
                        
                          1
                          -
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            θ
                          
                        
                        )
                      
                    
                    +
                    
                      
                        
                          ψ
                          
                            OA
                            ⁢
                            
                              
                            
                            ⁢
                            2
                          
                        
                        θ
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                  
                
                
                  
                    x
                    
                      OA
                      ⁢
                      
                        
                      
                      ⁢
                      2
                    
                  
                
              
              
                
                  
                    
                      
                        
                          
                            ψ
                            
                              OA
                              ⁢
                              
                                
                              
                              ⁢
                              2
                            
                          
                          ⁢
                          
                            ξ
                            
                              OA
                              ⁢
                              
                                
                              
                              ⁢
                              2
                            
                          
                        
                        
                          θ
                          2
                        
                      
                      ⁢
                      
                        (
                        
                          1
                          -
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            θ
                          
                        
                        )
                      
                    
                    +
                    
                      
                        
                          ζ
                          
                            OA
                            ⁢
                            
                              
                            
                            ⁢
                            2
                          
                        
                        θ
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                  
                
                
                  
                    
                      
                        
                          ψ
                          
                            OA
                            ⁢
                            
                              
                            
                            ⁢
                            2
                          
                          2
                        
                        
                          θ
                          2
                        
                      
                      ⁢
                      
                        (
                        
                          1
                          -
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            θ
                          
                        
                        )
                      
                    
                    +
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                  
                
                
                  
                    
                      
                        
                          
                            ψ
                            
                              OA
                              ⁢
                              
                                
                              
                              ⁢
                              2
                            
                          
                          ⁢
                          
                            ζ
                            
                              OA
                              ⁢
                              
                                
                              
                              ⁢
                              2
                            
                          
                        
                        
                          θ
                          2
                        
                      
                      ⁢
                      
                        (
                        
                          1
                          -
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            θ
                          
                        
                        )
                      
                    
                    -
                    
                      
                        
                          ξ
                          
                            OA
                            ⁢
                            
                              
                            
                            ⁢
                            2
                          
                        
                        θ
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                  
                
                
                  
                    y
                    
                      OA
                      ⁢
                      
                        
                      
                      ⁢
                      2
                    
                  
                
              
              
                
                  
                    
                      
                        
                          
                            ζ
                            
                              OA
                              ⁢
                              
                                
                              
                              ⁢
                              2
                            
                          
                          ⁢
                          
                            ξ
                            
                              OA
                              ⁢
                              
                                
                              
                              ⁢
                              2
                            
                          
                        
                        
                          θ
                          2
                        
                      
                      ⁢
                      
                        (
                        
                          1
                          -
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            θ
                          
                        
                        )
                      
                    
                    -
                    
                      
                        
                          ψ
                          
                            OA
                            ⁢
                            
                              
                            
                            ⁢
                            2
                          
                        
                        θ
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                  
                
                
                  
                    
                      
                        
                          
                            ζ
                            
                              OA
                              ⁢
                              
                                
                              
                              ⁢
                              2
                            
                          
                          ⁢
                          
                            ψ
                            
                              OA
                              ⁢
                              
                                
                              
                              ⁢
                              2
                            
                          
                        
                        
                          
                            θ
                            ⁢
                            
                              
                            
                          
                          2
                        
                      
                      ⁢
                      
                        (
                        
                          1
                          -
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            θ
                          
                        
                        )
                      
                    
                    +
                    
                      
                        
                          ξ
                          
                            OA
                            ⁢
                            
                              
                            
                            ⁢
                            2
                          
                        
                        θ
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                  
                
                
                  
                    
                      
                        
                          ζ
                          
                            OA
                            ⁢
                            
                              
                            
                            ⁢
                            2
                          
                          2
                        
                        
                          θ
                          2
                        
                      
                      ⁢
                      
                        (
                        
                          1
                          -
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            θ
                          
                        
                        )
                      
                    
                    +
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                  
                
                
                  
                    z
                    
                      OA
                      ⁢
                      
                        
                      
                      ⁢
                      2
                    
                  
                
              
              
                
                  0
                
                
                  0
                
                
                  0
                
                
                  1
                
              
            
            ]
          
        
      
    
    
      
        (
        30
        )
      
    
  
  
    
      
        θ
        =
        
          
            
              ξ
              
                OA
                ⁢
                
                  
                
                ⁢
                2
              
              
                2
                ⁢
                
                  
                
              
            
            +
            
              ψ
              
                OA
                ⁢
                
                  
                
                ⁢
                2
              
              2
            
            +
            
              ζ
              
                OA
                ⁢
                
                  
                
                ⁢
                2
              
              2
            
          
        
      
    
    
      
        (
        31
        )
      
    
  




On the other hand, when the j-th data is about a reference index, function Fj(s) is configured by modifying expressions (26) to

xCPkji=[xCPkjiyCPkjizCPkji1]T=MA2C−1·MOA2−1(sOA2)·MOMkj·xMPkji  (26A)

Here, MOMkj is a transformation matrix (i.e., a matrix for transformation from the kj-th index-coordinate system to the object-coordinate system) deduced from known position & orientation of the reference index in the object coordinate system.

In step S6090, with respect to the corresponding j from 1 through Nj and i from 1 through Nk, the computing-section 1060 computes an error Δupkji between the actual image coordinates upkji of the index Pknj included in each data in the data list DL and the calculation value upkji, of the corresponding image coordinates with the following expression:

ΔuPkji=uPkji−uPkji′  (32)

In step S6100, with respect to the corresponding j from 1 through Nj and i from 1 through Nk, the computing section 1060 computes an image Jacobian Juspkji (=∂upkji/∂s) including a state vector: s=[sm1T sm2T . . . smKoT sOA2T]T, as a parameter, i.e., a Jacobian matrix of 2 rows and (6Ko+6) columns, having solutions as its respective elements obtained by partially differentiating the function Fj(s) given by expression (35), which will be described later, with the respective elements of each of the state vectors smk and sOA2.
In step S6110, the computing-section 1060 computes a compensation value Δs of the vector s on the basis of the error Δupkji and the Jacobian Juspkji computed with the above-described steps with respect to the corresponding j from 1 through Nj and i from 1 through Nk. To be more specific, the computing section 1060 prepares a (2×Nj×Nk)-dimension error vector U configured by vertically aligning Δupkji and a matrix φ of (2×Nj×Nk) rows and (6Ko+6) columns configured by vertically aligning the Jacobian Juspkji, respectively, given by expressions (33) and (34), and computes the compensation value Δs with the aid of expression (35) on the basis of the pseudo inverse matrix φ+ of the matrix φ given by expression (34).




  
    
      
        U
        =
        
          [
          
            
              
                
                  Δ
                  ⁢
                  
                    
                  
                  ⁢
                  
                    u
                    
                      
                        p
                        
                          k
                          1
                        
                      
                      ⁢
                      1
                    
                  
                
              
            
            
              
                
                  Δ
                  ⁢
                  
                    
                  
                  ⁢
                  
                    u
                    
                      
                        p
                        
                          k
                          1
                        
                      
                      ⁢
                      2
                    
                  
                
              
            
            
              
                ⋮
              
            
            
              
                
                  Δ
                  ⁢
                  
                    
                  
                  ⁢
                  
                    u
                    
                      
                        p
                        
                          k
                          j
                        
                      
                      ⁢
                      i
                    
                  
                
              
            
            
              
                ⋮
              
            
            
              
                ⋮
              
            
            
              
                
                  Δ
                  ⁢
                  
                    
                  
                  ⁢
                  
                    u
                    
                      
                        p
                        
                          k
                          
                            N
                            j
                          
                        
                      
                      ⁢
                      
                        N
                        k
                      
                    
                  
                
              
            
          
          ]
        
      
    
    
      
        (
        33
        )
      
    
  
  
    
      
        Φ
        =
        
          [
          
            
              
                
                  J
                  us
                  
                    
                      p
                      
                        k
                        1
                      
                    
                    ⁢
                    1
                  
                
              
            
            
              
                
                  J
                  us
                  
                    
                      p
                      
                        k
                        1
                      
                    
                    ⁢
                    2
                  
                
              
            
            
              
                ⋮
              
            
            
              
                
                  J
                  us
                  
                    
                      p
                      
                        k
                        j
                      
                    
                    ⁢
                    i
                  
                
              
            
            
              
                ⋮
              
            
            
              
                ⋮
              
            
            
              
                
                  J
                  us
                  
                    
                      p
                      
                        k
                        
                          N
                          j
                        
                      
                    
                    ⁢
                    
                      N
                      k
                    
                  
                
              
            
          
          ]
        
      
    
    
      
        (
        34
        )
      
    
  
  
    
      
        
          Δ
          ⁢
          
            
          
          ⁢
          s
        
        =
        
          
            φ
            +
          
          ⁢
          U
        
      
    
    
      
        (
        35
        )
      
    
  




Because of being given by a vector of (6Ko+6) dimensions, the compensation value As can be obtained if (2×Nj×Nk) is equal to (6Ko+6) or greater. Although φ+ can be computed by, for example, an equation: φ+=(φTφ)−1φT, it may be obtained differently.
In step S6120, the computing-section 1060 compensates for the vector s on the basis of the compensation value Δs computed in step S6110 and sets the obtained value as a new vector s according to the following expression:

s=s+Δs  (36)

The state vector: s=[sm1T sm2T - - - smKoT sOA2T]T is configured by the state vectors smk and sOA2, respectively, of the positions & orientations of the respective calibrating indices and the second six-degree-of-freedom position-orientation sensor both in the object coordinate system.
In step S6130, the computing section 1060 determines whether calculation of these values converges with the aid of a certain determination criterion, for example, the magnitude of the error vector U whether being smaller than a predetermined threshold or the magnitude of the computed compensation value Δs whether being smaller than a predetermined threshold. If the determination is non-convergence of the calculation, the computing section 1060 performs the steps from step S6080 again on the basis of the compensated state vector s.
If determination in step S6130 is convergence of calculation, in step S6140, the computing section 1060 outputs the positions & orientations smk (k=1 . . . Ko) and sOA2 of the obtained state vector s, respectively, of the respective calibrating indices and the second six-degree-of-freedom position-orientation sensor both in the object coordinate system so as to serve as calibration information. On this occasion, the state vectors smk (k=1 . . . Ko) and sOA2 may be output in any one of output forms: (a) smk and sOA2 without modification, (b) a ternary vector standing for the position components and an Euler angle or a 3×3 rotation matrix standing for the orientation components of smk and sOA2, and (c) 4×4 coordination-transformation matrices MOM and MOA2 generated from smk and sOA2, respectively.
Finally, the computing section 1060 determines in step S6150 whether the calibration process is to be finished. When the operator instructs the calibration apparatus 1100 to finish the calibration process, the process ends. When the operator instructs the calibration apparatus to continue the calibration process (to perform recalibration), the process returns to step S6010 and the calibration apparatus waits for an input of a data-obtaining command.
With the above-described process, the positions or the respective positions & orientations of the respective indices and the second six-degree-of-freedom position-orientation sensor both placed and relative to the measuring object (i.e., in the measuring-object coordinate system) can be obtained.
Modification 1 of the Second Embodiment
While the positions & orientations of each of the plurality of indices and the second six-degree-of-freedom position-orientation sensor both in the object coordinate system and placed on the object serve as calibration information in the second embodiment, the position & orientation of the first six-degree-of-freedom position-orientation sensor placed on the camera 1010 in the camera coordinate may be additionally added to the calibration information.
While the position & orientation MCA1 of the first six-degree-of-freedom position-orientation sensor placed on the camera in the camera coordinate system are regarded as known in the second embodiment, these are regarded here as unknown parameters. On the basis of the positions & orientations MWA1 and MWA2 of the six-degree-of-freedom position-orientation sensors A1 and A2 placed on the camera and the object, obtained from sensor-measurement values of the six-degree-of-freedom position-orientation sensors A1 and A2, respectively, the position & orientation MA2A1 of the six-degree-of-freedom position-orientation sensor A1 placed on the camera 1010 in the coordinate system of the six-degree-of-freedom position-orientation sensor A2 is computed by the following expression:

MA2A1=MWA2−1·MWA1  (37)

When MCA1, MA2A1, MOA2, and MOM, respectively, stand for a transformation matrix expressing the position & orientation of the first six-degree-of-freedom position-orientation sensor in the camera coordinate system; a transformation matrix expressing the position & orientation of the first six-degree-of-freedom position-orientation sensor A1 in the coordinate system of the second six-degree-of-freedom position-orientation sensor A2; a transformation matrix expressing the position & orientation of the second six-degree-of-freedom position-orientation sensor A2 in the object coordinate system; and a transformation matrix expressing the positions & orientations of the respective indices placed on the object in the object-coordinate system, a transformation matrix MCM from the index-coordinate system to the camera-coordinate system is given by the following expression:

MCM=MCA1·MA2A1−1·MOA2−1·MOM  (38)

In the same fashion as in the second embodiment, calibration information to be obtained, in other words, the positions & orientations MOM, MOA2, and MCA1, respectively, of each of the respective indices in the object-coordinate system; the second six-degree-of-freedom position-orientation sensor in the object coordinate system; and the first six-degree-of-freedom position-orientation sensor in the camera coordinate system, are treated as senary vectors. Here, the position & orientation of the corresponding index in the object-coordinate system are written by the state vector: smk=[xmk ymk zmk ξmk ψmk ζmk]T (k=1 . . . Ko), those of the second six-degree-of-freedom position-orientation sensor in the object coordinate system are written by the state vector: sOA2=[xOA2 yOA2 zOA2 ξ0A2 ψOA2 ζOA2]T, and those of the first six-degree-of-freedom position-orientation sensor in the camera coordinate system are written by a state vector: sCA1=[xCA1 yCA1 zCA1 ξCA1 ψCA1 ζCA1]T.
Appropriate initial values are provided to the respective state vectors smk, sOA2, and sCA1. For example, the initial values are previously obtained by manual measurement. An unknown parameter serving as calibration information is expressed by s=[sm1T sm2T . . . smKoT sOA2T sCA1T]T, and, for example, with the repetitive calculation by using an image Jacobian in the same fashion as in the second embodiment, an optimal solution of the vector s is computed with respect to all inputted data. With this, the positions & orientations of the respective indices in the object-coordinate system, and the positions & orientations of the first and second six-degree-of-freedom position-orientation sensors respectively in the object and camera coordinate systems are obtained and output as calibration information.
Modification 2 of the Second Embodiment
The calibration apparatus according to the second embodiment is intended to calibrate the respective positions & orientations of the respective indices and the six-degree-of-freedom sensor both placed on and relative to a measuring object (i.e., in the measuring-object coordinate system), and the positions of the respective points constituting each of the indices placed on the object are known in the index-coordinate system defined with respect to the corresponding index. In the meantime, the shape of the index is not limited to the above-described one, and it may have a point shape (hereinafter, an index having a point shape is referred to as a point index). In this case, the calibration apparatus computes the positions of point indices placed on and relative to an object and the position & orientation of the six-degree-of freedom sensor relative to the measuring object so as to serve as calibration information. The point indices may be configured by, for example, circular markers having respectively different colors or feature points such as natural features having respectively different texture features.
Point indices placed on the object 1000 as shown in FIG. 6 are written by Pk (k=1 . . . Ko), where Ko stands for the number of calibrating indices placed on the object. The point indices Pk placed on the object may be of any type as long as the image coordinates of each projection figure on a captured image can be detected, and the point indices can be individually identified. The object has at least three indices Pk (k=Ko+1 . . . Ko+K1) (not shown in FIG. 6) displaced thereon, serving as reference indices for defining the object-coordinate system, and their positions in the object-coordinate system are known and not aligned on a common straight line. When a vector expressing the positions of the point indices Pk placed on the measuring-object in the object coordinate is written by xOMk, a geometrical transformation upon observing the point indices Pk placed on the object through the camera is given by the following expression:

xCPkj=MA2C−1·MOA2−1(sOA2)·xOMkj  (39)

In the expression (39), xCPkj stands for the camera coordinates of the point index Pk, j (=1 . . . Nj) stands for an index of the corresponding set of data registered in the data list DL, and Nj stands for the total number of sets of registered data.
In the same fashion as in the second embodiment, calibration information to be obtained, i.e., the position xOMk of the corresponding one of the calibrating point indices in the object coordinate system is written by the state vector: sOMk=[xOMk yOMk zOMk]T (k=1 . . . Ko), and the position & orientation MOA2 of the second six-degree-of-freedom position-orientation sensor in the object coordinate system is written by the state vector: sOA2=[xOA2 yOA2 zOA2 ξOA2 ψOA2 ζOA2]T. The position xOMk is known value if the point index is reference one.
Appropriate initial values are provided to the respective state vectors sOMk (k=1 . . . Ko) and sOA2. The initial values are previously obtained, for example, by manual measurement. An unknown parameter serving as calibration information is expressed by s=[sOM1T sOM2T . . . sOMKoT sOA2T]T, and, for example, with the repetitive calculation by using an image Jacobian in the same fashion as in the second embodiment, an optimal solution of the vector s is computed with respect to all inputted data. With this, the positions of the respective calibrating point indices and the position & orientation of the six-degree-of-freedom position-orientation sensor both in the object coordinate system can be output as calibration information.
Other Embodiments
One skilled in the art will appreciate that the present invention can be embodied such that a storage medium (or a recording medium) having the program code (software) stored therein, for achieving the functions of each of the foregoing embodiments, is supplied to a system or an apparatus, and a computer (or a CPU or a micro-processing unit (MPU)) of the system or the apparatus reads and executes the program code stored in the storage medium. Also, one skilled in the art will appreciate that, besides achievement of the functions of each of the embodiments by reading and executing the program code with the computer, the functions can be achieved such that an operating system (OS) or the like running on the computer performs a part of or all of the actual process according to instructions of the program code.
In addition, those skilled in the art will appreciate that the functions can be achieved such that the program code read from the storage medium is written in a functional extension card inserted in the computer or a memory included in a functional extension unit connected to the computer, and a CPU included in the functional extended card or the functional extended unit performs a part of or all of the actual process according to instructions of the program code.
When the present invention is applied to the storage medium, the storage medium stores the program code corresponding to the above-described flowchart shown FIG. 2 or 4.
As many apparently widely different embodiments of the present invention can be made without departing from the spirit and scope thereof, it is to be understood that the invention is not limited to the specific embodiments thereof except as defined in the appended claims.
This application claims the benefit of Japanese Application No. 2004-320638 filed Nov. 4, 2004 and Japanese Application No. 2005-217554 filed Jul. 27, 2005, which are hereby incorporated by reference herein in their entireties.

## Claims

1. An information processing method for an information processing apparatus including a calibration-information computing section to calculate first arrangement information of first indices arranged on an object and second arrangement information of a sensor arranged on the object, the information processing method comprising steps of:
obtaining a first image of the object captured by a capture device;
receiving orientation information of the object, the orientation information being measured by the sensor arranged on the object;
detecting; the first indices arranged on the object, from the first image and
calculating, by the calibration-information computing section, the first arrangement information relative to the object and the second arrangement information relative to the object, from the received orientation information and first image coordinates of the detected first indices.

2. The information processing method according to claim 1, wherein the second arrangement information is information about orientation of the sensor relative to the object.

3. The information processing method according to claim 1, wherein the sensor is a six-degree-of-freedom position-orientation sensor and the second arrangement information is information about position and orientation of the six-degree-of-freedom position-orientation sensor relative to the object.

4. The information processing method according to claim 1, wherein the object includes an imaging section,
the information processing method further comprising steps of:
obtaining a second image captured by the imaging section; and
detecting second indices from the second image,
wherein, in the calculating step, the first arrangement information relative to the object and the second arrangement information relative to the object are calculated from the received orientation information, the first image coordinates of the detected first indices, and second image coordinates of the detected second indices.

5. The information processing method according to claim 4,
wherein the calculating step includes steps of:
setting values as the first arrangement information and the second arrangement information;
estimating image coordinates of the second indices of the second image from the received orientation information and the first image coordinates of the detected first indices, based on the set first arrangement information and the set second arrangement information; and
updating values as the first arrangement information and the second arrangement information such that an error between the second image coordinates and the estimated image coordinates is reduced.

6. A computer-readable storage medium having stored thereon a program implementing an information processing method on an information processing apparatus including a calibration-information computing section to calculate first arrangement information of first indices arranged on an object and second arrangement information of a sensor arranged on the object, the information processing method performed by the program stored on the computer-readable storage medium comprising:
obtaining a first image of the object captured by a capture device;
receiving orientation information of the object, the orientation information being measured by the sensor arranged on the object;
detecting; the first indices arranged on the object, from the first image and
calculating, by the calibration-information computing section, the first arrangement information relative to the object and the second arrangement information relative to the object, from the received orientation information and first image coordinates of the detected first indices.

7. An information processing apparatus for computing first arrangement information of first indices arranged on an object and second arrangement information of a sensor arranged on the object, the information processing apparatus comprising:
an imaging section configured to capture the object;
a measurement-value inputting section configured to input orientation information of the object, the orientation information being measured by the sensor arranged on the object;
an image-receiving section configured to receive an image captured by the imaging section;
a detecting section configured to detect the first indices arranged on the object, from the first image; and
a calibration-information computing section configured to calculate the first arrangement information relative to the object and the second arrangement information relative to the object, from the received orientation information and first image coordinates of the detected first indices.