JP2011085971A - Apparatus, method, and program for processing image, recording medium, and image processing system - Google Patents

Abstract

A three-dimensional coordinate of a predetermined object can be estimated easily and appropriately.
A storage unit that stores a plurality of images, position information and shooting direction information of a camera at the time of shooting the images, and stores three-dimensional coordinates of one or more target points of the target; The actual position of the feature point corresponding to the target point for which the three-dimensional coordinates are stored in the storage unit 15 in a certain image, the three-dimensional coordinates of the target point, the position information of the camera at the time of shooting a certain image, and the shooting Based on the direction information, the focal length of the camera at the time of shooting a certain image is estimated, and based on the estimated focal length, the three-dimensional coordinates included in the certain image and other images are unknown. And an image processing unit 13 for estimating the three-dimensional coordinates of the target point.
[Selection] Figure 1

Description

  The present invention relates to an image processing apparatus that estimates three-dimensional coordinates for a plurality of target points existing in a target such as a surrounding environment.

  2. Description of the Related Art Conventionally, there is known a technique for obtaining a three-dimensional coordinate for an object included in an image based on a plurality of images and forming a three-dimensional model for the object.

  For example, in Patent Document 1, a camera orientation is fixed, a camera is moved on a straight path, and images are sequentially taken at regular intervals, thereby obtaining a series of images (image sequence) about an object. ) Is obtained to simplify the process of creating a three-dimensional model for an object.

  Patent Document 2 discloses a technique for restoring a 3D (three-dimensional) model of a scene from a set of images having unknown external values.

  Non-Patent Document 1 discloses a method for restoring a 3D model of an object using a handheld camera. In the algorithm shown in Non-Patent Document 1, since the camera position and orientation of each frame are not grasped, the relationship between the two fields of view is solved by using epipolar constraints, and the 3D model is restored. ing.

US Patent Application Publication No. 2007/0242872 US Patent Application Publication No. 2007/0253618

