JP5027747B2 - POSITION MEASUREMENT METHOD, POSITION MEASUREMENT DEVICE, AND PROGRAM - Google Patents

Description

  The present invention relates to a position measurement technique for measuring a captured position and posture and a three-dimensional position of a subject based on a moving image, and more particularly to a position measurement technique for reducing errors.

  Conventionally, the theory of photogrammetry has been studied. In recent years, a technique for measuring the position and orientation of a camera and the three-dimensional position of a subject from a moving image using a photogrammetry theory has been disclosed (for example, Patent Document 1).

  In the invention described in Patent Document 1, when the position cannot be measured by GPS (Global Positioning System), the position of the camera is obtained by the backward intersection method, and the position of the subject is obtained by the forward intersection method. According to this aspect, position measurement is not interrupted.

  However, the method for obtaining the position of the camera and the position of the subject from the image tends to cause errors. The technique which solves this problem is disclosed by the nonpatent literature 2 or the patent document 3, for example.

  The invention described in Non-Patent Document 2 is based on an error (reprojection error) between coordinates obtained by projecting an estimated three-dimensional position onto an image and coordinates detected on the image, and a position defined by a GPS positioning value. The error is minimized simultaneously.

Further, the invention described in Patent Document 3 synchronizes the camera vector (CV) data indicating the position of the camera and the triaxial rotation position obtained from the video image and the position measurement data of the moving object with reference time, They are compared on the same time axis and compensated for each other.
JP 2007-171048 A IPSJ Journal Vol. 0, No. 13, Yuji Yokochi, p1-11 JP 2006-250917 A

  In view of such a background, the present invention reduces errors caused by changes in shooting scenes and changes in shooting devices in a position measurement technique that measures a shot position and posture and a three-dimensional position of a subject based on a moving image. It aims at providing the technology to do.

  The invention according to claim 1 is an initial value for calculating an initial value of an external orientation element composed of a photographing position and a photographing posture based on a known point in the initial image or a photographing position and / or photographing posture measured externally. An external orientation element calculation step for calculating an external orientation element based on a known point in the moving image following the initial image used in the initial value calculation step or a feature point that has already been given a three-dimensional coordinate; The external orientation element calculated in the external orientation element calculation step is at least one of a feature point trajectory direction, a feature point trajectory amount, a feature point distribution rate, a feature point overlap rate, and vertical parallax. One or more images based on an external orientation element evaluation step evaluated based on one and the external orientation elements modified by the external orientation element evaluation step A bundle adjustment step for simultaneously adjusting the three-dimensional coordinates of the partial orientation element and the feature point, and a tertiary of the feature point newly detected in the region where the density of the feature points is reduced based on the bundle-adjusted external orientation element A position measurement method comprising: a three-dimensional coordinate calculation step for calculating original coordinates; and a repetition step for repeating the processes from the external orientation element calculation step to the three-dimensional coordinate calculation step until a final image is obtained. It is.

  According to the first aspect of the present invention, based on at least one of the direction and amount of the trajectory of the feature points, the distribution rate of the feature points, the overlap rate, and the vertical parallax, the change in the shooting scene and the shooting device Can be judged. Changes in the shooting scene and changes in the shooting apparatus make it difficult to track feature points and degrade the accuracy of the external orientation elements calculated from the images. Therefore, when the change in the shooting scene and the fluctuation of the shooting apparatus are relatively large, it is evaluated that the reliability of the external orientation element calculated from the image is low, and the change in the shooting scene and the fluctuation of the shooting apparatus are relatively If it is small, it is evaluated that the external orientation element calculated from the image has high reliability.

  When it is evaluated that the reliability of the external orientation element calculated from the image is low, the external orientation element calculated from the image is not used, but the externally measured imaging position or orientation is used. By this aspect, errors due to changes in the shooting scene and fluctuations in the shooting apparatus are reduced.

  According to a second aspect of the present invention, in the first aspect, the direction of the trajectory of the feature point or the amount of the trajectory of the feature point corresponds to the feature point of the previous image in the processing target image. It is calculated based on the difference between the trajectory of the point and the trajectory of the newly detected feature point.

  According to the second aspect of the present invention, a change in the shooting scene and a change in the shooting apparatus are easily detected. The current image to be processed includes a corresponding point corresponding to the feature point of the previous image and a newly detected feature point. When there is a change in the shooting scene or a change in the shooting apparatus, the trajectory of the corresponding point and the trajectory of the newly detected feature point have different directions and different amounts.

  According to a third aspect of the present invention, in the first aspect of the present invention, the distribution rate of the feature points or the overlap rate of the feature points is the X coordinate and / or Y coordinate of the feature point with the image center as the origin. It is calculated by the total value.

  According to the third aspect of the present invention, a change in the shooting scene and a change in the shooting apparatus are easily detected.

  According to a fourth aspect of the present invention, in the first aspect of the invention, the external orientation element evaluation step includes the direction of the trajectory of the feature point, the amount of the trajectory of the feature point, the distribution rate of the feature point, The external orientation element is evaluated based on a value obtained by multiplying a plurality of values selected from the overlap rate and the vertical parallax.

  According to the fourth aspect of the present invention, a change in the shooting scene and a change in the shooting apparatus are detected with one parameter.

  According to a fifth aspect of the present invention, in the first aspect of the invention, the bundle adjustment step includes a weight of a photographing position and / or photographing posture given from outside, a weight of a three-dimensional coordinate of an external orientation element and a feature point. , And bundle adjustment is performed by weighting each data.

  According to the fifth aspect of the present invention, in addition to the external orientation elements and the three-dimensional coordinates of the feature points, the photographing position and / or photographing posture given from the outside are also bundle-adjusted. At this time, each data is weighted and bundle-adjusted, so that data with low accuracy is light and data with high accuracy is handled heavy. For this reason, errors in the three-dimensional coordinates of the external orientation elements and feature points are reduced.

  According to a sixth aspect of the present invention, in the fifth aspect of the present invention, the weight of the photographing position and / or photographing posture given from the outside is the photographing timing of the image and the photographing position and / or photographing posture given from the outside. It is calculated based on the difference from the acquisition timing of

  According to the sixth aspect of the present invention, even when the image capturing timing and the capturing position and / or capturing posture acquisition timing given from outside are not synchronized, the three-dimensional coordinates of the external orientation element and the feature point Error is reduced.

  According to a seventh aspect of the present invention, in the fifth aspect of the present invention, the weights of the three-dimensional coordinates of the external orientation element and the feature point include the direction of the feature point locus, the amount of the feature point locus, and the feature point It is calculated based on at least one of a distribution rate, an overlap rate of feature points, and vertical parallax.

  According to the seventh aspect of the present invention, errors in the three-dimensional coordinates of the external orientation elements and feature points are reduced.

  The invention according to claim 8 is the invention according to claim 5, wherein the bundle adjustment step is performed before calculating the three-dimensional coordinates of the newly detected feature point, or of the newly detected feature point. It is performed after calculating the three-dimensional coordinates.

  According to the invention described in claim 8, when bundle adjustment is performed before calculating the three-dimensional coordinates of the newly detected feature point, the calculation accuracy of the three-dimensional coordinates of the newly detected feature point is high. improves. When bundle adjustment is performed after the three-dimensional coordinates of the newly detected feature points are calculated, the three-dimensional coordinates of the newly detected feature points are calculated based on the external orientation elements after the evaluation. Therefore, the same effect is produced.

  According to a ninth aspect of the present invention, in the first aspect of the present invention, the image coordinates of the corresponding points calculated by tracking the feature points and the image coordinates obtained by projecting the three-dimensional coordinates of the corresponding points on the image are obtained. And an erroneous corresponding point removing step for removing an erroneous corresponding point based on the residual.

  According to the ninth aspect of the present invention, even if the miscorresponding point is included in the tracking result of the feature point, the miscorresponding point is removed, so that the error of the external orientation element and the three-dimensional coordinate of the feature point is reduced. To do.

  The invention according to claim 10 is the invention according to claim 1, wherein the three-dimensional coordinates of the corresponding points in the preceding and following images are projected onto the stereo image, and the coordinate value in the direction orthogonal to the baseline direction in the preceding and following stereo images. And an erroneous corresponding point removing step of removing an erroneous corresponding point based on the difference between the two.

  According to the tenth aspect of the present invention, even if the miscorresponding point is included in the tracking result of the feature point, the miscorresponding point is removed, so that the error of the external orientation element and the three-dimensional coordinate of the feature point is reduced To do.

  According to an eleventh aspect of the present invention, in the invention according to the ninth or tenth aspect, the erroneous corresponding point removing step is performed after the bundle adjusting step or before the external orientation element calculating step. Features.

  According to the eleventh aspect of the present invention, when the step of removing the miscorresponding points is performed after the bundle adjustment, the calculation accuracy of the external orientation elements after the next image is improved. On the other hand, when the removal of the miscorresponding point is performed before the external orientation element calculation, the external orientation element is not calculated based on the miscorresponding point, so that the error of the external orientation element is reduced.

  According to a twelfth aspect of the present invention, in the invention according to the ninth or tenth aspect of the present invention, the step of eliminating the miscorresponding point according to the ninth or tenth aspect is selected based on a change in the photographing situation. And

  According to the twelfth aspect of the present invention, it is possible to calculate the three-dimensional coordinates of the external orientation elements and the feature points without depending on the change in the photographing situation.

  The invention described in claim 13 is the invention described in claim 9 or 10, characterized in that a threshold value is estimated by LMedS estimation in the erroneous corresponding point removal step.

  According to the thirteenth aspect of the present invention, the LmedS method can robustly estimate an outlier even if the data includes many outliers as compared to the least square method. For this reason, the error of the three-dimensional coordinate of an external orientation element or a feature point can be reduced.

  The invention according to claim 14 is an initial value for calculating an initial value of an external orientation element composed of a photographing position and a photographing posture based on a known point in the initial image or a photographing position and / or photographing posture measured outside. An external orientation element calculation step for calculating an external orientation element based on a known point in the moving image following the initial image used in the initial value calculation step or a feature point that has already been given a three-dimensional coordinate; The external orientation element calculated in the external orientation element calculation step is at least one of a feature point trajectory direction, a feature point trajectory amount, a feature point distribution rate, a feature point overlap rate, and vertical parallax. One or more images based on the external orientation element evaluation step evaluated based on one and the external orientation element modified by the external orientation element evaluation step. A bundle adjustment step of simultaneously adjusting bundles of the three-dimensional coordinates of the external orientation elements and feature points, and a tertiary of feature points newly detected in an area where the density of feature points is reduced based on the bundle-adjusted external orientation elements A position measurement program for executing a three-dimensional coordinate calculation step for calculating original coordinates and a repetition step for repeating the processes from the external orientation element calculation step to the three-dimensional coordinate calculation step until a final image is obtained. .

  According to the fourteenth aspect of the present invention, based on at least one of the direction and amount of the trajectory of the feature points, the distribution rate of the feature points, the overlap rate, and the vertical parallax, the change in the shooting scene and the shooting device Can be judged. For this reason, it is possible to reduce errors due to changes in the shooting scene and fluctuations in the shooting apparatus.

  According to the fifteenth aspect of the present invention, a moving image acquisition unit that acquires a moving image captured by relatively moving the object and the imaging unit little by little, and an image acquired by the moving image acquisition unit are sequentially input. A feature point detection and tracking unit for detecting and tracking feature points from the image, a position measuring unit for measuring the shooting position of the moving image acquiring unit from the outside, and a shooting posture of the moving image acquiring unit from the outside A posture measurement unit, a moving image acquired by the moving image acquisition unit, a feature point detected and tracked by the feature point detection tracking unit, a shooting position measured by the position measurement unit, and a posture measurement unit A position measurement device comprising: a measurement processing posture that inputs a measured photographing posture and calculates a three-dimensional coordinate of the feature point and an external orientation element composed of the photographing position and photographing posture of the moving image. The calculation processing unit is an external orientation composed of a shooting position and a shooting posture based on a known point in an initial image or a shooting position measured by the position measurement unit and / or a shooting posture measured by the posture measurement unit. An external value element based on an initial value calculation unit that calculates an initial value of an element, and a known point in a moving image following the initial image used in the initial value calculation unit or a feature point that has already been given three-dimensional coordinates An external orientation element calculation unit that calculates the orientation of the external orientation element calculated by the external orientation element calculation unit, the direction of the trajectory of the feature point, the amount of the trajectory of the feature point, the distribution rate of the feature point, and the overlap rate of the feature point And one or a plurality of images based on the external orientation element evaluation unit evaluated based on at least one of the vertical parallax and the external orientation element modified by the external orientation element evaluation unit A bundle adjusting unit that simultaneously adjusts the three-dimensional coordinates of the external orientation element and the feature point in the image, and a feature point newly detected in a region where the density of feature points is reduced based on the bundle-adjusted external orientation element. A position measuring device comprising: a three-dimensional coordinate calculating unit that calculates three-dimensional coordinates, and repeating the processing from the external orientation element calculating unit to the three-dimensional coordinate calculating unit until a final image is obtained. .

  According to the fifteenth aspect of the present invention, based on at least one of the direction and amount of the trajectory of the feature points, the distribution rate of the feature points, the overlap rate, and the vertical parallax, the change in the shooting scene and the shooting device Can be judged. For this reason, it is possible to reduce errors due to changes in the shooting scene and fluctuations in the shooting apparatus.

  According to the present invention, in a position measurement technique that measures a captured position and orientation and a three-dimensional position of a subject based on a moving image, errors due to changes in the shooting scene and changes in the shooting apparatus are reduced.

