Self-position estimation device and self-position estimation method

Abstract

The present invention provides a self-position estimation device and method that enable highly accurate self-position estimation of a camera without being affected by refraction errors caused by a light-transmitting part placed in front of the target. [Solution] The self-position estimation device 50 includes a self-position estimation unit 62 that estimates the self-position of the camera 20 based on an image taken by the camera 20 of a target group 16 in which a transparent window material 30 is placed in front of a plurality of targets 24; a line of sight detection unit 68 that detects a line of sight L from the principal point of the camera 20 toward the target 24 through the transparent window material 30 based on the self-position of the camera 20 estimated by the self-position estimation unit 62; and a correction unit 70 that corrects the self-position of the camera 20 based on the distance between the line of sight L and the target 24.

Description

[Technical Field]

[0001] The present invention relates to a self-position estimation device and a self-position estimation method capable of estimating the self-position of a camera. [Background technology]

[0002] Traditionally, portable three-dimensional coordinate measuring machines (CMMs) have been mounted on robots to automatically measure large workpieces. While portable CMMs are meeting accuracy requirements of several tens of micrometers, bridge-type CMMs are currently being used to address high-precision requirements of 10 micrometers or less.

[0003] Furthermore, measurement methods using laser trackers or markers are known as techniques to achieve an accuracy of several tens of micrometers (see, for example, Patent Document 1). These methods include a base station equipped with an elevation and azimuth swivel function, and further equipped with a distance sensor or camera to calculate the distance and orientation of the measurement head. With this measurement method, because the movable range of the swivel mechanism and the measuring range of the distance sensor are long, measurements can be taken over a wide area.

[0004] However, in measurement methods using laser trackers or markers, the measurement accuracy tends to deteriorate as the distance between the base station and the measurement head increases, mainly due to the limitations of the angle accuracy of the swivel mechanism.

[0005] On the other hand, a method is known for estimating the camera's own position (position and orientation) from images taken with a camera of targets whose positions in three-dimensional space are known (see, for example, Patent Document 2). This method has the advantage of being able to easily obtain high accuracy (5 μm or less) with a relatively simple mechanism by relating the position of each target in three-dimensional space with the position of each target in the captured image and performing calculations. [Prior art documents] [Patent Documents]

[0006] [Patent Document 1] Japanese Patent Application Laid-Open No. 2020-148515 [Patent Document 2] Japanese Patent Application Laid-Open No. 2022-30807 [Summary of the Invention] [Problems to be Solved by the Invention]

[0007] By the way, in the target plate (target group composed of a plurality of targets) used in the above method, a transparent window material (light transmission part) such as a flash material may be arranged on the front surface of the target for antifouling or the like. In this case, an error occurs in the position of the target (target coordinates) on the image captured by the camera due to the influence of refraction by the transparent window material. As a result, there is a problem that the estimation accuracy of the self-position of the camera decreases.

[0008] FIG. 7 is a diagram for explaining the refraction of light. As shown in FIG. 7, when light is incident on the boundary of media having different refractive indices, the traveling direction of the light changes due to refraction. Here, the incident angle with respect to the normal line of the boundary surface is θ A and the refraction angle is θ B Let. Also, the absolute refractive index of the medium on the incident side is n A and the absolute refractive index of the medium on the transmission side is n B Let. Also, the speed of light in the medium on the incident side is v A and the speed of light in the medium on the transmission side is v B Let. Also, since the frequency f of the wave does not change before and after refraction, from the formula v = fλ representing the speed of the wave using the wavelength λ, the refractive index n A , n B For the speed of light in the medium of, v A = fλ A , v B = fλ B Holds. Then, from Snell's law, the following formula (1) holds. Note that n AB Is the ratio of the absolute refractive indices of the two media (referred to as "relative refractive index"). [Equation]