Marc Pollefeys, “Visual modeling with a hand-held camera”, International Journal of Computer Vision, 2004, 59 (3), P207-232.

  However, according to the technique of Patent Document 1, since the orientation of the camera is fixed and the target image is taken only at positions at regular intervals on a straight line, there is a problem that the complicated shape of the target cannot be restored properly. is there. Further, in Patent Document 2, there is a problem that complicated processing such as skeletonization is performed and the 3D model cannot be easily restored. Further, in Non-Patent Document 1, there is a problem that the processing becomes complicated because it is necessary to solve the relationship between the fields of view of two images using epipolar constraints.

  This invention is made | formed in view of the said subject, The objective is to provide the technique which can estimate the three-dimensional coordinate of a predetermined object easily and appropriately.

  In order to achieve the above object, an image processing apparatus according to a first aspect of the present invention is based on a plurality of images including at least a part of a target based on three-dimensional coordinates of a plurality of target points existing in a predetermined target. An image processing apparatus for estimation, an image storage means for storing a plurality of images, camera position information and shooting direction information at the time of shooting the images, and three-dimensional about one or more target points of the target A coordinate storage means for storing coordinates, a real position in a certain image of a feature point corresponding to a target point for which the three-dimensional coordinates are stored in the coordinate storage means, a three-dimensional coordinate of the target point, A focal length estimating means for estimating a focal length of the camera at the time of shooting a certain image based on the position information and shooting direction information of the camera at the time of shooting, and a certain image based on the estimated focal length Tertiary included in other images It coordinates and a coordinate estimating means for estimating the three-dimensional coordinates of the target point is unknown. According to such an image processing apparatus, it is possible to easily and appropriately estimate the focal length in a certain image, and based on the certain image and another image, the three-dimensional coordinates of the target points included in these images are obtained. Can be estimated.

  In the image processing apparatus, the focal length estimation means includes an estimated position on a certain image of a feature point corresponding to a target point estimated based on position information, shooting direction information, and three-dimensional coordinates, and The focal length may be estimated based on the actual position on the image. According to such an image forming apparatus, an appropriate focal length can be estimated based on the estimated position and an actual position on a certain image.

  Further, in the image processing apparatus, the focal length estimation unit includes a function relating to a deviation between the estimated position of each feature point corresponding to the plurality of target points and the actual position of each feature point corresponding to the plurality of target points. The focal length may be estimated so that the sum becomes the smallest. According to such an image processing apparatus, since the focal length is estimated so that the sum of the functions related to the deviation between the estimated position of the feature point corresponding to a plurality of target points and the actual position of the target point becomes small, more appropriate focus The distance can be estimated.

  In the image processing apparatus, the focal length estimation means may estimate a focal length that minimizes a sum of functions related to deviation using a conjugate gradient method. According to such an image processing apparatus, the focal length can be estimated easily and appropriately.

  In the image processing apparatus, the camera that captures an image is attached to a device that can change the position and shooting direction of the camera, and the camera is based on state-identifiable data that can identify the state of the device. Camera information specifying means for specifying the position information and the shooting direction information, and a storage control means for storing the image taken by the camera and the position information and shooting direction information of the camera at the time of shooting in the image storage means. You may do it. According to such an image processing apparatus, it is possible to easily and appropriately acquire camera position information and shooting direction information. For this reason, the precision of the three-dimensional coordinates of the target point to be estimated can be improved.

  In the image processing apparatus, after estimating the three-dimensional coordinates of a plurality of target points using a plurality of images to be processed, the three-dimensional coordinates of the plurality of target points included in each image are used. You may make it further have a camera information optimization means which optimizes the positional information on a camera at the time of imaging | photography, and imaging | photography direction information. According to such an image processing apparatus, it is possible to optimize the camera position information and shooting direction information at the time of shooting each image, and based on the optimized start point position coordinates and shooting direction information, a plurality of target points 3 Dimensional coordinates can be estimated more appropriately.

  In the image processing apparatus, after estimating the three-dimensional coordinates of the target point using a plurality of images to be processed, the camera position information and the shooting direction information in the plurality of images including the same target point are used. Based on this, a three-dimensional coordinate optimization unit that optimizes the three-dimensional coordinates of the target point may be further included. According to such an image processing apparatus, it is possible to appropriately optimize the three-dimensional coordinates of the target point.

  The image processing apparatus may further include model forming means for forming a three-dimensional model of the object based on the three-dimensional coordinates of a plurality of target points. According to such an image processing apparatus, a three-dimensional model of a target including a plurality of target points can be appropriately formed.

  In order to achieve the above object, an image processing method according to a second aspect of the present invention is based on a plurality of images including at least a part of a target based on three-dimensional coordinates of a plurality of target points existing in a predetermined target. An image processing method by the image processing apparatus to be estimated, wherein the image storage means of the image processing apparatus stores a plurality of images, camera position information and shooting direction information at the time of shooting the images. A coordinate storage step in which the coordinate storage means of the image processing apparatus stores three-dimensional coordinates for one or more target points, and a focal length estimation means of the image processing apparatus stores the three-dimensional coordinates in the coordinate storage means. Based on the actual position of the feature point corresponding to the target point in the image, the three-dimensional coordinates of the target point, and the position information and shooting direction information of the camera at the time of shooting the image When shooting images The focal length estimation step for estimating the focal length of the camera and the coordinate estimation means of the image processing device include the three-dimensional coordinates included in one image and another image based on the estimated focal length. And a coordinate estimation step for estimating a three-dimensional coordinate for the target point. According to such an image processing method, it is possible to easily and appropriately estimate the focal length in a certain image, and based on the certain image and another image, the three-dimensional coordinates of the target points included in these images are obtained. Can be estimated.

  To achieve the above object, an image processing program according to a third aspect of the present invention provides a computer with a plurality of three-dimensional coordinates of a plurality of target points existing in a predetermined target, including at least a part of the target. An image processing program for presuming based on the image of the image, a computer storing a plurality of images, camera position information and shooting direction information at the time of shooting the image, and a target 1 Coordinate storage means for storing the three-dimensional coordinates of the target points described above, an actual position in a certain image of a feature point corresponding to the target point for which the three-dimensional coordinates are stored in the coordinate storage means, and 3 of the target points A focal length estimating means for estimating a focal length of the camera at the time of shooting a certain image based on the dimensional coordinates and the camera position information and shooting direction information at the time of shooting a certain image; Based on, are included in the one image and the other image, 3-dimensional coordinates to function as the coordinate estimation means for estimating the three-dimensional coordinates of the target point is unknown. According to the computer that executes the image processing program, it is possible to easily and appropriately estimate the focal length in a certain image. Based on the certain image and other images, the target points included in these images can be estimated. Three-dimensional coordinates can be estimated.

  In order to achieve the above object, a recording medium according to the fourth aspect of the present invention provides a computer with a plurality of images including at least a part of a target, and three-dimensional coordinates of a plurality of target points existing in a predetermined target. A storage medium for storing an image processing program for estimation based on the image storage means for storing a plurality of images, camera position information and shooting direction information at the time of shooting the images, A coordinate storage unit that stores three-dimensional coordinates of one or more target points of the target, a real position in a certain image of a feature point corresponding to the target point for which the three-dimensional coordinates are stored in the coordinate storage unit, and a target Based on the three-dimensional coordinates of the point and the position information and shooting direction information of the camera at the time of shooting a certain image, the focal length estimation means for estimating the focal length of the camera at the time of shooting a certain image was estimated Scorching Distance based, are included in the one image and the other image, 3-dimensional coordinates to store the image processing program to function as the coordinate estimation means for estimating the three-dimensional coordinates of the target point is unknown. According to the computer that executes the image processing program, it is possible to easily and appropriately estimate the focal length in a certain image. Based on the certain image and other images, the target points included in these images can be estimated. Three-dimensional coordinates can be estimated.

  In order to achieve the above object, an image processing system according to a fifth aspect of the present invention is based on a plurality of images including at least a part of a target based on three-dimensional coordinates of a plurality of target points existing in a predetermined target. An image processing system for estimation, including a camera that captures an image, a device that can be attached to the camera, and that can change the position and direction of the camera, and state-identifiable data that can identify the state of the device Storage control for storing in the image storage means camera information specifying means for specifying the camera position information and the shooting direction information, the image taken by the camera, and the camera position information and shooting direction information at the time of shooting Means, a coordinate storage means for storing three-dimensional coordinates for one or more target points, and a target point for which the three-dimensional coordinates are stored in the coordinate storage means. Based on the actual position of the feature point in a certain image, the three-dimensional coordinates of the target point, and the position information and photographing direction information of the camera at the time of photographing a certain image, A focal length estimation means for estimating a focal length, and based on the estimated focal length, estimate a three-dimensional coordinate of a target point that is included in a certain image and another image and whose three-dimensional coordinate is unknown. Coordinate estimation means. According to such an image processing system, it is possible to easily and appropriately estimate the focal length in a certain image, and based on the certain image and another image, the three-dimensional coordinates of the target points included in these images are obtained. Can be estimated.