1. First Embodiment Hereinafter, an example of a position measurement method, a position measurement apparatus, and a program will be described with reference to the drawings.

(Configuration of position measuring device)
FIG. 1 is an overall view of the position measuring device, and FIG. 2 is a functional block diagram of the position measuring device. The position measurement apparatus 1 includes a moving image acquisition unit 2, a feature point detection tracking unit 3, a position measurement unit 4, an attitude measurement unit 5, a reference clock unit 6, and a calculation processing unit 7.

  The moving image acquisition unit 2 includes a camera that acquires moving images, such as a video camera, a CCD camera (Charge Coupled Device Camera) for industrial measurement, and a CMOS camera (Complementary Metal Oxide Semiconductor Camera). The moving image acquisition unit 2 may be a camera that acquires a still image. In this case, a plurality of still images taken while the subject and the camera move relatively little by little are acquired.

  The feature point detection tracking unit 3 includes a feature point detection unit 8 and a feature point tracking unit 9. The feature point detection unit 8 detects a feature point from the moving image acquired by the moving image acquisition unit 2. The feature point tracking unit 9 tracks corresponding points corresponding to the feature points detected by the feature point detection unit 8 in other images. The feature point detection unit 8 and the feature point tracking unit 9 are implemented by firmware such as ASIC (Application Specific Integrated Circuit) and FPGA (Field Programmable Gate Array). Note that the feature point detection unit 8 and the feature point tracking unit 9 may be configured by software executed on a general-purpose computer.

  The position measurement unit 4 includes a GPS (Global Positioning System) receiver, an inertial measurement unit (IMU: Internal Measurement Unit), and the like. The position measuring unit 4 is used for measuring the position of the camera (projection center). The reliability of the evaluation of the external orientation element can be enhanced by appropriately combining GPS and IMU based on the shooting environment and the like. A positional offset between the installation position of the position measurement unit 4 and the installation position of the camera is set in advance.

  The attitude measurement unit 5 includes an attitude sensor such as a gyroscope, an IMU, an acceleration clock, an angular velocity meter, an angular accelerometer, and a combination thereof. Based on the shooting environment and the like, the reliability of the evaluation of the external orientation element can be improved by appropriately combining these. The posture measuring unit 5 is used for measuring the posture of the camera.

  The reference clock unit 6 includes a crystal oscillator, a rubidium oscillator, a radio clock, and the like. The reference clock unit 6 converts the time of the moving image photographing unit 2, the time of the position measuring unit 4, and the time of the posture measuring unit 5 into the same time axis.

  The calculation processing unit 7 is configured by a personal computer (PC: Personal Computer). The calculation processing unit 7 includes a control calculation unit 10 and a storage unit 11. The control operation unit 10 is a CPU (Central Processing Unit), and the storage unit 11 is configured by a semiconductor storage device such as a RAM (Random Access Memory) and a ROM (Read Only Memory), a magnetic storage device, and the like. The storage unit 11 stores an operating system and a position measurement program, and the control calculation unit 10 reads and executes the program.

  The position measurement program includes an initial value calculation unit 12, an external orientation element calculation unit 13, an image determination unit 20, an external orientation element evaluation unit 14, a bundle adjustment unit 15, a three-dimensional coordinate calculation unit 16, an error point removal unit 17, and a detailed measurement. And an external orientation element / three-dimensional coordinate output unit 19. The position measurement program can be provided by a recording medium such as a CDROM.

  Based on the three-dimensional coordinates of the reference point measured by the total station or GPS and the image coordinates of the reference point in the image in which the reference point is captured, the initial value calculation unit 12 is configured to detect the internal orientation element (focal length, The initial values of principal point position (shift of optical axis center, lens distortion coefficient) and external orientation elements (camera position and orientation) are calculated.

  The external orientation element calculation unit 13 calculates an external orientation element based on the backward association method or the relative orientation method. The backward intersection method is a method of observing the direction from an unknown point to three or more known points and determining the position of the unknown point as an intersection of these direction lines. Examples of the backward intersection method include single photo orientation and DLT method (Direct Liner Transformation Method).

  The image determination unit 20 determines a change in the shooting scene or a change in the shooting apparatus based on the direction of the trajectory of the feature points, the movement amount of the feature points, the distribution rate of the feature points, the overlap rate, the vertical parallax, and the like.

  The external orientation element evaluation unit 14 evaluates the external orientation element calculated from the image based on the determination result of the image determination unit 20. If it is determined that the change in the shooting scene is relatively large, the external orientation element is corrected with the position data acquired by the position measurement unit 4 and the posture data acquired by the posture measurement unit 5. If it is determined that the change in the shooting scene is relatively small, the external orientation element calculated from the image is selected as it is.

  The bundle adjustment unit 15 simultaneously adjusts the evaluated external orientation elements and the three-dimensional coordinates of the feature points between one or a plurality of images or all the images.

  The three-dimensional coordinate calculation unit 16 calculates the three-dimensional coordinates of the newly detected feature point based on the forward intersection method. The forward intersection method is a method of observing a direction from two or more known points toward an unknown point and determining the position of the unknown point as an intersection of these direction lines. The bundle adjustment is performed before calculating the three-dimensional coordinates of the newly detected feature point or after calculating the three-dimensional coordinates of the newly detected feature point.

  The miscorresponding point removal unit 17 removes a point having a relatively large error from the tracked corresponding points by using the vertical parallax calculated from the backward projection or the relative orientation. Backward projection is a method for removing error points from the residual between image coordinates calculated from three-dimensional coordinates of feature points and image coordinates of tracked corresponding points based on a backward intersection method. Also, the relative orientation is to obtain an external orientation element by using 6 or more corresponding points in the left and right images. Further, the vertical parallax is a difference in y-coordinates of corresponding points in two images shot in stereo, and means a difference in coordinate values in a direction orthogonal to the baseline direction of the stereo image. Note that a stereo image is an image pair in which the optical axes of cameras that photographed two images are parallel and the directions orthogonal to the baseline direction are parallel.

  The detailed measurement unit 18 performs three-dimensional modeling. Three-dimensional modeling means forming a three-dimensional object to be photographed. The detailed measurement unit 15 calculates a contour line from the three-dimensional coordinates of the feature points to form a wire frame model and calculates a surface to form a surface model. A two-dimensional image is texture-mapped on the surface of the formed surface model. Further, the detailed measurement unit 15 performs texture analysis based on the mapped texture, and calculates the three-dimensional coordinates of the feature portion of the texture. The three-dimensional coordinates of the texture feature are bundle-adjusted and corrected to an optimum value.

  The external orientation element / three-dimensional coordinate output unit 19 outputs the external orientation element and the three-dimensional coordinate data of the feature point or the texture feature portion.

(Operation of position measuring device)
Next, the overall operation of the position measuring device will be described. 3 and 4 are flowcharts showing the operation of the position measuring apparatus.

  First, a moving image is acquired by the moving image acquisition unit 2 (step S1). The moving image is composed of a plurality of continuous images captured by moving the moving image acquisition unit 2 and the object relatively little by little. A plurality of continuous images are sequentially input to the feature point detection tracking unit 3.

  The feature point detection unit 8 detects a feature point from the image input to the feature point detection tracking unit 3 (step S2). Filters such as Moravec, Laplacian, and Sobel are used for feature point detection. Next, the feature point tracking unit 9 tracks corresponding points corresponding to the feature points in the next image (step S3). Template matching is used for tracking feature points. Examples of template matching include a residual sequential detection method (SSDA), a cross-correlation coefficient method, and the like.

  In the image in which the feature points are tracked, the feature point detection unit 8 newly detects the feature points in the region where the density of the feature points is reduced. Step S2 for detecting feature points and step S3 for tracking feature points are repeated in successive images. Information about feature points is embedded in each image. Then, the image in which the information regarding the feature point is embedded is sequentially output to the calculation processing unit 7. Details of the processing in steps S2 and S3 are described in Japanese Patent Application Nos. 2007-147457 and 2007-183256.

  Next, the calculation processing unit 7 calculates initial values of the internal orientation element and external orientation element of the camera from the image where the reference point is captured (step S4). Prior to this work, the worker installs a reference point in the target space and photographs the reference point. The worker measures the three-dimensional coordinates of the reference point in the real space using a total station or GPS. In the initial value calculation in step S4, the operator calculates initial values of the internal orientation element and external orientation element of the camera by the backward intersection method based on the three-dimensional coordinates and the image coordinates of the reference point. Alternatively, the initial value of the external orientation element is obtained by using a total station or GPS, instead of directly giving the three-dimensional coordinate values of the feature points, giving the three-dimensional coordinate values of a plurality of shooting positions and shooting at the plurality of shooting positions. You may make it obtain | require from a stereo image.

  Alternatively, the operator captures a plurality of images while moving a reference plate on which a plurality of reference points are drawn without using a total station or GPS. The distance between the reference points drawn on the reference plate is known. In this case, in the initial value calculation in step S4, the image coordinates of the reference point are calculated from a plurality of images taken with the reference point, and the initial values of the camera position and orientation are obtained using relative orientation or the like. However, in this case, the local coordinate system is used.

  Next, for each input image, the external orientation element and the three-dimensional coordinates of the newly detected feature point are calculated (step S5). The backward intersection method or the relative orientation method is used for the calculation of the external orientation element, and the forward intersection method is used for the calculation of the three-dimensional coordinates of the newly detected feature points. This process is repeated for all images. Details of this processing will be described later.

  If more detailed measurement is performed (step S6), detailed measurement is performed (step S8). Detailed measurement includes wire frame model formation, surface model formation, texture mapping, texture analysis, and the like. In the texture analysis, the three-dimensional coordinates of the characteristic part of the texture are calculated. The three-dimensional coordinates of the texture feature are bundle-adjusted together with the three-dimensional coordinates of the external orientation elements and feature points calculated in step S5, and are corrected to optimum values. The calculated external orientation elements and three-dimensional coordinates are output as data (step S7).

  Hereinafter, the details of the three-dimensional coordinate / external orientation element calculation process in step S5 according to the present invention will be described. As shown in FIG. 3, the calculation process of the three-dimensional coordinate / external orientation element in step S5 includes the calculation of the external orientation element (step S10), the evaluation of the calculated external orientation element (step S11), the evaluated external orientation element and It consists of bundle adjustment of the three-dimensional coordinates of the known points (step S12) and calculation of the three-dimensional coordinates of the newly detected feature points (step S13).

  The bundle adjustment (step S12) is performed before calculating the three-dimensional coordinates of the newly detected feature points (FIG. 3) or after calculating the three-dimensional coordinates of the newly detected feature points (FIG. 4). You may go to In the case of FIG. 3, the calculation accuracy of the three-dimensional coordinates of the newly detected feature points is improved. In the case of FIG. 4, since the three-dimensional coordinates of the newly detected feature point are calculated based on the external orientation element after the evaluation, the same effect is obtained.

(Step S10) Calculation of External Orientation Element FIG. 5 is an explanatory diagram for explaining the backward intersection method. The backward intersection method is a method of observing directions from the unknown point O toward three or more known points P ₁ , P ₂ , and P ₃ and determining the position of the unknown point O as an intersection of these direction lines. First, based on the three-dimensional coordinates (or three-dimensional coordinates obtained in the previous frame) of the reference points P ₁ , P ₂ , and P ₃ set as initial values, the external orientation elements (X ₀ , Y ₀ , Z ₀ , ω, φ, κ) are calculated. Hereinafter, the single photo orientation and DLT method used as the backward intersection method will be described.

[Single photo orientation]
Single photo orientation uses the collinear condition that holds the reference point in a single photo, and uses the camera position O (X ₀ , Y ₀ , Z ₀ ) and camera posture (ω , Φ, κ). The collinear condition is a condition that the projection center, the photographic image, and the ground target points (Op ₁ P ₁ , Op ₂ P ₂ , Op ₃ P ₃ ) are on a straight line. The camera position O (X ₀ , Y ₀ , Z ₀ ) and the camera posture (ω, φ, κ) are external orientation elements.

  First, the camera coordinate system is x, y, z, the photographic coordinate system x, y, and the ground coordinate system is X, Y, Z. It is assumed that photographing is performed in a direction in which the camera is sequentially rotated counterclockwise by ω, φ, and κ counterclockwise with respect to the positive direction of each coordinate axis. Then, the four image coordinates (only three points are shown in FIG. 5) and the corresponding three-dimensional coordinates of the reference point are substituted into the quadratic projective transformation equation shown in Equation 1, and an observation equation is established to set the parameters b1 to b2. b8 is obtained.

Using the parameters b1 to b8 in Equation 1, an external orientation element is obtained from Equation 2 below.

[DLT method]
The DLT method approximates the relationship between the photographic coordinates and the three-dimensional coordinates of the target space with a cubic projective transformation formula. The basic formula of the DLT method is as shown in Equation 13 below. For details of the DLT method, refer to “Shunji Murai: Analytical Photogrammetry, p46-51, p149-155” and the like.

  If the denominator of Equation 3 is deleted, the linear form of Equation 4 can be derived.

  Further, when Expression 4 is transformed, the following Expression 5 is obtained.

Substituting the three-dimensional coordinates of six or more reference points into Equation 5 and solving using the least square method, eleven unknown variables L _{1 to} L ₁₁ that determine the relationship between the photo coordinates and the target point coordinates are obtained. You can get it. Note that L _{1 to} L ₁₁ include external orientation elements.

  Next, calculation of external orientation elements by the relative orientation method will be described. The relative orientation is a method in which a relative external orientation element is obtained even if there is no known point. If there is a known point, absolute coordinates can be obtained by performing absolute orientation.

[Mutual orientation]
FIG. 6 is an explanatory diagram for explaining relative orientation. In the relative orientation, an external orientation element is obtained from six or more corresponding points in the left and right images. In the relative orientation, a coplanar condition that two rays connecting the projection centers O ₁ and O ₂ and the reference point P must be in the same plane is used. The following formula 6 shows the coplanar conditional expression.

As shown in FIG. 6, the origin of the model coordinate system left nitrilase projection center O _1, to take a line connecting the right projection center O ₂ on the X axis. For the scale, the base length is the unit length. At this time, the parameters to be obtained are the rotation angle κ ₁ of the left camera, the rotation angle φ _{1 of} the Y axis, the rotation angle κ _{2 of the} right camera, the rotation angle φ ₂ of the Y axis, and the rotation angle φ _{2 of} the X axis. There are five rotation angles ω ₂ . In this case, since the rotation angle ω ₁ of the X axis of the left camera is 0, there is no need to consider it. Under such conditions, the coplanar conditional expression of Equation 6 becomes as shown in Equation 7, and each parameter can be obtained by solving this equation.

  Here, the following relational expression for coordinate transformation is established between the model coordinate system XYZ and the camera coordinate system xyz.

Using these equations, an unknown parameter (external orientation element) is obtained by the following procedure.
(1) The initial approximate values of unknown parameters (κ ₁ , φ ₁ , κ ₂ , φ ₂ , ω ₂ ) are normally 0.
(2) The coplanar conditional expression of Equation 7 is Taylor-expanded around the approximate value, and the value of the differential coefficient when linearized is obtained by Equation 8, and an observation equation is established.
(3) A least square method is applied to obtain a correction amount for the approximate value.
(4) The approximate value is corrected.
(5) Using the corrected approximate value, the operations (1) to (4) are repeated until convergence.

When the relative orientation converges, connection orientation is further performed. The connection orientation is a process of unifying the inclination and scale between models to make the same coordinate system. When this processing is performed, a connection range represented by the following formula 9 is calculated. As a result of the calculation, if ΔZ _j and ΔD _j are equal to or less than a predetermined value (for example, 0.0005 (1/2000)), it is determined that the connection orientation has been normally performed.

(Step S11) Evaluation of external orientation elements Next, external orientation elements are evaluated. The calculation of the external orientation element is based on the reference point and the three-dimensional coordinates of the feature points for which the three-dimensional coordinates have already been obtained, and the image coordinates obtained by tracking the feature points. Therefore, the accuracy of the external orientation element depends on the tracking accuracy of the feature points. Further, the tracking accuracy of the feature points depends on a significant change in the shooting scene and a change in the shooting apparatus. For this reason, an error occurs due to a change in the shooting scene or a change in the shooting apparatus, or calculation becomes impossible.

  Therefore, when the change of the shooting scene or the change of the shooting apparatus is large and the external orientation element calculated from the image is not reliable, the position data acquired from the position measuring unit 4 or the posture data acquired from the posture measuring unit 5 is used. To do. At this time, the external orientation element calculated from the image is not used.

  The change amount of the shooting scene and the change amount of the shooting apparatus are determined based on the direction and amount of the trajectory of the feature points, the distribution rate of the feature points, the overlap rate, and the vertical parallax. This determination is performed by the image determination unit 20. Hereinafter, a method for determining a change in the shooting scene will be described.

[Judgment by direction and amount of trajectory of feature points]
FIG. 7A is a diagram showing an image in which the direction of the trajectory of the feature point of the previous frame is different from the direction of the trajectory of the newly detected feature point, and FIG. 7B is a feature point of the previous frame. It is a figure which shows the image from which the amount of this locus | trajectory differs from the amount of the locus | trajectory of the newly detected feature point.

  For example, as shown in FIG. 7A, when the camera moves in the right direction, newly detected feature points increase in an area where the density of feature points decreases. At this time, if the direction of the trajectory of the newly detected feature point is greatly changed to 90 degrees, 180 degrees,... Judge that the device has changed. In such a case, the external orientation element calculated from the image is evaluated as having low reliability. Then, the external orientation element is corrected to position data acquired from the position measurement unit 4 or posture data acquired from the posture measurement unit 5.

  The change in the direction of the trajectory of the feature point is defined as an angle θ formed by the trajectory vector a of the feature point in the previous frame and the trajectory vector b of the newly detected feature point. This angle is obtained from the definition of the inner product of Formula 10.

  Further, for example, as shown in FIG. 7B, when the amount of the trajectory of the newly detected feature point is greatly changed to 2 times, 4 times,. It is determined that the shooting scene has changed. In such a case, the external orientation element calculated from the image is evaluated as having low reliability. Then, the external orientation element is corrected to position data acquired from the position measurement unit 4 or posture data acquired from the posture measurement unit 5.

  The change in the amount of the locus is a difference between the size of the locus vector a of the feature point of the previous frame and the size of the locus vector b of the newly detected feature point. This difference is obtained from the following equation (11). The threshold for the direction and amount of the trajectory of the feature point can be adjusted.

[Judgment based on feature point distribution rate and overlap rate]
When the change in the shooting scene or the change in the shooting apparatus is large, the feature points cannot be tracked, and the arrangement of the points in the image may be biased. FIG. 8A is a diagram showing an image with a feature point distribution rate of 0, FIG. 8B is a diagram showing an image with a feature point distribution rate negative, and FIG. It is a figure which shows the image with the positive distribution rate of a feature point.

  When the camera moves in the horizontal direction, the distribution rate of the feature points in the horizontal direction is obtained as the total value of the X coordinates of the feature points when the image center is the origin (0, 0). For example, in the case of FIG. 8A, since the feature points are uniformly distributed throughout the image, the total value of the X coordinates is zero. In the case of FIG. 8B, since the feature points are distributed on the left side of the image, the total value of the X coordinates is negative. In the case of FIG. 8C, since the feature points are distributed on the right side of the image, the total value of the X coordinates is positive.

  When the camera moves in the vertical direction, the distribution rate of the feature points in the vertical direction is obtained as the total value of the Y coordinates of the feature points when the image center is the origin (0, 0). Therefore, the distribution rate of the feature points in the vertical direction may be obtained from the total value of the Y coordinates of the feature points.

  In addition, when there are large changes in shooting scenes or changes in the shooting apparatus, feature points that overlap in the previous and subsequent frames may be biased. The overlapping feature points are points obtained by tracking feature points. FIG. 9A is a diagram illustrating an image in which feature points overlapping in the preceding and following frames are biased, and FIG. 9B is a diagram in which feature points overlapping in the preceding and following frames are biased. It is a figure which shows no image.

  As shown in FIG. 9A, when feature points overlapping in the preceding and following frames are biased, the external orientation element calculated from the image is evaluated as having low reliability. Then, the external orientation element is corrected to position data acquired from the position measurement unit 4 or posture data acquired from the posture measurement unit 5. Further, as shown in FIG. 9B, when the feature points overlapping in the preceding and following frames are not biased, the external orientation element calculated from the image is evaluated as having high reliability. Then, an external orientation element calculated from the image is used.

  A method for calculating the distribution rate of feature points is used for the degree of bias (overlap rate) of overlapping feature points. Note that the threshold for the distribution rate of feature points and the overlap rate can be adjusted.

[Judgment based on vertical parallax]
Longitudinal parallax refers to the difference between the y-coordinates of corresponding points in two stereo images. That is, a difference in coordinate values in a direction orthogonal to the baseline direction of the stereo image. A stereo image is an image pair in which the optical axes of the cameras that photographed two images are parallel and the directions orthogonal to the baseline direction are parallel. Therefore, since the y-coordinates of the corresponding points of the two images are equal, the vertical parallax of the stereo image is normally 0 pixels.

  However, when there is a change in the photographing apparatus, the vertical parallax is not 0 pixels. Therefore, when the vertical parallax is 1 pixel or more, the external orientation element calculated from the image is not used, and the position data acquired from the position measurement unit 4 and the posture data acquired from the posture measurement unit 5 are used.

  The vertical parallax is obtained by calculating the difference between the y coordinate of the feature point of the previous frame and the y coordinate of the feature point of the current frame. The threshold for vertical parallax is adjustable.

  Note that changes in shooting scenes and fluctuations in the shooting apparatus may include two, three, four, or all of the direction and amount of the feature point trajectory, the feature point distribution rate, the overlap rate, and the vertical parallax. You may judge based on the multiplied value.

(Step S12) Bundle adjustment After evaluating the external orientation elements, bundle adjustment is performed simultaneously on one or a plurality of images, or the 3D coordinates of the external orientation elements and feature points in all images. The bundle adjustment is based on a collinear condition that a light beam (bundle) connecting three points of the target space, a point on the image, and the projection center must be on the same straight line, and an observation equation for each light beam of each image. And simultaneously adjusting by the least square method. The collinear conditional expression is as shown in Equation 12 below.

The bundle adjustment uses a successive approximation method. Specifically, the three-dimensional coordinates of the reference points and feature points, and the true values of the external orientation elements are respectively converted into approximate values (X ′, Y ′, Z ′), (X ₀ ′, Y ₀ ′, Z ₀ ′). , Ω ′, φ ′, κ ′) plus a correction amount (X ′ + ΔX, Y ′ + ΔY, Z ′ + ΔZ), (X ₀ ′ + ΔX ₀ , Y ₀ ′ + ΔY ₀ , Z ₀ ′ + ΔZ ₀ , ω ′ + Δω, φ ′ + Δφ, κ ′ + Δκ), and the true value of the image coordinates corresponding to the reference point or feature point is obtained by adding an error to the measured value (x ′, y ′) (x ′ + dx, y) '+ Dy). Then, the approximate value to which the correction amount is added and the measurement value including the error are substituted into the collinear conditional expression (Equation 12) of one or a plurality of images, and the Taylor expansion is performed around the approximate value to linearize it. The correction amount is obtained by multiplication. The approximate value is corrected by the obtained correction amount, and the same operation is repeated to obtain a converged solution.

  Δx and Δy represent correction terms for internal orientation elements (focal length, principal point position, lens distortion coefficient). The following Expression 14 is a correction model of the internal orientation element when the lens has radial distortion. The internal orientation element can also be adjusted at the same time by Equation (14). This is so-called bundle adjustment with self-calibration.