[0009] Figure 8 shows the angle of incidence θ. A and the angle of refraction θ B This graph shows the relationship. In Figure 8, the horizontal axis is the angle of incidence θ. A The vertical axis represents the angle of refraction θ. B and the angle of incidence θ A This shows the difference. Here, as an example, if the incident medium is air (n A ≈1.00) and the medium on the transmitting side is glass (n B The graph shows the case where the angle of incidence θ is approximately 1.44. As shown in Figure 8, A The larger the angle of refraction θ becomes (that is, the further the incident light on the interface is from perpendicular to the surface), the greater the angle of refraction θ. B and the angle of incidence θ A The difference (absolute value) between the two values ​​increases, indicating that the error due to refraction increases.

[0010] Figure 9 is a schematic diagram showing the line of sight from camera 120 to each target 124. As shown in Figure 9, when camera 120 photographs multiple targets 124 through the transparent window material 130, the amount of refraction by the transparent window material 130 (the difference between the angle of refraction and the angle of incidence) differs depending on the direction of the line of sight from camera 120 to each target 124.

[0011] Figure 10 illustrates the effect of the transparent window material on refractive error. In Figure 10, Image 100A is an image of the target plate without the transparent window material. Image 100B is an image of the target plate with the transparent window material (in this example, a 5mm thick glass material). Image 100C is a composite image created by superimposing Images 100A and 100C, each made semi-transparent.

[0012] As shown in Figure 10, when the camera is tilted relative to the target plate during shooting, the direction and amount of shift in the target coordinates differ for each line of sight to each target due to differences in the direction and angle of incidence to the transparent window material. In other words, if a transparent window material (light-transmitting part) that causes refraction is placed between the camera and the target, the amount of error due to refraction differs depending on the direction and angle of the line of sight (light) from the camera to each target. As a result, errors occur in the position of the targets in the image captured by the camera, which reduces the accuracy of the camera's self-position estimation.

[0013] This invention has been made in view of these circumstances, and aims to provide a self-position estimation device and a self-position estimation method that can perform high-precision self-position estimation of a camera without being affected by refraction errors caused by a light-transmitting part placed in front of the target. [Means for solving the problem]

[0014] To achieve the above objective, the present invention comprises the following embodiments.

[0015] The self-position estimation device according to the first embodiment includes: a self-position estimation unit that estimates the self-position of a camera based on an image taken by a camera of a group of targets in which light-transmitting parts are arranged in front of multiple targets; a line-of-sight detection unit that detects a line of sight from the principal point of the camera toward the target via the light-transmitting parts based on the self-position of the camera estimated by the self-position estimation unit; and a correction unit that corrects the self-position of the camera based on the distance between the line of sight and the target.

[0016] In the second embodiment of the self-position estimation device, in the first embodiment, the line-of-sight detection unit detects a first line of sight passing through the principal point of the camera and the feature point of the target image on the image, a second line of sight that refracts with respect to the optical axis of the first line of sight and passes through the light-transmitting part, and a third line of sight that refracts with respect to the optical axis of the second line of sight and passes through the space in which the target is placed.

[0017] In the third embodiment of the self-position estimation device, in the first or second embodiment, the gaze detection unit detects multiple gazes for each target, and the correction unit corrects the camera's self-position so that the sum of the squares of the distances obtained for each gaze is minimized.

[0018] A self-position estimation method according to the fourth embodiment comprises: a self-position estimation step of estimating the self-position of a camera based on an image taken by a camera of a group of targets in which light-transmitting parts are arranged in front of multiple targets; a line-of-sight detection step of detecting a line of sight from the principal point of the camera toward the targets through the light-transmitting parts based on the self-position of the camera estimated in the self-position estimation step; and a correction step of correcting the self-position of the camera based on the distance between the line of sight and the targets. [Effects of the Invention]

[0019] According to the present invention, it is possible to accurately estimate the camera's own position without being affected by refraction errors caused by a light-transmitting part placed in front of the target. [Brief explanation of the drawing]

[0020] [Figure 1] This is a block diagram of the self-localization system according to this embodiment. [Figure 2] This is a schematic diagram showing the general configuration of the self-localization system according to this embodiment. [Figure 3] This is an explanatory diagram for describing the camera projection model. [Figure 4] This flowchart shows an example of the self-position estimation process performed by the self-position estimation device of this embodiment. [Figure 5] This diagram models the positional relationship between the camera and the target group. [Figure 6] This diagram shows the relationship between the direction vectors (line of sight vectors) that constitute the first, second, and third lines of sight, respectively. [Figure 7] This is a diagram to explain the refraction of light. [Figure 8]This graph shows the relationship between the angle of incidence and the angle of refraction. [Figure 9] This is a schematic diagram showing the line of sight from the camera to each target. [Figure 10] This diagram illustrates the effect of refractive error caused by transparent window materials. [Modes for carrying out the invention]

[0021] Embodiments of the present invention will be described below with reference to the attached drawings.

[0022] [Self-localization system] Figure 1 is a block diagram showing the self-localization system 10 according to this embodiment. Figure 2 is a schematic diagram showing the general configuration of the self-localization system 10 according to this embodiment.

[0023] As shown in Figures 1 and 2, the self-position estimation system 10 comprises a probe head 12, a target group 16, and a self-position estimation device 50. The self-position estimation device 50 is an example of the self-position estimation device of the present invention. In this embodiment, the self-position estimation device 50 is configured separately from the probe head 12, but the probe head 12 may have at least some of the functions of the self-position estimation device 50.

[0024] The probe head 12 is a portable three-dimensional coordinate measuring machine that measures the three-dimensional coordinates of a workpiece (not shown). The probe head 12 is equipped with a contact-type or non-contact-type probe 18. The probe 18 may be a contact-type (touch probe type) or a non-contact-type (laser type, optical type), as long as it is capable of measuring the three-dimensional coordinates of the workpiece. Examples of non-contact probes include laser scanners, point lasers, and line lasers. By holding the probe head 12 and performing measurements, the three-dimensional coordinates at the measurement points of the workpiece can be obtained.

[0025] The probe head 12 has a self-position estimation function, and when the camera 20 mounted on the probe head 12 photographs the target group 16, the self-position estimation device 50 described later can estimate the probe head 12's own position (position and orientation).

[0026] As shown in Figure 2, the target group 16 consists of multiple (many) targets 24 arranged in a two-dimensional manner. Each target 24 is formed as a point or dot, and is spaced apart from each other. Each target 24 may also be composed of a small point-shaped light source (point light source), such as an LED. The relative positions of the targets 24 are known. The shape and size of each target 24 are also known.

[0027] In this embodiment, the target group 16 has a transparent window material 30 (see Figure 5) made of, for example, glass, placed in front of (on the surface of) a plurality of targets 24. The transparent window material 30 constitutes a light-transmitting portion that transmits light to which the camera 20 is sensitive (visible light in this example), and causes refraction of light at the interface of the transparent window material 30. Note that the transparent window material 30 is an example of a light-transmitting portion of the present invention.

[0028] [Self-position estimation device] Next, the self-position estimation device 50 will be described. As shown in Figure 1, the self-position estimation device 50 is composed of, for example, a personal computer and includes a calculation processing unit 52 and a storage unit 54. The camera 20 mounted on the probe head 12 is connected to the self-position estimation device 50. The method of connection to the camera 20 is not particularly limited and may be via a cable, or via a wired or wireless network.

[0029] The storage unit 54 stores control programs and various data. The storage unit 54 is composed of, for example, a hard disk drive (HDD) or a solid state drive (SSD). The storage unit 54 may also include temporary memory elements composed of, for example, DRAM (Dynamic Random Access Memory) or SRAM (Static Random Access Memory), or other RAM (Random Access Memory), and may function as a work area for the arithmetic processing unit 52.

[0030] The memory unit 54 stores the target images, which will be described later. The memory unit 54 also stores information (target group information) regarding the shape, size, and arrangement of each target 24 that make up the target group 16.

[0031] The arithmetic processing unit 52 executes various arithmetic processes performed by the self-position estimation device 50. The arithmetic processing unit 52 comprises an arithmetic circuit composed of various processors and memory. The various processors include CPUs (Central Processing Units), GPUs (Graphics Processing Units), ASICs (Application Specific Integrated Circuits), and programmable logic devices [e.g., SPLDs (Simple Programmable Logic Devices), CPLDs (Complex Programmable Logic Devices), and FPGAs (Field Programmable Gate Arrays)]. The various functions of the arithmetic processing unit 52 may be implemented by a single processor, or by multiple processors of the same or different types.

[0032] The arithmetic processing unit 52 functions as the image acquisition unit 60, the self-position estimation unit 62, the gaze detection unit 68, the correction unit 70, and the judgment processing unit 72 by reading and executing the control program stored in the memory unit 54. The self-position estimation unit 62 includes a feature point detection unit 64 and an optimization calculation unit 66.

[0033] Note that the image acquisition unit 60 is an example of the image acquisition unit of the present invention. The self-position estimation unit 62 is an example of the self-position estimation unit of the present invention. The gaze detection unit 68 is an example of the gaze detection unit of the present invention. The correction unit 70 is an example of the correction unit of the present invention.

[0034] [Camera projection model] Before describing the self-position estimation process performed by the self-position estimation device 50 of this embodiment, we will first explain the camera projection model that underlies it. Figure 3 is an explanatory diagram illustrating the camera projection model.

[0035] As shown in Figure 3, the world coordinate system (Σ) w O is a coordinate system that represents a position in three-dimensional space (real space), with the origin being O w Let X be mutually orthogonal. w Axis, Y w axis, Z w This is a three-dimensional Cartesian coordinate system with the axes as coordinate axes. Note that this is the world coordinate system Σ. w Any coordinate system can be used as long as the position in three-dimensional space (three-dimensional position) can be determined. Camera coordinate system Σ c The optical axis center of camera 20 is O c Let O be the origin. c From there, turn right to X c Axis, downward direction is Y c The axis, the optical axis direction, is Z c This is a three-dimensional Cartesian coordinate system. Image coordinate system Σ s is the camera coordinate system Σ c The origin of O c From Z c The origin is the upper left corner of the image plane IP, which is at a focal distance f in the direction X c Axis and Y cThis is a two-dimensional Cartesian coordinate system (pixel coordinate system) with U-axis and V-axis running parallel to the other axes.

[0036] First, the world coordinate system Σ of point P (object point) in three-dimensional space w Coordinates (x w , y w , z w The camera coordinate system Σ is obtained using the rotation matrix R and translation vector t of camera 20, as shown in equation (2) below. c It can be converted to coordinates (x, y, z) in a given space.

[0037]

number

[0038] Here, [R|t] w,c is the world coordinate system Σ w From camera coordinate system Σ c This is a transformation matrix (external parameter matrix) for transforming coordinates to the world coordinate system Σ w This shows the orientation and position of camera 20. [R|t] w,c Each component r 11 , r 12 , ..., r 33 t x t y t z These are called external camera parameters. Note that in the following explanation, "R" and "R(R) x , R y , R z )" is used synonymously. x , R y , R z represents the rotation components in the x, y, and z directions of the rotation matrix R. Also, the transformation matrix [R|t] w,c This is sometimes simply written as [R|t].

[0039] Next, the camera coordinate system Σ c If point P, located at (x, y, z) as viewed from the image plane IP, has coordinates (pixel coordinates) of the projected point Q as (u, v), then the following relationships shown in equations (3) through (8) hold.

[0040]

number

[0041]

number

[0042]

number

[0043]

number

[0044]

number

[0045]

number

[0046] Here, (x', y') is the camera coordinate system Σ c This represents the coordinates of the projected point when point P, located at (x, y, z) as viewed from the camera, is projected onto the normalized image plane (z=1). Furthermore, (x'', y'') represents the coordinates of the projected point (distorted point) when point P is projected onto the normalized image plane, taking into account the lens distortion of camera 20.

[0047] Also, PH x ,f y This indicates the focal length in the x and y directions, expressed in pixels. Also, c x , c y This is the image coordinate system Σ sThis indicates the optical center (the position where the optical axis of camera 20 intersects the image plane IP, the optical center on a pixel-by-pixel basis). k1, k2, and k3 are radial distortion coefficients, and p1 and p2 are tangential distortion coefficients. In this specification, the focal length f x ,f y , optical center c x , c y These are called the internal parameters of camera 20, and the distortion coefficients k1, k2, k3, p1, and p2 are called the distortion parameters of camera 20.

[0048] [Self-position estimation method] Next, the processing procedure (an example of a self-position estimation method) of the self-position estimation process performed by the self-position estimation device 50 of this embodiment will be described. Figure 4 is a flowchart showing an example of the self-position estimation process performed by the self-position estimation device 50 (arithmetic processing unit 52) ​​of this embodiment. At the start of this flowchart, it is assumed that the camera 20 has already been calibrated, and that the camera matrix K, including the intrinsic parameters (focal length, optical center) and distortion parameters (distortion coefficient) of the camera 20, is known.

[0049] First, as shown in Figure 2, the camera 20 mounted on the probe head 12 captures images of the target group 16 (step S10). The images (target images) captured by the camera 20 are transmitted to the self-position estimation device 50. When the target images are transmitted to the self-position estimation device 50, the image acquisition unit 60 acquires the target images and stores them in the storage unit 54.

[0050] Next, the self-position estimation unit 62 estimates the camera 20's own position (position and orientation) based on the target image captured by the camera 20. The following describes the processing performed by the self-position estimation unit 62.

[0051] First, the feature point detection unit 64 performs a feature point detection process to detect multiple feature points from the target image (step S12).

[0052] Specifically, the feature point detection unit 64 reads the target image from the storage unit 54. Then, the feature point detection unit 64 performs predetermined image processing (such as grayscale conversion) on the read target image, detects feature points (image points) that indicate the position of each target 24 (target image) from the target image, and determines the coordinates (pixel coordinates) of each feature point on the target image (image plane IP; see Figure 3). The position of the centroid of the target 24 is detected as a feature point on the target image.

[0053] The feature points detected on the target image in the feature point detection process described above are called Q i This is shown, and its coordinates (2D coordinates) are (u i , v i ) (where i is a variable (a number uniquely assigned to each feature point) and is a natural number greater than or equal to 2). Note that, if necessary, each feature point Q i Coordinates (u i , v i Apply lens distortion correction to the image.

[0054] Also, an object point P in three-dimensional space j The coordinates indicating the position of each target 24 in the target group 14 are (x wj , y wj , z wj If we assume that (where j is a variable (a number uniquely assigned to each object point) and is a natural number greater than or equal to 2), then from the camera projection model shown in Figure 3, we can formulate it as follows: equation (9).

number

[0055] In equation (9), K is the camera matrix representing the intrinsic parameters of camera 20, and is known.

[0056] Here, the error function E is defined as shown in equation (10) below.

number

[0057] The error function E defined by Equation (10) is the error function indicating the correspondence between the feature point Q on the target image i and the object point P in the three-dimensional space j (the point indicating the position of each target 24). In these combinations, when the combination is such that the points are in a projective relationship with each other, the error function E becomes the minimum value.

[0058] The optimization calculation unit 66 obtains the transformation matrix [R|t] at which the error function E becomes the minimum value by non-linear optimization calculation (step S14). The transformation matrix [R|t] obtained in the optimization calculation unit 66 indicates the self-position (position and orientation) of the camera 20 in the world coordinate system Σ w is the one that indicates the self-position (position and orientation) of the camera 20 in the world coordinate system Σ.

[0059] The self-position estimation unit 62 stores the information (camera self-position information) indicating the position and orientation of the camera 20 obtained as described above in the storage unit 54. The camera self-position information stored in the storage unit 54 is used when calculating the three-dimensional coordinates of the workpiece using the probe head 12.

[0060] Here, the transformation matrix [R|t] indicating the self-position of the camera 20 is obtained based on the coordinates (target coordinates) of each feature point Q i on the target image. Therefore, in order to accurately obtain the self-position of the camera 20, it is necessary that the coordinates of each feature point Q i are accurately obtained.

[0061] However, when the transparent window material 30 is arranged in front of the target, due to the influence of refraction by the transparent window material 30, an error (hereinafter also referred to as "refraction error") occurs in the coordinates of each feature point Q i on the target image. In particular, the greater the inclination angle of the line of sight from the camera 20 to each target 24, the greater the refraction error, which is a factor that significantly reduces the estimation accuracy of the self-position of the camera 20.

[0062] Therefore, in the present embodiment, after the self-position estimation unit 62 obtains the self-position of the camera 20, the line-of-sight detection unit 68 and the correction unit 70 perform processing to reduce the influence of the refraction error caused by the transparent window material 30. Hereinafter, the processing respectively executed by the line-of-sight detection unit 68 and the correction unit 70 will be described in detail.

[0063] 〔Line-of-sight detection processing〕 The line-of-sight detection unit 68 performs a line-of-sight detection process for detecting the line of sight from the camera 20 toward each target 24 based on the self-position of the camera 20 obtained by the self-position estimation unit 62 (step S16).

[0064] FIG. 5 is a diagram modeling the positional relationship between the camera 20 and the target group 16. In the model shown in FIG. 5, the self-position of the camera 20 is defined by the transformation matrix [R|t]. Also, the coordinates (target coordinates) of the feature point Q i on the image (target image) captured by the camera 20 are (u i , v i ), and the coordinates of the object point P i in the three-dimensional space corresponding thereto (target 24) are (x wi , y wi , z wi ). Note that, by the processing performed in the self-position estimation unit 62 described above, the correspondence between the feature point Q i on the target image and the object point P i in the three-dimensional space (target 24) has already been obtained and is known. Also, in order to simplify the formulation in the following description, it is assumed that the lens distortion of the camera 20 has been corrected for the coordinates of the feature point Q i on the target image.

[0065] As shown in FIG. 5, the line of sight L c from the principal point of the camera 20 (corresponding to the origin O shown in FIG. 3) i passes through the feature point Q i on the target image (image plane IP) and heads toward the object point P i in the three-dimensional space (target 24). This line of sight L iThese are the principal point of camera 20 and feature point Q on the target image. i The first line of sight L passing through r,i And, the first line of sight L r,i The second line of sight L refracts relative to the optical axis and passes through the inside of the transparent window material 30. m,i And, the second line of sight L m,i The third line of sight L refracts relative to the optical axis and passes through the space (medium) where the target 24 is located. k,i It consists of the following. Note that the first line of sight L r,i , second line of sight L m,i , and the third line of sight L k,i Figure 6 shows the relationship between the direction vectors (line of sight vectors) that constitute each of these. The following describes each line of sight detection method.

[0066] (Detection of the first line of sight) Position of the principal point of camera 20 (first line of sight L) r,i The starting point of the first line of sight L is defined by the "t" (translation vector) of the transformation matrix [R|t] that indicates the self-position of camera 20. r,i The direction vector of (hereinafter referred to as the "first line of sight vector") is r i In that case, the first line of sight L r,i As shown in equation (11) below, the variable s r It can be obtained as a function of L. r,i (s r )" and "L r,i This is used synonymously with "".

number

[0067] First line of sight vector r i This is a direction vector indicating the direction of the line of sight from camera 20 to transparent window material 30. As shown in Figure 5, feature point Q on the target image. i Coordinates (u i , v i ) From, the first line of sight vector r i (=(x i , y i , z i )) can be expressed as shown in equation (12) below.

number

[0068] (Detection of the second line of sight) 1st line of sight L r,i The intersection of the transparent window material 30 and the surface 30a (second line of sight L) m,i (corresponding to the starting point) to x r,i And the second line of sight L m,i The direction vector (hereinafter referred to as the "second line of sight vector") is m i In that case, the second line of sight L m,i As shown in equation (13) below, the variable s m It can be obtained as a function of L. m,i (s m )" and "L m,i This is used synonymously with "".

number

[0069] As shown in Figure 6, the second line of sight vector m i The second line of sight L is incident from the surface 30a of the transparent window material 30, refracted, and transmitted through the transparent window material 30. m,i This is the direction vector. Second line-of-sight vector m i This can be calculated using Snell's Law, as shown in equation (14) below. The derivation method of equation (14) is publicly known, so a detailed explanation is omitted. Note that h1 represents the normal vector of the surface 30a of the transparent window material 30. Also, n1 is the refractive index of the space between the camera 20 and the transparent window material 30, and n2 represents the refractive index of the transparent window material 30.

number

[0070] (Detection of the third line of sight) 2nd line of sight L m,i The intersection of the transparent window material 30 and the back surface 30b (third line of sight L) k,i (corresponding to the starting point) to x m,i And the third line of sight L k,iThe direction vector (hereinafter referred to as the "third line of sight vector") is k i In that case, the third line of sight L k,i As shown in equation (15) below, the variable s k It can be obtained as a function of L. k,i (s k )" and "L k,i This is used synonymously with "".

number

[0071] As shown in Figure 6, the third line of sight, vectork i The light exits from the back surface 30b of the transparent window material 30, is refracted, and reaches object point P i Third line of sight L towards (Target 24) k,i This is the direction vector. The third line of sight vector k i This can be calculated using Snell's Law, as shown in equation (16) below. The derivation method of equation (16) is publicly known, so a detailed explanation is omitted. Note that h2 represents the normal vector of the back surface 30b of the transparent window material 30. However, the normal vector here is in the direction from the outside to the inside of the transparent window material. Also, n3 is the refractive index of the space (medium) in which the target 24 exists.

number

[0072] Note that the intersection x r,i , x m,i The calculation method is not particularly limited, and conventionally known methods can be used. Furthermore, the thickness of the transparent window material 30 is assumed to be known.

[0073] The gaze detection unit 68 detects the first gaze L using the method described above. r,i , second line of sight L m,i , and the third line of sight L k,i By determining this, the line of sight L from camera 20 to each target 24 is determined. i Detects.

[0074] The gaze detection unit 68 processes the gaze detection process described above using multiple feature points Q on the target image. i This process is repeated. This results in multiple feature points Q on the target image. i The corresponding line of sight L i The gaze detection process detects all feature points Q on the target image. i You may perform this on some of the feature points Q. i This may be done for all feature points Q on the target image. i When performing gaze detection processing on certain feature points Q, i Compared to performing gaze detection processing on the target image, the correction process described later makes it possible to correct the camera 20's own position with high accuracy. In addition, some feature points Q on the target image i When performing gaze detection processing on all feature points Q, i Compared to performing gaze detection processing on the subject, it becomes possible to reduce the time required for the correction processing described later.

[0075] [Correction process] Next, the correction unit 70 analyzes the line of sight L detected in the line of sight detection process. i Based on this, a correction process is performed to correct the self-position of the camera 20 (step S18).

[0076] The transformation matrix [R|t] indicating the self-position of the camera 20, obtained by the self-position estimation unit 62, includes the refraction error caused by the transparent window material 30 mentioned above. Therefore, the first line-of-sight vector r, which is determined based on the self-position of the camera 20, is i , second line of sight vector m i , and the third line of sight vector k i This will also include an error. In that case, the line of sight L detected by the line of sight detection unit 68 will be included. i As shown in Figure 5, its line of sight L i The corresponding object point Q i It passes a position far from the actual position of (Target 24). And the larger the error in the camera 20's own position, the greater the line of sight L i and the corresponding object point Q i distance di On the other hand, the closer the camera 20's position gets to its correct position, the greater the line of sight L becomes. i and the corresponding object point Q i distance d i It becomes smaller.

[0077] Therefore, all or some of the feature points Q on the target image i Multiple lines of sight L passing through each of them i And each feature point Q i The corresponding object point P i Distance d from (Target 24) i The self-position [R|t] of camera 20 when the sum of the squares of the values ​​is minimized can be considered as the true self-position.

[0078] In this embodiment, focusing on these characteristics, the gaze line L detected by the gaze detection process is considered. i and the corresponding object point Q i Based on the distance to the camera, the self-position estimation unit 62 corrects the self-position of the camera 20.

[0079] Here, the error function E(R x , R y , R z t) is defined as shown in equation (17) below. N is the number of gazes L detected in the gaze detection process, and Q is a feature point on the target image. i The value is less than or equal to the number of items.

number

[0080] The correction unit 70 calculates the error function E(R x , R y , R z The transformation matrix [R|t] that minimizes t) is determined by nonlinear optimization. The transformation matrix [R|t] obtained here becomes the correction value for the camera 20's own position. The correction unit 70 then updates the camera 20's own position stored in the memory unit 54 with the above correction value.

[0081] Furthermore, various nonlinear solution methods such as the steepest descent method, conjugate gradient method, and Adam can be used for the optimization calculations performed by the self-position estimation unit 62 (optimization calculation unit 66) and the correction unit 70. Since these methods are well known, their explanation is omitted here.

[0082] Next, the determination processing unit 72 determines whether or not to repeat the self-position estimation process of the camera 20 (step S20). Specifically, while the probe head 12 is measuring the three-dimensional coordinates of the workpiece, the determination processing unit 72 determines whether to continue the self-position estimation process of the probe head 12 (YES in step S20), and repeats the processes from step S10 to step S18. As a result, the self-position estimation process of the camera 20 is continuously and repeatedly performed while the probe head 12 is measuring the three-dimensional coordinates of the workpiece.

[0083] On the other hand, when the measurement of the three-dimensional coordinates of the workpiece by the probe head 12 is completed, the determination processing unit 72 determines that the self-position estimation process of the camera 20 has finished (NO in step S20), and terminates this flowchart.

[0084] 〔effect〕 Next, the effects of this embodiment will be described.

[0085] According to this embodiment, after a self-position estimation process is performed to estimate the camera 20's own position based on images of the target group 16 captured by the camera 20, a line of sight L is directed from the principal point of the camera 20 towards the target 24 through the transparent window material 30. i A gaze detection process is performed to detect each of the 24 targets. Then, each gaze L i A correction process is performed to adjust the camera 20's own position (position and orientation) so that the sum of the squares of the distances to the corresponding target 24 is minimized. This makes it possible to estimate the camera 20's own position with high accuracy without being affected by the refraction error caused by the transparent window material 30 in front of the target.

[0086] The camera 20 is not limited to a visible light camera; for example, it may be an infrared light camera. In this case, the transparent window material 30 only needs to be transparent to the wavelength range of infrared light. That is, the transparent window material 30 is not limited to being transparent to the wavelength range of visible light, but needs to be transparent to the wavelength range of light to which the camera 20 is sensitive.

[0087] Furthermore, in this embodiment, one example is shown where the target group 16 is composed of a dot pattern in which a plurality of targets 24 formed as dots or points are arranged in a two-dimensional manner. However, the embodiment is not limited to this, and for example, the target group 16 may be composed of a grid pattern or a checker pattern as disclosed in Patent Document 2. In addition, each target group 16 may be various two-dimensional patterns including AR markers (Augmented Reality Markers) or QR codes (Quick Response code, registered trademark).

[0088] Furthermore, the probe head 12 is not limited to one that the user grasps and uses to measure the workpiece. For example, the probe head 12 may be attached to the tip (end effector) of a multi-joint robot arm, and the probe head 12 may be moved by controlling the movement of the robot arm, thereby measuring the three-dimensional coordinates of the workpiece with the probe head 12.

[0089] Although embodiments of the present invention have been described above, the present invention is not limited to the above examples, and various improvements and modifications may be made without departing from the spirit of the present invention. [Explanation of Symbols]

[0090] 10...Self-position estimation system, 12...Probe head, 16...Target group, 18...Probe, 20...Camera, 24...Target, 30...Transparent window material, 50...Self-position estimation device, 52...Calculation processing unit, 54...Storage unit, 60...Image acquisition unit, 62...Self-position estimation unit, 64...Feature point detection unit, 66...Optimization calculation unit, 68...Look-line detection unit, 70...Correction unit, 72...Decision processing unit

Claims

1. A self-position estimation unit estimates the camera's own position based on an image taken by a camera of a group of targets in which light-transmitting parts are placed on the front of multiple targets, A line-of-sight detection unit detects a line of sight from the principal point of the camera toward the target via the light-transmitting unit, based on the camera's self-position estimated by the self-position estimation unit. A correction unit corrects the camera's own position based on the distance between the line of sight and the target, A self-position estimation device equipped with the following features.

2. The line-of-sight detection unit detects a first line of sight passing through the principal point of the camera and the feature point of the target image on the image, a second line of sight that refracts with respect to the optical axis of the first line of sight and passes through the light-transmitting part, and a third line of sight that refracts with respect to the optical axis of the second line of sight and passes through the space in which the target is located. The self-position estimation device according to claim 1.

3. The gaze detection unit detects multiple gazes for each target, The correction unit corrects the camera's position so that the sum of the squares of the distances obtained for each line of sight is minimized. A self-position estimation device according to claim 1 or 2.

4. A self-position estimation step in which the camera's own position is estimated based on an image taken by the camera of a group of targets in which light-transmitting parts are placed in front of multiple targets, A line-of-sight detection step, based on the camera's self-position estimated in the self-position estimation step, detects a line of sight from the camera's principal point toward the target via the light-transmitting part, A correction step to correct the camera's own position based on the distance between the line of sight and the target, A self-localization method comprising the following features.

Images