It is a functional block diagram of the image processing system which concerns on one Embodiment of this invention. It is a figure showing an example of a device and a camera in an image processing system concerning one embodiment of the present invention. It is a flowchart which shows a series of processes of the image processing system which concerns on one Embodiment of this invention. It is a figure which shows an example of the frame sequence which concerns on one Embodiment of this invention. It is a figure which shows an example of the calibration mark which concerns on one Embodiment of this invention. It is a figure explaining an example of the first two frames of the image sequence which concerns on one Embodiment of this invention. It is a figure explaining the feature point matching which concerns on one Embodiment of this invention. It is a figure explaining the method of calculating a three-dimensional coordinate from the set of the feature points which the different images match concerning one Embodiment of this invention. It is a figure which shows an example of the three-dimensional coordinate of the target point obtained from a pair of image which concerns on one Embodiment of this invention. It is a figure explaining the method to calculate the focal distance which concerns on one Embodiment of this invention. It is a figure which shows the three-dimensional coordinate of the target point obtained from the some image which concerns on one Embodiment of this invention. It is a figure which shows the normal line corresponding to the target point which concerns on one Embodiment of this invention. It is a figure explaining the marching cube method concerning one embodiment of the present invention. It is a figure which shows an example of the some cube in the surrounding environment which concerns on one Embodiment of this invention. It is a figure which shows an example of the mesh in the surrounding environment which concerns on one Embodiment of this invention. It is a figure which shows an example of the three-dimensional model with which the texture which concerns on one Embodiment of this invention was stuck.

  Embodiments of the present invention will be described with reference to the drawings. The embodiments described below do not limit the invention according to the claims, and all the elements and combinations described in the embodiments are essential for the solution of the invention. Is not limited.

  First, an image processing system according to an embodiment of the present invention will be described.

  FIG. 1 is a functional configuration diagram of an image processing system according to an embodiment of the present invention.

  The image processing system 1 includes a device 2, a camera 3 that can capture surrounding images, a device controller 4 for controlling the operation of the device 2, and an image processing apparatus 5.

  The device 2 is arranged in a predetermined environment and can operate to change its posture. In this embodiment, the camera 3 is attached to the device 2, and by operating the device 2, the position of the camera 3 (viewpoint position) and the orientation of the camera 3 (shooting direction: yaw, pitch, roll) are changed. be able to. Since the position of the camera 3 with respect to the part to which the device 2 is attached is determined, the position and orientation of the camera 3 can be grasped from the posture of the device 2.

  An example of the device 2 is a robot placed inside a work space. For example, in the case of a robot, the camera 3 is arranged with respect to the arm of the robot. Since the position and orientation of the camera 3 in the robot arm are determined when fixed to the robot, the position and orientation of the camera 3 can be grasped from the state of the robot arm. For example, if the robot arm is composed of a plurality of members connected via a plurality of joints, the position and orientation of the camera 3 can be grasped from the angles of the joints of the arms. The device 2 can change its posture by the device controller 4.

  The camera 3 receives an image capture instruction via the device 2, captures an image (image data) of the surrounding environment (target), and passes it to the image processing apparatus 5. The camera 3 can capture images by changing the focal length. For example, the camera 3 changes the focal length so that a certain object in the shooting range is in focus.

  The device controller 4 receives a command from the main control unit 11 of the image processing apparatus 5 and controls the attitude of the device 2 according to the command.

  The image processing apparatus 5 is configured by a computer including, for example, a CPU (Central Processing Unit), a ROM (Read Only Memory), a RAM (Random Access Memory), and the like. The image processing apparatus 5 includes a main control unit 11 as an example of a camera information specifying unit, an image acquisition unit (frame grabber) 12 as an example of a storage control unit, a focal length estimation unit, a coordinate estimation unit, and camera information optimization. And an image processing unit 13 as an example of a three-dimensional coordinate optimization unit and a model forming unit, a simulation unit 14, and a storage unit 15 as an example of an image storage unit and a coordinate storage unit. In the present embodiment, the main control unit 11, the image processing unit 13, and the simulation unit 14 are provided in the same image processing apparatus 5. However, for example, at least one of them is a separate apparatus. Each part may be provided in a different device.

  The storage unit 15 stores various data. In the present embodiment, the storage unit 15 stores camera information 16, an image sequence 17, an environment three-dimensional (3D) model 18, a device three-dimensional (3D) model 19, and a simulation result 20. The storage unit 15 also stores data used for processing by the main control unit 11, the image acquisition unit 12, the image processing unit 13, or the simulation unit 14.

  The main control unit 11 is mainly configured by the CPU executing device control software, and commands (for example, the device 2) for controlling the posture of the device 2 in accordance with a user instruction or setting content by an input unit (not shown). The command including the angle of each joint is output to the device controller 4. Further, the main control unit 11 sequentially outputs commands for causing the image acquisition unit 12 to capture an image in accordance with a user instruction or setting content by an input unit (not shown). Here, considering the speed of subsequent processing, there is sufficient overlap between successive images in a series of captured images (images, frames), and each image has a different camera position (orientation at the same camera position). It is preferable to output the command so that it is obtained from the above. For example, when the user sequentially instructs the user to move the device 2 to shoot, it is necessary to instruct the main control unit 11 to obtain the above-described image.

  In addition, the main control unit 11 outputs the position of the camera 3 attached to the device 2 at that time point based on the command (state identifiable data) that controls the attitude of the device 2 every time a command for capturing an image is output. (Viewpoint position) and orientation are calculated, and camera information including position information indicating the position and orientation information indicating the orientation is passed to the image acquisition unit 12.

  The image acquisition unit 12 is mainly configured by the CPU executing a frame grabber application. Each time a command for capturing an image is received from the main control unit 11, the image acquisition unit 12 captures an image at that time from the camera 3. The camera information passed from the main control unit 11 (that is, the position information and orientation information of the camera when the image is taken) is stored in the storage unit 15 in association with each other. By such processing, the image acquisition unit 12 stores a series of images (image sequence 17) from various viewpoints in the surrounding environment of the device 2 in the storage unit 15.

  The image processing unit 13 is mainly configured by a CPU executing a processing program, and creates and creates a three-dimensional model of the surrounding environment (environmental three-dimensional model) using the camera information 16 and the image sequence 17. The stored environment three-dimensional model 18 is stored in the storage unit 15. Details of the process of creating the three-dimensional model will be described later.

  The simulation unit 14 is mainly configured by a CPU executing a simulation program, and executes various simulation processes based on an environment three-dimensional model 18 and a device three-dimensional model (device three-dimensional model) 19. Then, the simulation result 20 is stored in the storage unit 15. Here, the target of the device three-dimensional model 19 is not limited to the device 2 to which the camera 3 is attached in order to create the environment three-dimensional model 18, and may be any device, for example, used in the environment. It may be the device you want to consider. The simulation process may be, for example, a point teaching process, a route search process, a collision detection process for the device environment, or the like.

  FIG. 2 is a diagram illustrating an example of a device and a camera in the image processing system according to the embodiment of the present invention.

  The device 2 is used, for example, in an ambient environment SE where various objects as shown in FIG. 2 exist. In the surrounding environment SE, it is required that the device 2 does not collide with an object existing in the surrounding environment SE. Therefore, it is beneficial to create a three-dimensional model of the surrounding environment SE and perform collision processing and the like.

  In the present embodiment, the camera 3 is attached to the arm portion of the device 2 in order to obtain an image of the surrounding environment SE. Since the camera 3 is attached to the arm portion of the device 2 as shown in FIG. 2, the state of the arm portion of the device 2 (arm Therefore, the position and orientation of the camera 3 can be grasped relatively accurately based on the angle of each joint in the unit. Therefore, in the present embodiment, the main control unit 11 calculates the position and orientation of the camera 3 based on the state of the device 2.

  First, an outline of a series of processes of the image processing system will be described.

  FIG. 3 is a flowchart showing a series of processes of the image processing system according to the embodiment of the present invention.

  The image processing system 1 executes a process of acquiring and storing a series of images (image sequence) of the surrounding environment as shown in FIG. 4 (step S1). Here, as an image sequence, there is sufficient overlap between successive images, and the camera viewpoint at the time of acquiring each image is practical so that depth information necessary for subsequent processing can be generated reliably. It is preferable that they are separated from each other. In addition, when acquiring a series of images according to a user's instruction, the user needs to pay attention to this point. Note that doing this is not a difficult task for the user to perform and gain experience.

  In this step, the image processing system 1 calculates the position and orientation of the camera 3 at the time of capturing each image based on, for example, each joint angle of the arm of the device 2 and stores it together with the image.

  Next, initial processing for calculating various initial setting values is executed using the first two frames of the frame sequence (step S2). Here, the first two frames are frames including a predetermined calibration pattern (see FIG. 5), and using the calibration pattern, the focal length of the camera 3 at the time of shooting each frame, The three-dimensional coordinates of the target point corresponding to the set of matching feature points included in these two images are calculated.

  Next, a plurality of feature points are detected from each subsequent frame (step S3), and it is detected whether there is a feature point that matches the feature point of each frame and the feature point of the previous frame (step S3). S4) Estimating and optimizing the three-dimensional coordinates of the target points corresponding to the set of matching feature points, and restoring a set of three-dimensional coordinates for a plurality of target points in the surrounding environment (step S5).

  Next, based on the three-dimensional coordinates of each point of the surrounding environment, a process of adjusting the mesh structure indicating the surrounding environment (target) is executed (step S6), and the texture showing the appearance with respect to the patch indicating the surface of the target Is executed (step S7). Thereby, the three-dimensional model of the surrounding environment is restored.

  Next, the image processing apparatus 5 accepts whether or not the restoration of the three-dimensional model is successful from the user (whether or not the three-dimensional model is satisfactory) (step S8), and if the three-dimensional model is not successful. Processing for acquiring an additional image to be used for reconstruction of the three-dimensional model is executed (step S9), and the processing from step S3 is executed again.

  On the other hand, when it is accepted from the user that the restoration of the three-dimensional model is successful (step S8: YES), the simulation unit 14 loads the data of the three-dimensional model (step S10) and uses the three-dimensional model. Then, a predetermined simulation process is executed (step S11).

  Next, a series of processes of the image processing system will be described in detail. The description will be given with reference to FIG. 3 as appropriate.

  First, in the image sequence acquisition step (step S1), the following processing is performed.

  That is, the main control unit 11 issues a command to the device controller 4, and the device controller changes the attitude of the device 2. Then, the main control unit 11 causes the image acquisition unit 12 to acquire an image from the camera 3 and causes the storage unit 15 to store the image acquired by the image acquisition unit 12. At this time, the main control unit 11 calculates the position and orientation of the camera 3 attached to the device 2 based on the joint angle of the device 2 in the command issued to the device controller 4 and notifies the image acquisition unit 12 of the calculated position and orientation. The image acquisition unit 12 stores the camera position and orientation in the storage unit 15 in association with the image. Note that the image acquisition unit 12 executes a process of removing radial distortion from the image obtained from the camera 3 and stores the image after execution in the storage unit 15. It should be noted that the parameters used to remove radial distortion from the image can be estimated in advance based on a pinhole camera model that can be assumed to be compatible with an actual camera.

  By repeating such processing, a series of image groups (image sequences) from a plurality of viewpoints about the surrounding environment are stored in the storage unit 15. Here, as an image sequence, there is a sufficient overlap between consecutive images (frames), and the camera viewpoint when each image is acquired so that depth information necessary for subsequent processing can be generated reliably. Are substantially separated. In addition, when acquiring a series of images according to a user's instruction, the user needs to pay attention to this point. Note that doing this is not a difficult task for the user to perform and gain experience.