(Step S13) Calculation of three-dimensional coordinates of feature points Based on the bundle-adjusted external orientation elements, the three-dimensional coordinates of feature points newly detected in an area where the density of feature points has decreased are calculated. Examples of the calculation method include a forward intersection method or DLT. As shown in FIG. 4, the calculation of the three-dimensional coordinates in step S13 may be processed prior to bundle adjustment (step S12).

FIG. 10 is an explanatory diagram for explaining the forward meeting method. The forward intersection method is a method of observing a direction from _two or more known points (O ₁ , O ₂ ) toward the unknown point P and determining the position of the unknown point P as an intersection of these direction lines.

In FIG. 10, the coordinate system of the target space is assumed to be O-XYZ. The coordinates (X ₀₁ , Y ₀₁ , Z ₀₁ ) of the camera projection center O ₁ of the previous frame image and the tilts (ω ₁ , φ ₁ , κ ₁ ) of the camera coordinate axis and the camera projection center O ₂ of the current frame image The coordinates (X ₀₂ , Y ₀₂ , Z ₀₂ ) and the tilt (posture) (ω ₂ , φ ₂ , κ ₂ ) of the camera coordinate axes are known. Also, the internal orientation elements (focal length, principal point position, lens distortion coefficient) are known. At this time, if the point p ₁ (x ₁ , y ₁ ) on the previous frame image and the corresponding point p ₂ (x ₂ , y ₂ ) on the current frame image are known, the unknown point P ( X, Y, Z) is determined as the intersection of the light beam O ₁ p ₁ and the light beam O ₂ p ₂ . However, since there is actually an error and the two rays do not intersect, the position of the intersection is obtained by the least square method.

  Specifically, two collinear conditional expressions (Equation 12) are established, and the known external orientation elements, internal orientation elements, and image coordinates of corresponding points are substituted into them. Further, the approximate value (X ′, Y ′, Z ′) of the unknown point P plus the correction amount (X ′ + ΔX, Y ′ + ΔY, Z ′ + ΔZ) is substituted into this collinear conditional expression. A Taylor expansion around the approximate value is linearized, and a correction amount is obtained by the least square method. The approximate value is corrected by the obtained correction amount, and the same operation is repeated to obtain a converged solution. By this operation, the three-dimensional coordinates P (X, Y, Z) of the feature points are obtained.