Here, the position t _j = (x _j , y _j , z _j ) of the camera 3 stored in association with the frame j (j is an arbitrary integer) and the orientation R _j (3 × 3 rotation matrix: world The matrix (converted from the coordinate system to the camera coordinate system) is an initial value (initial guess value) for the frame j. The position and orientation of the camera 3 at the time of acquiring the frame j will be refined by the processing described later.

Next, the focal length f _j of each frame j, which is a parameter relating to another camera necessary for processing, is estimated. Here, in this embodiment, the coordinate system is described according to the following convention. In the image space (the coordinate system in the frame), the coordinates of each pixel are indicated by coordinates (x, y) with the upper left corner of the frame as the origin. The x value increases from left to right on the frame, and the y value increases from top to bottom. The world coordinate system, which is a three-dimensional coordinate system serving as a reference for the surrounding environment, is a right-handed system in which the positive direction of the x axis indicates the right, the positive direction of the y axis indicates the top, and the positive direction of the z axis indicates the outside. Is following. For this reason, the direction of the camera 3 always faces the negative direction of the z axis of the local coordinate system (camera coordinate system).

  FIG. 4 is a diagram illustrating an example of a frame sequence according to an embodiment of the present invention.

  The following steps will be described by way of example in which the frame sequence shown in FIG. 4 is obtained in step S1.

  First, initialization processing (step S2) is executed using the first two frames of the sequence. In this embodiment, the first two frames have two crosses CM1 and CM2 (calibration patterns) as shown in FIG. 5 facing the front of the camera 3 and separated by a known distance Z (see FIG. 6). The frame is taken with the point of view as the viewpoint of the camera 3. The distance between the two crosses CM1 and CM2 is a known distance D as shown in FIG.

  FIG. 6 is a diagram illustrating an example of the first two frames of an image sequence according to an embodiment of the present invention.

  As shown in FIG. 6, the first frame (frame 1) is a frame taken by the camera 3 from the viewpoint P1 facing the front with respect to the crosses CM1 and CM2 and separated by a distance Z from these crosses. The second frame (frame 2) is a frame taken by the camera 3 from the viewpoint P2 facing the front with respect to the crosses CM1 and CM2 and a distance Z from these crosses.

Here, if the positions of the crosses CM1 and CM2 on the frame are x _ij (where _i is the cross (CM1 or CM2) and _j is the frame number), x ₁₁ , x Specific coordinates of ₂₁ , x ₁₂ , and x ₂₂ can be grasped from each frame 1 and frame 2. The focal lengths f ₁ and f ₂ of the camera 3 at the time of photographing each of the frames 1 and 2 can be expressed as the following formulas (1) and (2). The image processing unit 13 calculates the focal length of the camera 3 in the frame 1 and the frame 2 using the mathematical expressions (1) and (2).

Next, the image processing unit 13 detects a plurality of feature points for each frame (step S3), and performs processing (step S4) for detecting a set of matching feature points between the two frames. Known methods for detecting a set of feature points include Harris corner matching using sum of squared difference (SSD) and zero-mean cross-correlation (ZMCC), and scale invariant feature transform (SIFT) matching. . By this method, a plurality of corresponding (matching) pairs of feature points in two frames are detected.

  FIG. 7 is a diagram for explaining feature point matching according to an embodiment of the present invention.

  By the above-described processing, as shown in FIG. 7, a set of matching feature points is detected. For example, feature points FPA, FPB, and FPC of frame 1 are detected as matching with feature points FPa, FPb, and FPc of frame 2, respectively. Therefore, the feature points FPA and FPa, the feature points FPB and FPb, and the feature points FPC and FPc are detected as a set of matching feature points.

  The three-dimensional coordinates of the points (target points) in the surrounding environment corresponding to each pair of matching feature points detected in this way are already calculated with the position and orientation of the camera 3 at the time of shooting each frame. It can be calculated by triangulation according to the focal length.

  Here, a method of calculating three-dimensional coordinates for a set of matching feature points will be described.

  FIG. 8 is a diagram illustrating a method for calculating three-dimensional coordinates from a set of matching feature points of different images according to an embodiment of the present invention.

The target points corresponding to the feature points in the first frame and the feature points in the second frame, which are sets of matching feature points, exist on the lines shown in the following equations (3) and (4). To do. Here, l _A and l _B represent straight line equations, s and t represent straight line parameters, P _A and P _B represent the coordinates of the viewpoint of the camera 3 in each frame, and v _A and v _B represents a vector from the viewpoint in the negative direction of the Z axis of the world coordinate system to each feature point in each frame.

Here, since the target point corresponding to the feature point is one point, ideally, l _A and l _B intersect at one point, but actually, due to various error factors, The straight lines l _A and l _B may not intersect. Therefore, in the present embodiment, as shown in FIG. 8, the points D _A and D _B where the straight lines l _A and l _B are closest to each other are obtained, and the intermediate point is set as a target point corresponding to the set of feature points. . Here, as shown below, by obtaining s ′ and t ′ which are specific values from the formula (5), the three-dimensional coordinates of the points D _A and D _B from the formula (6) and the formula (7) And the three-dimensional coordinates of the target point which is the intermediate point can be obtained.

By calculating the three-dimensional coordinates of the target point from the set of feature points described above for a plurality of sets of feature points obtained from two frames, the three-dimensional coordinates of the plurality of target points are calculated. can do.

  FIG. 9 is a diagram illustrating an example of three-dimensional coordinates of a target point obtained from a pair of images according to an embodiment of the present invention.

  As shown in FIG. 9, the three-dimensional coordinates of a plurality of target points OP can be calculated from a pair of images (frame 1 and frame 2).

  Next, the processes in steps S3 to S5 are executed using one frame (for example, the second frame) used in the above process and the next frame (for example, the third frame) in the frame sequence. Here, the feature point detection processing from the third frame and the matching feature point pair detection processing between the second frame and the third frame are executed by the same processing as described above.

  Next, processing for calculating the focal length for the third frame is executed.

  FIG. 10 is a diagram illustrating a method for calculating a focal length according to an embodiment of the present invention.

In the present embodiment, in the matching feature point pair detection process (step S4), the matching feature point pairs detected between the second frame and the third frame are already three-dimensional coordinates. There is a feature point corresponding to the target point for which is calculated. Here, let X _i be such a target point. In addition, since there is a set of feature points corresponding to the target point for which the three-dimensional coordinates have already been calculated, the frame in which the set of feature points has already been detected (for example, the first frame) and the third frame Some surrounding environment including the target point needs to overlap. This can be realized by considering it when acquiring the frame sequence.

Here, regarding the three-dimensional coordinates (world coordinate system) of the target point X _i , X _i (for example, the frame j−1 and the frame j−2 when the current frame is j, for example, X _i ( The following description will be made assuming that italic font) = (X _i , Y _i , Z _i ) ^T is calculated. Here, T indicates transposition.

The camera position t _j and the orientation R _j in the frame j (for example, the third frame) can be grasped because they are associated with the image data in advance and stored in the storage unit 15. Also, the coordinates x _{i, j} (italic) = (x _{i, j} , y _{i, j} ) of the position (actual position) of the feature point corresponding to the target point X _i in the frame j should be grasped from the frame j. Can do.

On the other hand, when the three-dimensional coordinates X _i (italic font) of the corresponding target point are used, the three-dimensional coordinates Y _ij (italic font) in the camera coordinate system at the time of shooting the frame j for the target point are expressed by 8). Here, R _j is an orientation, and is represented as a 3 × 3 rotation matrix in the present embodiment. Then, the coordinates P _j (X _i ) of the position (estimated position) where the feature point corresponding to the target point is estimated to be located in the frame j are expressed by Equation (9). Here, P _j represents the mapping from the world coordinate system to the image coordinate system of frame j, f _j represents the focal length in frame j, and N represents the frame j from the camera coordinate system shown in Equation (10). Represents a transformation into a two-dimensional plane parallel to the frame coordinate system.

Here, in order to estimate the focal length, the mapping position (estimated position) P _{j of} the frame j to the frame coordinate system estimated from the already calculated three-dimensional coordinates of the target point as shown in Equation (11). An error function e _ij relating to a deviation between (X _i ) and the coordinates x _{i, j} (italic font) of the position (actual position) of the corresponding feature point that actually exists in the frame j is introduced, and the error function e although the _ij 3D coordinates were for multiple target points being grasped to introduce a total error value e _j shown in equation (12) representing the total. The error function is not limited to this, and may be a function related to the deviation between the estimated position and the actual position.

Here, when the focal length f _j of the frame j is changed, the position of the estimated mapping of the target point calculated by Expression (9) is changed, and as a result, the error function e _ij and Expression (11) of Expression (11) are changed. and thus affect the total error value _{e j} 12). The focal length in the case where such a total error value e _j is minimized, it may be estimated that the focal length in the frame j. Note that the minimization problem of the total error value e _j may be solved in closed form (closed form).

In the present embodiment, the camera position and orientation are acquired from the device 2, but it is preferable that the camera position and orientation are also accurate. Therefore, the total error value e _j of Equation (12) is changed by changing the camera position, orientation, and focal length for all target points for which three-dimensional coordinates have already been detected (estimated) in the frame j. In order to minimize this, a conjugate gradient method will be used.

The coordinate Y _ij shown in the equation (8) can be rewritten into a gradient expression as shown in the equation (13) so that the rotation and transformation variables are independent of each other. Here, the rotation matrix T _j is as shown in Equation (14), and s _j is as shown in Equation (15).

Further, the rotation matrix T _j is expressed as a three-dimensional vector in the Rodriguez form as shown in the equations (16), (17), and (18) so that no additional restriction is required in the optimization process. .

In this representation, rotation, axis _a = it is defined by the rotation of the _{(a x, a y, a} z) related angular theta. Here, a _x ² + a _y ² + a _z ² = 1.

  According to the Rodriguez form expression, the rotation matrix is as shown in equations (19) to (25).

In this representation, the following _{gradient, δP j (X i) /} δv j, δP j (X i) / δs j, δP j (X i) / δf j is as shown in equation (26) - (30) become. Here, “:” in Equation (26) indicates a column matrix.

There was thus obtained by the conjugate gradient method, the focal length that can minimize the total error value e _j in Equation (12), camera position, and with the orientation, similar processes (triangulation or the like as described above ) To calculate (estimate) the three-dimensional coordinates of the target point corresponding to each of the plurality of sets of feature points matched between the third frame and the second frame. As a result, the three-dimensional coordinates of target points (including target points whose three-dimensional coordinates are unknown) corresponding to all of the pairs of feature points matched between the second frame and the third frame are estimated. Note that the three-dimensional coordinate data for each target point is stored in the storage unit 15.

  Also, with respect to a new processing target frame (new target frame) after the third frame, the same processing as the third frame described above, that is, matching between the new target frame and the previous frame is performed. Using feature point detection processing, focal length estimation processing in the new target frame, and estimated focal length, a matching feature point set between the new target frame and the previous frame is supported. A process of detecting the three-dimensional coordinates of the target point is executed.

  In this way, it is possible to estimate the three-dimensional coordinates for a plurality of target points using all the frames to be processed in the frame sequence.

  In the above embodiment, the feature points are matched between successive frames to estimate the three-dimensional coordinates of the target point. However, the present invention is not limited to this, and the camera viewpoint is similar, that is, the surroundings It is also possible to perform feature point matching between any two frames in which a part of the environment overlaps, and estimate the three-dimensional coordinates of the target point corresponding to the obtained set of matching feature points.

  In the present embodiment, in order to estimate the three-dimensional coordinates of the target point with higher accuracy, the image processing unit 13 further performs a global optimization process described below.