(Step S14) Final Frame Determination After the three-dimensional coordinates of the newly detected feature points are calculated, it is determined whether or not the currently processed image is the final image. If the currently processed image is not the final image, the process returns to step S10 to calculate the external orientation element of the next image. Thereafter, steps S10 to S14 are repeated until the final image. If the image currently being processed is the final image, the process ends.

(Advantages of the first embodiment)
According to the first embodiment, a change in the shooting scene or a change in the shooting device is performed based on at least one of the direction and amount of the trajectory of the feature point, the distribution rate of the feature point, the overlap rate, and the vertical parallax. Is judged. Changes in the shooting scene and changes in the shooting apparatus make it difficult to track feature points, and degrade the accuracy of external orientation elements calculated from images. Therefore, when the change in the shooting scene and the fluctuation of the shooting apparatus are relatively large, it is evaluated that the reliability of the external orientation element calculated from the image is low, and the change in the shooting scene and the fluctuation of the shooting apparatus are relatively If it is small, it is evaluated that the external orientation element calculated from the image has high reliability.

  When it is evaluated that the reliability of the external orientation element calculated from the image is low, the external data calculated from the image using the position data acquired from the position measurement unit 4 or the posture data acquired from the posture measurement unit 5 is used. The orientation element is not used. By this aspect, errors due to changes in the shooting scene and fluctuations in the shooting apparatus are reduced.

2. Second Embodiment Hereinafter, a modification of the first embodiment will be described. In the second embodiment, weighted bundle adjustment is performed.

  FIG. 11A is a conceptual diagram of bundle adjustment according to the first embodiment. As shown in FIG. 11A, in the first embodiment, the external orientation elements calculated from the camera are the direction and amount of the trajectory of the feature points, the distribution rate of the feature points, the overlap rate, and the vertical parallax. It is evaluated based on at least one of the following. As the external orientation elements to be bundle-adjusted, the best data is selected from the GPS position data, the orientation data of the orientation sensor, and the external orientation elements calculated from the camera, and the bundle is based on the selected external orientation elements. Adjustments are made. Therefore, bundle adjustment is performed based on the data indicated by the dotted frame in FIG.

  On the other hand, in the second embodiment, the external orientation element is not corrected to the best data, but the GPS location data, orientation sensor orientation data, and the external orientation element calculated from the camera are weighted to perform bundle adjustment. Is done. FIG. 11B is a conceptual diagram of weighted bundle adjustment according to the second embodiment. As shown in FIG. 11B, data with poor accuracy has a light weight, and data with high accuracy has a heavy weight. As a result, the three-dimensional coordinates of the external orientation elements and feature points are bundle-adjusted within the range of the solid line frame in FIG.

  The weight is calculated based on the accuracy of each data. Assuming that the accuracy of each data is σ, the weight w is calculated by Equation 15. Hereinafter, the accuracy of each data will be described.