Here, the parameters to be optimized are the camera position t _j , orientation R _j , focal length f _j , and three-dimensional coordinates X _i (italic font) of the target point of each frame j.

  Here, the objective function E for optimizing them is as shown in Equation (31).

In order to optimize this objective function, a two-stage approach is performed: optimization of camera parameters (camera position, orientation, focal length) and optimization of three-dimensional coordinates. With this approach, errors contained in the three-dimensional coordinates can be reduced.

  In this approach, when optimizing the camera parameters, the three-dimensional coordinates of the target point are treated as fixed values, and when optimizing the three-dimensional coordinates of the target points, the camera parameters are treated as fixed values.

  In this approach, when optimizing camera parameters, the camera parameters in each frame are irrelevant (independent) to the other frames, and when optimizing 3D coordinates, The target point is irrelevant (independent) to other target points.

  In the optimization of camera parameters, all of a plurality of target points detected in one target frame are used and optimized using the conjugate gradient method as described above. This optimizes the camera parameters.

  On the other hand, in the three-dimensional coordinate optimization step, the following processing is executed.

The error function e _i to be minimized for a certain target point i is the sum of the values for all the frames j in which the target point exists, as shown in Equation (32). Here, R ^t _p indicates the p-th column of the transposed matrix of the matrix R. Formula (32) is a function (displacement) regarding the distance (deviation) between the position (actual position) of the feature point corresponding to a certain target point i on the frame and the mapping position estimated from the three-dimensional coordinates of the target point i. In the present embodiment, it means the sum of all the frames in which the feature points corresponding to the target point i exist.

Equation (32) can be rewritten as shown in Equations (33-1) and (33-2). Equations (32) and (33-1) are equivalent because they deal with one target point (as opposed to optimizing all target points simultaneously). As a result, there is no need to change each error term. Equation (33-1) can be minimized in closed form.

Here, regarding a point that is considered to correspond to a set of feature points that are erroneously detected when matched, the value in a certain frame becomes extremely large. Therefore, when the size becomes extremely large in this way, such target points are excluded from the target points in the surrounding environment. For this reason, it is possible to appropriately remove the three-dimensional coordinates of the target point for which an erroneous determination has been made, and to appropriately prevent an adverse effect on the formation of the three-dimensional model. In this way, as shown in FIG. 11, it is possible to appropriately estimate the three-dimensional coordinates of a plurality of target points in the surrounding environment. A set of data of the three-dimensional coordinates of each target point estimated in this way is stored in the storage unit 15. Thereby, the process up to step S5 is completed.

  Next, the image processing unit 13 executes a process (step S6) for applying a mesh structure indicating a target in the surrounding environment.

  First, a tangent plane for each target point in the surrounding environment where the three-dimensional coordinates are estimated is estimated. These tangent planes are treated as local linear approximations to the surface of the object in the surrounding environment.

For each target point i, neighboring target points that are within a predetermined distance ρ from the three-dimensional coordinates X _i of the target point are selected, and a plane represented by the formula of N _i X + c _i = 0 for these target points. Is applied. Here, N _i indicates a vertical plane, and c _i is a corresponding constant plane.

  If there are almost no target points in the vicinity, a plane cannot be applied to the target point.

  There are two methods for fitting the plane. First, as a first method, there is a method of minimizing the objective function expressed by the mathematical formulas (34), (35), and (36).

Here, the IRLS (Iteratively reweighted least squares) method using the Geman-Mc-Clure estimator with the initial estimation value given by the least square method is used.

Expressed as shown in equations (37) to (40), the perpendicular plane that minimizes the objective function and the constant plane are in the form shown in equations (41) and (42). Here, w (e) shows a formula for calculating the weighting of X _k, M represents the average value of the weighted, C is showed variance matrix, s indicates the total value of the weighting . Further, μ represents the minimum eigenvalue of the 3 × 3 dispersion matrix. Therefore, N _i is an eigenvector corresponding to the eigenvalue of the matrix C.