[Accuracy of GPS position data and attitude sensor attitude data]
The accuracy of the GPS position data and attitude sensor attitude data depends on the error based on the difference between the image capturing timing and the GPS position data and attitude sensor acquisition timing, and the GPS position data and attitude sensor attitude. Based on the inherent accuracy of the data.

  In general, the GPS and attitude sensor times are not synchronized with the camera time. Therefore, in the present invention, the GPS position data acquisition time and the image shooting time are converted to the reference time output by the reference clock 6. The converted time takes into account the transmission delay of position data and attitude data and the transmission delay of image data.

  In addition, since the frame rate of the moving image and the acquisition rate of the GPS and the attitude sensor do not match, the image capturing timing is not synchronized with the acquisition timing of the GPS position data and the attitude sensor. For this reason, the accuracy of the GPS and the attitude sensor considers an error based on the difference between the image capturing timing and the acquisition timing of the position data of the GPS and the attitude data of the attitude sensor.

If the difference between the image capturing timing and the GPS position data or the posture sensor acquisition timing is Δt, the GPS position is determined when the camera moves at a speed v (angular velocities v _x , v _y , v _z ). The error Et of the data is expressed by Equation 16, and the errors Ext, Eyt, Ezt of the posture data of the posture sensor are expressed by Equation 17.

  Since the image capture time and the acquisition time of the GPS position data and attitude sensor attitude data can be represented on the same time axis by the reference clock 6, Δt is the time when the GPS position data and attitude sensor attitude data are acquired. And the time difference from the image taken at the time closest to this time.

Further, the speed v is obtained from the moving distance of the feature points in the moving image and the frame rate of the moving image as shown in Equation 18. Further, as the angular velocities v _x , v _y , and v _z , values acquired from an attitude sensor or an IMU are used.

  Next, the inherent accuracy of the GPS and attitude sensor will be described. The positioning accuracy of GPS varies depending on values such as satellite position (DOP: Division of Precision), multipath, link quality (LQ: Link Quality), and convergence evaluation of positioning calculation. For example, in the case of an RTK-GPS (Real Time Kinematic GPS) system, the positioning mode changes in real time in consideration of the position of the GPS satellite, the influence of multipath due to the surrounding environment, correction information from the reference station, and the like. Table 1 below shows an outline of positioning accuracy in each positioning mode.

_Assuming that the GPS positioning accuracy is GPS _rtke , the total Et _ALL of the accuracy of the GPS position data is _expressed by _Equation 19.

In addition, since the attitude sensor theoretically takes acceleration integration, double integration, etc., errors accumulate according to elapsed time. Therefore, if the measurement time from when the initial value is set or when the hardware is reset is t, and the accumulated error approximates the exponential function 1.05 ^t −1, the accuracy of the attitude sensor is several 20 In consideration of the accuracy IMUe inherent to the posture sensor, the total accuracy Ext _ALL , Eyt _ALL , and Ezt _ALL of the posture sensor is expressed by Equation 21.

[Accuracy of 3D coordinates of external orientation elements and feature points]
The accuracy of the three-dimensional coordinates of the external orientation element and the feature point is at least one of the direction and amount of the trajectory of the feature point, the distribution rate of the feature point, the overlap rate, and the vertical parallax in the first embodiment. Is required. That is, the change in the direction of the trajectory of the feature point is the angle θ formed by the two vectors a and b (Equation 10), and the change in the amount of the trajectory of the feature point is the difference between the two vectors a and b | Since a−b | (Equation 11), the accuracy σ is θ or | a−b |.

  Further, since the distribution rate and overlap rate of the feature points are the total values of the X coordinates and / or Y coordinates of the feature points with the image center as the origin, the accuracy σ is the absolute value of the distribution rate and the overlap rate. Become. Further, since the vertical parallax is a difference between the y coordinates of the feature points in the stereo image, the accuracy σ is an absolute value of the difference between the y coordinates of the feature points.

  Note that the accuracy σ of the three-dimensional coordinates of the external orientation element and the feature point is two, three, four among the direction and amount of the trajectory of the feature point, the distribution rate of the feature point, the overlap rate, and the vertical parallax. Or a value obtained by multiplying all of them.

[Weighted bundle adjustment]
Each correction amount in the weighted bundle adjustment can be obtained as a value that minimizes the following function G. For example, the weight of the three-dimensional coordinate w ₂ is by the camera, w ₃ and w ₄ is the weight, the exterior orientation parameters.

Further, when position data by GPS or IMU and attitude data of the attitude sensor are added, the following Expression 23 is obtained, which is minimized. For example, w ₅ is the weight of GPS position data, w ₆ is the weight of IMU position data, and w ₇ is the weight of the attitude sensor.

(Advantage of the second embodiment)
According to the second embodiment, in addition to the three-dimensional coordinates of the external orientation elements and feature points, the position data of the position measurement unit 4 and the posture data of the posture measurement unit 5 are simultaneously bundle-adjusted. At this time, each data is weighted and bundle-adjusted, so that data with low accuracy is light and data with high accuracy is handled heavy. For this reason, errors in the three-dimensional coordinates of the external orientation elements and feature points are reduced.

  The weight of the position data of the position measurement unit 4 and the posture data of the posture measurement unit 5 is based on the difference between the image capturing timing and the position data of the position measurement unit 4 and the acquisition timing of the posture data of the posture measurement unit 5. Calculated. For this reason, even when the image capturing timing is not synchronized with the position data acquisition position of the position measurement unit 4 or the posture measurement unit 5, the error in the three-dimensional coordinates of the external orientation elements and feature points is reduced. .

  Furthermore, the weights of the three-dimensional coordinates of the external orientation elements and feature points are calculated based on at least one of the direction and amount of the feature point trajectory, the feature point distribution rate, the overlap rate, and the vertical parallax. . The accuracy in the three-dimensional coordinates of the external orientation elements and feature points calculated from the image tends to depend on changes in the shooting scene and changes in the shooting apparatus. For this reason, the direction and amount of the trajectory of feature points, which are used to determine changes in shooting scenes and fluctuations in the shooting device, the distribution rate of feature points, the overlap rate, and the vertical parallax are used as the three-dimensional coordinates of external orientation elements and feature points. By using this weight, the errors in the three-dimensional coordinates of the external orientation elements and feature points are reduced.

3. Third Embodiment Hereinafter, modified examples of the first to third embodiments will be described. The fourth embodiment removes a point having a relatively large error (miscorresponding point) from the corresponding points that have been tracked (tracked), and improves the calculation accuracy of the three-dimensional coordinates of the external orientation elements and feature points. It is.

  FIG. 12 to FIG. 14 are flowcharts incorporating removal of erroneous corresponding points. The erroneous corresponding points are removed after the bundle adjustment (FIGS. 12 and 13) or before the external orientation element calculation (FIG. 14). In the case of FIG. 12, the accuracy of calculating the external orientation elements in the next frame and thereafter is improved. Further, in the case of FIG. 13, since the three-dimensional coordinates of the removed miscorresponding points are not calculated, the calculation process becomes efficient. In the case of FIG. 14, the calculation accuracy of the external orientation elements in the current frame is improved.

  For the method of removing the erroneous corresponding point, the vertical parallax calculated by backward projection or relative orientation is used. These removal methods are selectively used depending on the shooting situation or both. For example, erroneous correspondence point removal by backward projection using single photograph orientation is used when there is a large change in the shooting scene, and erroneous correspondence point removal by mutual orientation is used when there is little change in the photography scene.

  Further, robust estimation may be used for estimation of the error determination threshold. For example, LMedS estimation (Least Median of Squares estimation), RANSAC estimation (RANdom Sample Consensus estimation), and M estimation are used. In particular, the LMedS estimation is excellent in robustness and can automatically remove a false corresponding point even if the error range is unknown.

  Note that the threshold for error determination may be an adjustable fixed threshold without depending on robust estimation. According to this aspect, since the calculation process is simple, the processing speed is improved.

  Hereinafter, a description will be given according to the flowchart of FIG. FIG. 15A is a flowchart for removing erroneous corresponding points by backward projection, and FIG. 15B is a flowchart for removing erroneous corresponding points using vertical parallax.

[Removal of mis-corresponding points by backward projection]
First, the tracked corresponding points are sampled randomly (step S20). Next, for each sampled point, based on the external orientation element calculated by the backward intersection method (single photo orientation or DLT) and the 3D coordinates of the tracked corresponding point, the image coordinates (x ′, y ′) is calculated. For example, in the case of single photograph orientation, the external orientation elements (X ₀ , Y ₀ , Z ₀ , ω, φ, κ) and the corresponding three-dimensional coordinates (X, Y, Z) are added to the collinear conditional expression of Equation 6. By substituting, the image coordinates (x ′, y ′) are calculated backward (step S21).

  Then, for each sampled point, the square of the residual between the image coordinates (x ′, y ′) obtained by the backward intersection method and the image coordinates (x, y) of the tracked corresponding point is obtained, and the square The median of the residual is calculated (step S22). Thereafter, Step S20 to Step S22 are repeated to obtain the minimum median value (LMedS) (Step S23). The following formula 24 shows the LMedS standard.

  Note that the number of times required to repeat Step S20 to Step S22 can be determined by considering the probability that no exceptional value is included at least once in q random samplings. For example, if the ratio of exceptional values in all data is c, this probability P is as shown in the following equation (25). For example, if c = 0.3 and n = 3, q = 11 times in order to achieve P = 0.01.

  A point with a large error is removed using the LMedS value obtained in step S23 as a threshold (step S24).

[Removal of miscorresponding points using vertical parallax]
First, corresponding points in the preceding and following images are randomly sampled (step S30). Next, as shown in FIG. 6, the three-dimensional coordinates of the point P in the real space of the sampled corresponding points (P ₁ , P ₂ ) are expressed by dotted lines according to the co-planar conditional expression (Equation 6) Re-projected on the stereo image shown (step S31).

  The vertical parallax is obtained as the difference between the y coordinates of the two reprojected corresponding points. The vertical parallax is a difference in y-coordinates of corresponding points in two images taken in stereo, and means a difference in coordinate values in a direction orthogonal to the baseline direction of the stereo image. Note that a stereo image is an image pair in which the optical axes of cameras that photographed two images are parallel and the directions orthogonal to the baseline direction are parallel. Next, the square of the vertical parallax is obtained, and the median value is calculated (step S32). Then, Step S30 to Step S32 are repeated to obtain the minimum median value (LMedS) (Step S33). The LMedS standard is given by Equation 22.

  A point with a large error is removed using the LMedS value obtained in step S33 as a threshold (step S34).

(Advantage of the third embodiment)
The tracking result of feature points includes many miscorresponding points. Therefore, the external orientation elements and the three-dimensional coordinates of the feature points calculated based on the miscorresponding points are inferior in accuracy. However, by removing the miscorresponding points, the calculation accuracy of the three-dimensional coordinates of the external orientation elements and feature points is improved, and the error is reduced.

  In addition, based on the change in the shooting situation, a method of eliminating the miscorresponding point (residual calculated by backward projection or vertical parallax calculated by relative orientation) is selected, so that it does not depend on the change of the shooting situation. The three-dimensional coordinates of external orientation elements and feature points can be calculated.

  Further, the LmedS method can robustly estimate an outlier even if the data includes many outliers as compared to the least square method. For this reason, the error of the three-dimensional coordinate of an external orientation element or a feature point can be reduced.

  The present invention can be used for a position measuring method, a position measuring apparatus, and a program thereof for measuring a photographed position and posture and a three-dimensional position of a subject based on a moving image.

It is a general view of a position measuring device. It is a functional block diagram of a position measuring device. It is a flowchart which shows operation | movement of a position measuring apparatus. It is a flowchart which shows operation | movement of a position measuring apparatus. It is explanatory drawing explaining a back intersection method. It is explanatory drawing explaining a relative orientation. The figure (A) which shows the image from which the direction of the trajectory of the feature point of the previous frame differs from the direction of the trajectory of the newly detected feature point, the amount of the trajectory of the feature point of the previous frame, and the newly detected feature It is a figure (B) which shows the image from which the quantity of the locus | trajectory of a point differs. A diagram (A) showing an image with a feature point distribution rate of 0, a diagram (B) showing an image with a negative feature point distribution rate, and a diagram (C) showing an image with a positive feature point distribution rate. is there. FIG. 5A is a diagram showing an image in which feature points overlapping in preceding and following frames are biased, and FIG. 6B is a diagram showing an image in which feature points overlapping in preceding and following frames are not biased. It is a conceptual diagram of the forward dating method. It is a conceptual diagram (A) of bundle adjustment concerning a 1st embodiment, and a conceptual diagram (B) of weighted bundle adjustment concerning a 2nd embodiment. It is the flowchart which took in removal of a miscorresponding point. It is the flowchart which took in removal of a miscorresponding point. It is the flowchart which took in removal of a miscorresponding point. The flowchart (A) which removes the miscorresponding point by backward projection, and the flowchart (B) which removes the miscorresponding point by vertical parallax.