The normal facing outward returns to an image in which the target point i can be seen, and can be distinguished from the normal facing inward by comparing the viewpoint direction in the frame with the normal. Here, a normal line facing outward is stored in the storage unit 15. The center of the plane O _i is defined as the point on the plane closest to X _i . If the distance between O _i and X _i is sufficiently close, the tangent plane fitting process for object point i is complete.

  On the other hand, if not, no good match is found, so try the second method.

In this case, it should plan contains the target point X _i, changing only the orientation of the plane is performed. In other words, the error function is as shown in equations (43) and (44).

To calculate the solution of N _i, the same Geman-Mc Clure estimator IRLS method using are used. In order to solve only the above N _i , a closed solution is given. If sufficient support is obtained from nearby target points in the fitting of the plane, the normal line pointing to the outside is stored. The center of the plane O _i is the target point X _i , from which the value of C _i can be calculated. Otherwise, the plane cannot be applied to the object point i. Thus, the normal line NL estimated with respect to each target point is expressed as shown in FIG. 12, for example.

Once all the tangent planes are estimated, a signed distance function can be defined that calculates the distance from any object point P (= (P _x , P _y , P _z )) in space to the surface. For the target point P, all vertical planes j whose center point O _j is within a predetermined distance from the target point P are selected. If no such plane is found, the distance of point P from the surface is assumed to be infinite.

For a plane k whose normal is N _k and whose center is O _k , the distance from the point P to the plane k is simply expressed by Equation (45).

The estimated distance of point P from the surface is a weighted average of those distances for each plane. The weight assigned to a certain plane k is shown in Equation (46). Here, α is an attenuation constant for preventing division by 0. Equation (46), the distance to the plane k of the point P, which means that the point P is weighted more heavily than closer to the center O _k.

Using these weights, the final estimated distance of the point P from the surface is given by the equation (47).

In this way, the distance from the surface at any point can be calculated.

  Next, a specific process for applying a mesh to the target point from which the three-dimensional coordinates are detected is executed.

  Here, a marching cube algorithm is used. The basic concept of this marching cube algorithm is that a voxel (cube) can be defined by the values of the eight corners of the cube.

  If one or more corners of the cube are less than a predetermined value β and the value of one or more corners is greater than the predetermined value β, the voxel contributes to a certain configuration of the surface. It means that

  By determining which edge of the cube is traversed by the surface, a triangular patch is formed to divide the cube between the area inside the surface and the area outside the surface.

  Combining these patches from all cubes forms a surface appearance composed of triangular patches.

  Here, when the threshold value β is set to 0, the cube corner value d (P) is expressed by Equation (47).

  In the representation of the marching cube, there are a total of 256 different situations depending on the combination of the cube corner values and the threshold value β. Each situation can be generalized to 15 families in consideration of rotation and symmetry. Corresponding to each family, the state of the triangular patch can be determined as shown in FIG.

Starting from the point of interest X _i , the marching cube with both inner and outer corners of the surface (ie, with corners of both values greater than the threshold β and values less than the threshold β) is Initialize by centering the cube to O _i . Initialize the triangular patch in the cube. In this way, an initial structure for the surface is generated.

  And if all the corners are not cubes inside or outside the surface, the cube is grown on the side that surrounds the eight corners, the value of each corner is estimated, and a triangular patch is generated.

  The process of growing cubes in this way continues until all the cubes on the boundary are each inside the surface or outside the surface.

  For each triangular patch, it is stored along with the outward normal.

  All the cubes included in the restoration in the image sequence of this embodiment are as shown in FIG.

  If there is an object point that does not exist inside any previously constructed cube, this means that the object point is on a surface that is not connected to any of the previously constructed surfaces. In this case, as described above, the surface can be restored by the process of initializing the cube and growing the cube. Such processing is executed until there are no unprocessed target points. This completes the processing step (step S6) of adjusting (applying) the mesh of the three-dimensional model. FIG. 15 shows an example of a three-dimensional model in which a mesh composed of triangular patches PT is adjusted.

  Next, a texture pasting process (step S7) is executed. When the mesh is adjusted, the appearance of each triangular face can be grasped from the image in which the face appears in the foreground. This process is easy because the normal of each face, the camera position and orientation of each frame are already known. Therefore, the image processing unit 13 grasps the texture of each appearance from the image and pastes the texture on the corresponding mesh face. The image processing unit 13 obtains the data of the three-dimensional (3D) model for the surrounding environment including the three-dimensional coordinates of the target point, the mesh structure, the texture information to be attached to the face, and the like obtained by the above processing. Store in the storage unit 15.

  FIG. 16 shows a mesh in which the texture TX grasped for each face is attached, that is, a three-dimensional model. In this way, the texture pasting process (step S7) ends.

  The three-dimensional model created in this way is displayed on a display screen (not shown). On the other hand, the user can instruct confirmation of the processing result as to whether or not the three-dimensional model is satisfactory. For example, when an instruction indicating that the processing result is not good is received from the user, the image processing apparatus 5 performs a process of acquiring a new image in step S9. In this step, for example, an image in a range where the target points are not sufficient can be additionally acquired. For this reason, it is possible to more appropriately estimate the three-dimensional coordinates of the target point and restore the three-dimensional model more appropriately by continuously executing the processing after step S3 using the newly added image.

  On the other hand, when the instruction that the processing result is good is received, the simulation unit 14 reads the three-dimensional model 18 of the surrounding environment stored in the storage unit 15 (step S10), and uses the three-dimensional model of the surrounding environment. Run the simulation. As the simulation, for example, tasks such as teaching of robot operation, collision detection with the surrounding environment of the robot, and route search can be executed realistically.

  According to this embodiment, the task of creating a three-dimensional model can be simplified, the three-dimensional coordinates of target points in a more appropriate surrounding environment can be grasped, and a three-dimensional model indicating the surrounding environment can be appropriately selected. Can be restored.

  Although the present invention has been described based on the embodiments, the present invention is not limited to the above-described embodiments, and can be applied to various other modes.

  For example, in the above embodiment, the simulation of robot teaching or the like has been described using a three-dimensional model of the surrounding environment as an example. For example, a virtual tour in a museum is realized by creating a three-dimensional model of a museum, for example. can do. Further, the created three-dimensional model can be used as a virtual space in the game.

DESCRIPTION OF SYMBOLS 1 Image processing system, 2 devices, 3 cameras, 4 device controllers, 5 Image processing apparatus, 11 Main control part, 12 Image acquisition part, 13 Image processing part, 14 Simulation part, 15 Storage part, 16 Camera information, 17 Image sequence , 18 Environment 3D model, 19 Device 3D model, 20 Simulation results.

Claims (12)

An image processing apparatus that estimates three-dimensional coordinates of a plurality of target points existing in a predetermined target based on a plurality of images including at least a part of the target,
Image storage means for storing the plurality of images and positional information and shooting direction information of the camera at the time of shooting the images;
Coordinate storage means for storing three-dimensional coordinates for one or more target points of the target;
The actual position in a certain image of the feature point corresponding to the target point for which the three-dimensional coordinate is stored in the coordinate storage means, the three-dimensional coordinate of the target point, and at the time of shooting the certain image A focal length estimating means for estimating a focal length of the camera at the time of shooting the certain image based on the position information and shooting direction information of the camera;
An image having coordinate estimation means for estimating a three-dimensional coordinate for a target point having an unknown three-dimensional coordinate, which is included in the certain image and another image, based on the estimated focal length Processing equipment. The focal length estimation means includes an estimated position on the certain image of a feature point corresponding to the target point estimated based on the position information, the shooting direction information, and the three-dimensional coordinates, and the feature point The image processing apparatus according to claim 1, wherein the focal length is estimated based on the actual position on the certain image. The focal length estimation means has a smallest sum of functions relating to a deviation between the estimated position of each feature point corresponding to a plurality of target points and the actual position of each feature point corresponding to the plurality of target points. The image processing apparatus according to claim 2, wherein the focal distance is estimated. The image processing apparatus according to claim 3, wherein the focal length estimation unit estimates a focal length that minimizes a sum of the functions related to the shift by using a conjugate gradient method. The camera that shoots the image is attached to a device that can change the position and shooting direction of the camera,
Camera information specifying means for specifying the position information and the shooting direction information of the camera based on state-identifiable data capable of specifying the state of the device;
5. The storage control unit according to claim 1, further comprising: a storage control unit that stores the image captured by the camera and the position information and the shooting direction information of the camera at the time of the shooting in the image storage unit. An image processing apparatus according to claim 1. After estimating the three-dimensional coordinates of a plurality of target points using a plurality of images to be processed, the position of the camera at the time of photographing each image using the three-dimensional coordinates of the plurality of target points included in each image The image processing apparatus according to claim 1, further comprising a camera information optimizing unit that optimizes the information and the photographing direction information. After estimating the three-dimensional coordinates of the target point using the plurality of images to be processed, based on the position information and the shooting direction information of the camera in the plurality of images including the same target point, the target point The image processing apparatus according to claim 1, further comprising three-dimensional coordinate optimization means for optimizing the three-dimensional coordinates. The image processing apparatus according to claim 1, further comprising a model forming unit that forms a three-dimensional model for the target based on the three-dimensional coordinates of the plurality of target points. An image processing method by an image processing apparatus for estimating three-dimensional coordinates of a plurality of target points existing in a predetermined target based on a plurality of images including at least a part of the target,
An image storage step in which the image storage means of the image processing apparatus stores the plurality of images, and positional information and shooting direction information of the camera at the time of shooting the images;
A coordinate storage step in which the coordinate storage means of the image processing apparatus stores three-dimensional coordinates for one or more target points of the target;
The focal length estimation unit of the image processing apparatus includes an actual position in a certain image of a feature point corresponding to the target point for which the three-dimensional coordinate is stored in the coordinate storage unit, and the three-dimensional of the target point. A focal length estimating step for estimating a focal length of the camera at the time of shooting the certain image based on coordinates and position information and shooting direction information of the camera at the time of shooting the certain image;
The coordinate estimation means of the image processing device includes a three-dimensional coordinate for a target point whose unknown three-dimensional coordinates are included in the certain image and another image based on the estimated focal length. And a coordinate estimation step for estimating. An image processing program for causing a computer to estimate three-dimensional coordinates of a plurality of target points existing in a predetermined target based on a plurality of images including at least a part of the target,
Image storage means for storing the plurality of images, and positional information and shooting direction information of the camera at the time of shooting the images;
Coordinate storage means for storing three-dimensional coordinates for one or more target points of the target;
The actual position in a certain image of the feature point corresponding to the target point for which the three-dimensional coordinate is stored in the coordinate storage means, the three-dimensional coordinate of the target point, and at the time of shooting the certain image A focal length estimating means for estimating a focal length of the camera at the time of shooting the certain image based on the position information and shooting direction information of the camera;
An image that functions as a coordinate estimation unit that estimates a three-dimensional coordinate of a target point whose three-dimensional coordinate is unknown, which is included in the certain image and another image, based on the estimated focal length. Processing program. A recording medium for storing an image processing program for causing a computer to estimate three-dimensional coordinates of a plurality of target points existing in a predetermined target based on a plurality of images including at least a part of the target. And
Image storage means for storing the plurality of images, and positional information and shooting direction information of the camera at the time of shooting the images;
Coordinate storage means for storing three-dimensional coordinates for one or more target points of the target;
The actual position in a certain image of the feature point corresponding to the target point for which the three-dimensional coordinate is stored in the coordinate storage means, the three-dimensional coordinate of the target point, and at the time of shooting the certain image A focal length estimating means for estimating a focal length of the camera at the time of shooting the certain image based on the position information and shooting direction information of the camera;
An image that functions as a coordinate estimation unit that estimates a three-dimensional coordinate of a target point whose three-dimensional coordinate is unknown, which is included in the certain image and another image, based on the estimated focal length. A recording medium for storing a processing program. An image processing system that estimates three-dimensional coordinates of a plurality of target points existing in a predetermined target based on a plurality of images including at least a part of the target,
The camera for taking the image;
A device to which the camera is mounted and which can change the position and shooting direction of the camera;
Camera information specifying means for specifying the position information and the shooting direction information of the camera based on state-identifiable data capable of specifying the state of the device;
Storage control means for storing the image photographed by the camera and the position information and the photographing direction information of the camera at the time of photographing in an image storage means;
Coordinate storage means for storing three-dimensional coordinates for one or more target points of the target;
The actual position in a certain image of the feature point corresponding to the target point for which the three-dimensional coordinate is stored in the coordinate storage means, the three-dimensional coordinate of the target point, and at the time of shooting the certain image A focal length estimating means for estimating a focal length of the camera at the time of shooting the certain image based on the position information and shooting direction information of the camera;
An image having coordinate estimation means for estimating a three-dimensional coordinate for a target point having an unknown three-dimensional coordinate, which is included in the certain image and another image, based on the estimated focal length Processing system.