Explanation of symbols

  DESCRIPTION OF SYMBOLS 1 ... Position measuring device, 2 ... Moving image acquisition part, 3 ... Feature point detection tracking part, 4 ... Position measurement part, 5 ... Attitude measurement part, 6 ... Reference | standard clock, 7 ... Calculation processing part, 8 ... Feature point detection part , 9: Feature point tracking unit, 10: Control operation unit, 11: Storage unit, 12: Initial value calculation unit, 13: External orientation element calculation unit, 14: External orientation element evaluation unit, 15 ... Bundle adjustment unit, 16 ... A three-dimensional coordinate calculation unit, 17 ... an erroneous corresponding point removal unit, 18 ... a detailed measurement unit, 19 ... an external orientation element / three-dimensional coordinate output unit, 20 ... an image determination unit.

Claims (15)

An initial value calculating step for calculating an initial value of an external orientation element composed of a shooting position and a shooting posture based on a known point in the initial image or a shooting position and / or shooting posture measured externally;
An external orientation element calculating step for calculating an external orientation element based on a known point in the moving image following the initial image used in the initial value calculating step or a feature point that has already been given three-dimensional coordinates;
The external orientation element calculated by the external orientation element calculation step is selected from at least one of a feature point trajectory direction, a feature point trajectory amount, a feature point distribution rate, a feature point overlap rate, and a vertical parallax. An external orientation element evaluation step to evaluate based on
A bundle adjustment step of simultaneously adjusting bundle adjustment of the three-dimensional coordinates of the external orientation elements and feature points in one or a plurality of images based on the external orientation elements modified by the external orientation element evaluation step;
Based on the bundle-adjusted external orientation elements, a three-dimensional coordinate calculation step for calculating the three-dimensional coordinates of the feature points newly detected in the region where the density of the feature points is reduced;
A position measuring method comprising: repeating the process from the external orientation element calculating step to the three-dimensional coordinate calculating step until a final image is obtained.   The direction of the feature point trajectory or the amount of feature point trajectory is based on the difference between the trajectory of the corresponding point of the current image corresponding to the feature point of the previous image and the trajectory of the feature point newly detected in the current image. The position measuring method according to claim 1, wherein the position measuring method is calculated by:   2. The position according to claim 1, wherein the distribution rate of feature points or the overlap rate of feature points is calculated by a total value of X coordinates and / or Y coordinates of feature points with an image center as an origin. Measuring method.   The external orientation element evaluation step multiplies a plurality of values selected from the direction of the trajectory of the feature point, the amount of the trajectory of the feature point, the distribution rate of the feature points, the overlap rate of the feature points, and the vertical parallax. The position measurement method according to claim 1, wherein the external orientation element is evaluated based on the combined value.   The bundle adjustment step calculates the weight of the photographing position and / or photographing posture given from the outside, the weight of the three-dimensional coordinates of the external orientation element and the feature point, and performs bundle adjustment by weighting each data. The position measuring method according to claim 1.   The weight of the photographing position and / or photographing posture given from the outside is calculated based on a difference between a photographing timing of the image and an acquisition timing of the photographing position and / or photographing posture given from the outside. Item 6. The position measurement method according to Item 5.   The weight of the three-dimensional coordinates of the external orientation element and the feature point is at least one of the direction of the trajectory of the feature point, the amount of the trajectory of the feature point, the distribution rate of the feature points, the overlap rate of the feature points, and the vertical parallax. The position measurement method according to claim 5, wherein the position measurement method is calculated based on the two.   The bundle adjustment step is performed before calculating the three-dimensional coordinates of the newly detected feature point or after calculating the three-dimensional coordinates of the newly detected feature point. The position measuring method described.   An erroneous corresponding point removing step for removing erroneous corresponding points based on a residual between the image coordinates of the corresponding points calculated by tracking the feature points and the image coordinates obtained by projecting the three-dimensional coordinates of the corresponding points on the image; The position measuring method according to claim 1, further comprising:   An erroneous corresponding point removal step of projecting the three-dimensional coordinates of corresponding points in the preceding and following images onto a stereo image and removing the erroneous corresponding points based on a difference in coordinate values in a direction orthogonal to the baseline direction in the preceding and following stereo images; The position measuring method according to claim 1, further comprising:   The position measuring method according to claim 9 or 10, wherein the incorrect corresponding point removing step is performed after the bundle adjusting step or before the external orientation element calculating step.   11. The position measuring method according to claim 9 or 10, wherein the step of removing an erroneous corresponding point according to claim 9 or 10 is selected based on a change in photographing condition.   The position measuring method according to claim 9 or 10, wherein in the erroneous corresponding point removing step, a threshold is estimated by LMedS estimation. An initial value calculating step for calculating an initial value of an external orientation element composed of a shooting position and a shooting posture based on a known point in the initial image or a shooting position and / or shooting posture measured externally;
An external orientation element calculating step for calculating an external orientation element based on a known point in the moving image following the initial image used in the initial value calculating step or a feature point that has already been given three-dimensional coordinates;
The external orientation element calculated by the external orientation element calculation step is selected from at least one of a feature point trajectory direction, a feature point trajectory amount, a feature point distribution rate, a feature point overlap rate, and a vertical parallax. An external orientation element evaluation step to evaluate based on
A bundle adjustment step of simultaneously adjusting bundle adjustment of the three-dimensional coordinates of the external orientation elements and feature points in one or a plurality of images based on the external orientation elements modified by the external orientation element evaluation step;
Based on the bundle-adjusted external orientation elements, a three-dimensional coordinate calculation step for calculating the three-dimensional coordinates of the feature points newly detected in the region where the density of the feature points is reduced;
A position measurement program for executing a repetition step of repeating the processes from the external orientation element calculation step to the three-dimensional coordinate calculation step until a final image is obtained. A moving image acquisition unit that acquires a moving image captured by relatively moving the object and the imaging unit little by little;
A feature point detection tracking unit that sequentially inputs images acquired by the moving image acquisition unit and detects and tracks feature points from the image;
A position measurement unit for measuring the shooting position of the moving image acquisition unit from the outside;
A posture measuring unit for measuring the photographing posture of the moving image acquiring unit from the outside;
The moving image acquired by the moving image acquisition unit, the feature point detected and tracked by the feature point detection tracking unit, the shooting position measured by the position measurement unit, and the shooting posture measured by the posture measurement unit. A position measurement device comprising: an external orientation element including a shooting position and a shooting posture of the moving image and a calculation processing unit that calculates a three-dimensional coordinate of the feature point;
The calculation processing unit is an external orientation element including a shooting position and a shooting posture based on a known point in an initial image or a shooting position measured by the position measurement unit and / or a shooting posture measured by the posture measurement unit. An initial value calculation unit for calculating an initial value of
An external orientation element calculation unit that calculates an external orientation element based on a known point in the moving image following the initial image used in the initial value calculation unit or a feature point that has already been given three-dimensional coordinates;
The external orientation element calculated by the external orientation element calculation unit is at least one of the direction of the trajectory of the feature point, the amount of the trajectory of the feature point, the distribution rate of the feature points, the overlap rate of the feature points, and the vertical parallax. An external orientation element evaluation unit that evaluates based on
Based on the external orientation element modified by the external orientation element evaluation unit, a bundle adjustment unit that simultaneously bundle adjusts the external orientation element and the three-dimensional coordinates of the feature points in one or a plurality of images,
Based on the bundle-adjusted external orientation element, a three-dimensional coordinate calculation unit that calculates the three-dimensional coordinates of the feature points newly detected in the region where the density of the feature points is reduced, and
A position measuring apparatus that repeats the processing from the external orientation element calculation unit to the three-dimensional coordinate calculation unit until a final image is obtained.

Images