JP5032415B2 - Motion estimation apparatus and program - Google Patents

Description

  The present invention relates to a motion estimation apparatus and program, and more particularly, to a motion estimation apparatus and program for estimating changes in the position and orientation of an imaging unit based on a captured image.

  2. Description of the Related Art Conventionally, a moving object periphery monitoring device that identifies a relative position and posture of an imaging unit is known (Patent Document 1). In this moving object periphery monitoring device, four or more feature points on a specific plane are extracted from an image, the feature points are tracked, and the position is determined based on the coordinate values of the feature points on two image planes. The posture is calculated.

Also, the corresponding points are obtained from two or more images obtained from the imaging means, the constraint condition of the corresponding points of the image is calculated from the position and orientation information obtained by the acceleration sensor and the magnetic sensor, and the corresponding points are determined by the constraint conditions. An image input device that calculates the relative positional relationship of the imaging device with high accuracy by correction is known (Patent Document 2).
JP 2004-198211 A JP 2004-78315 A

  However, in the technique described in Patent Document 1 described above, the positional relationship of the imaging device is calculated by detecting four corresponding points on the same plane, but when there is no knowledge about the captured image, There is a problem that it is difficult to detect four points on the same plane.

  Further, the technique described in Patent Document 2 has a problem that other sensors such as an acceleration sensor and a magnetic sensor are required in addition to the imaging device.

  The present invention has been made to solve the above-described problems, and provides a motion estimation device and a program that can accurately estimate changes in the position and orientation of an imaging unit with a simple configuration. Objective.

  In order to achieve the above object, a motion estimation apparatus according to a first aspect of the present invention includes an imaging unit that is attached to a moving body and images a predetermined region, and the imaging unit has a first position and a first posture. A feature point extracting means for extracting a plurality of feature points from the first image captured when the image is taken, and points corresponding to each of the feature points extracted from the first image by the feature point extracting means. Search means for searching from a second image captured when the imaging means is in a second position and a second attitude different from the first position and the first attitude; and the first For each of the feature points of the image, a weighting factor determination unit that determines a larger weighting factor as the number of the feature points existing in the predetermined area including the feature point of the first image decreases, The feature point of the first image and the second image Change in the position and orientation of the imaging means estimated on the basis of a plurality of pairs with the point between the first image and the second image when the position and orientation change The change in the position and orientation at which the sum of the weighting factors of the pair of the corresponding feature point and the point is equal to or greater than a predetermined value or the maximum value is represented by the first position, the first orientation, and the second Estimation means for estimating the change in position and orientation of the imaging means between the position and the second orientation.

  A program according to a second aspect of the present invention provides a computer from a first image captured when an imaging unit that is attached to a moving body and images a predetermined area is in a first position and a first posture. A feature point extracting unit that extracts a plurality of feature points, and the imaging unit selects a point corresponding to each of the feature points extracted from the first image by the feature point extracting unit. Search means for searching from the second image captured when the second position and the second position are different from the posture of the first image, and the first image for each of the feature points of the first image Weight coefficient determining means for determining a larger weight coefficient as the number of the feature points existing in the predetermined region including the feature points decreases, and the corresponding feature points of the first image and the second image Based on multiple pairs with points A change in the position and orientation of the imaging means, and a set of the feature point and the point corresponding to each other between the first image and the second image when the position and orientation change The change in the position and posture at which the sum of the weighting coefficients is equal to or greater than a predetermined value or the maximum value is determined between the first position and the first posture and the second position and the second posture. It is a program for functioning as estimation means for estimating the change in position and orientation of the imaging means.

  According to the first invention and the second invention, when the imaging means is in the first position and the first posture, the imaging means takes a first image, and the imaging means takes the first position and When the second position and the second position are different from the first position, the second image is captured by the photographing unit.

  Then, a plurality of feature points are extracted from the first image by the feature point extraction means, and points corresponding to each of the feature points extracted from the first image by the feature point extraction means are obtained by the search means. Search from 2 images.

  Further, the weighting factor determining means determines a larger weighting factor for each of the feature points of the first image as the number of feature points existing in the predetermined area including the feature points of the first image decreases.

  And the change of the position and orientation of the imaging means estimated by the estimation means based on the plurality of pairs of the feature points of the corresponding first image and the points of the second image, the position and orientation change The position and orientation change at which the sum of the weighting factors of the pair of feature points corresponding to the first image and the second image is equal to or greater than a predetermined value or the maximum value when the And a change in the position and orientation of the imaging means between the first orientation and the second and second orientations.

  As described above, for the feature points, the smaller the number of feature points existing in the predetermined area including the feature points, the larger the weighting factor is determined, and when the position and orientation changes as the change in the position and orientation of the imaging means. Estimate changes in the position and orientation of the imaging means with high accuracy by estimating changes in the position and orientation where the sum of the weighting factors of the pairs of feature points corresponding to the images increases. be able to.

  According to a third aspect of the present invention, there is provided a motion estimation apparatus that is attached to a moving body and that captures an image of a predetermined area, and a first image that is captured when the imaging unit is in a first position and a first posture. The imaging means extracts a feature point extracting means for extracting a plurality of feature points from the image and points corresponding to each of the feature points extracted from the first image by the feature point extracting means. Retrieval means for retrieving from the second image captured when the second position and the second posture are different from the first position and the first posture, and each of the feature points of the first image , Weighting factor determination means for determining a larger weighting factor as the distance between the feature point of the first image and the other feature point is longer, and the feature point and the second image of the corresponding first image The imaging hand estimated based on a plurality of pairs with a point Of the weighting coefficient of the set of the feature point and the point corresponding to each other between the first image and the second image when the position and posture change. The position of the imaging means between the first position and the first position and the second position and the second position is the change in the position and position where the sum is equal to or greater than a predetermined value or the maximum value. And estimation means for estimating the change in posture.

  According to a fourth aspect of the present invention, there is provided a program from a first image captured when an imaging unit attached to a moving body and images a predetermined area is in a first position and a first posture. A feature point extracting unit that extracts a plurality of feature points, and the imaging unit selects a point corresponding to each of the feature points extracted from the first image by the feature point extracting unit. Search means for searching from the second image captured when the second position and the second position are different from the posture of the first image, and the first image for each of the feature points of the first image A weighting factor determination unit that determines a larger weighting factor as the distance between the feature point and the other feature point is longer, and a plurality of the feature point of the corresponding first image and the point of the second image Of the imaging means estimated based on the set A sum of the weighting factors of the set of feature points and points corresponding to the first image and the second image when the position and orientation change. Change in the position and orientation at which is equal to or greater than a predetermined value or a maximum value, the position of the imaging means between the first position and the first position and the second position and the second position, and It is a program for functioning as an estimation means for estimating a change in posture.

  According to the third invention and the fourth invention, when the imaging means is in the first position and the first posture, the imaging means takes a first image, and the imaging means takes the first position and When the second position and the second position are different from the first position, the second image is captured by the photographing unit.

  Then, a plurality of feature points are extracted from the first image by the feature point extraction means, and points corresponding to each of the feature points extracted from the first image by the feature point extraction means are obtained by the search means. Search from 2 images.

  In addition, the weighting factor determination unit determines a larger weighting factor for each of the feature points of the first image as the distance between the feature point of the first image and the other feature points is longer.

  And the change of the position and orientation of the imaging means estimated by the estimation means based on the plurality of pairs of the feature points of the corresponding first image and the points of the second image, the position and orientation change The position and orientation change at which the sum of the weighting factors of the pair of feature points corresponding to the first image and the second image is equal to or greater than a predetermined value or the maximum value when the And a change in the position and orientation of the imaging means between the first orientation and the second and second orientations.

  In this way, for feature points, the larger the distance from other feature points, the larger the weighting factor is determined, and the feature points corresponding between images when the position and orientation change as the change in the position and orientation of the imaging means. By estimating the change in the position and orientation at which the sum of the weight coefficients of the pairs of points and points becomes large, the change in the position and orientation of the imaging means can be estimated with high accuracy with a simple configuration.

  According to a fifth aspect of the present invention, there is provided a motion estimation apparatus that is attached to a moving body and that captures an image of a predetermined area, and a first image captured when the imaging unit is in a first position and a first posture. The imaging means extracts a feature point extracting means for extracting a plurality of feature points from the image and points corresponding to each of the feature points extracted from the first image by the feature point extracting means. Retrieval means for retrieving from the second image captured when the second position and the second posture are different from the first position and the first posture, and each of the feature points of the first image , A weighting factor determination unit that determines a larger weighting factor as the size of the feature amount indicating the degree of change in density or luminance between the feature point of the first image and surrounding points is larger, and the corresponding first The feature points of the image and the points of the second image; Changes in the position and orientation of the imaging means estimated based on a plurality of sets, the features corresponding between the first image and the second image when the position and orientation change The change of the position and orientation at which the sum of the weighting factors of a set of points and the point is equal to or greater than a predetermined value or the maximum value is represented by the first position, the first attitude, the second position, and the first Estimation means for estimating the change in the position and orientation of the imaging means between two orientations.

  According to a sixth aspect of the present invention, there is provided a program from a first image captured when an imaging unit that is attached to a moving body and images a predetermined area is in a first position and a first posture. A feature point extracting unit that extracts a plurality of feature points, and the imaging unit selects a point corresponding to each of the feature points extracted from the first image by the feature point extracting unit. Search means for searching from the second image captured when the second position and the second position are different from the posture of the first image, and the first image for each of the feature points of the first image Weighting factor determining means for determining a larger weighting factor as the size of the feature quantity indicating the degree of change in density or brightness between the feature point and the surrounding points of the first feature point, and the corresponding feature point of the first image, The second image point Changes in the position and orientation of the imaging means estimated on the basis of the set of the feature points corresponding between the first image and the second image when the position and orientation change The change of the position and orientation at which the sum of the weighting factors of the set of the point and the point is equal to or greater than a predetermined value or the maximum value is represented by the first position, the first attitude, the second position, and the second Is a program for functioning as an estimation unit that estimates a change in the position and orientation of the imaging unit between two positions.

  According to the fifth invention and the sixth invention, when the imaging means is in the first position and the first posture, the imaging means takes a first image, and the imaging means takes the first position and When the second position and the second position are different from the first position, the second image is captured by the photographing unit.

  Then, a plurality of feature points are extracted from the first image by the feature point extraction means, and points corresponding to each of the feature points extracted from the first image by the feature point extraction means are obtained by the search means. Search from 2 images.

  Further, for each of the feature points of the first image, the larger the feature amount indicating the degree of change in density or luminance between the feature points of the first image and the surrounding points by the weighting factor determination means. Determine a large weighting factor.

  And the change of the position and orientation of the imaging means estimated by the estimation means based on the plurality of pairs of the feature points of the corresponding first image and the points of the second image, the position and orientation change The position and orientation change at which the sum of the weighting factors of the pair of feature points corresponding to the first image and the second image is equal to or greater than a predetermined value or the maximum value when the And a change in the position and orientation of the imaging means between the first orientation and the second and second orientations.

  As described above, when the feature point indicating the degree of change in density or luminance with respect to the surrounding points is increased, a larger weighting factor is determined, and the position and orientation change as the change in the position and orientation of the imaging unit. By estimating changes in position and orientation that increase the sum of the weighting factors of pairs of feature points corresponding to images, the position and orientation of the imaging means can be estimated with high accuracy and with a simple configuration. can do.

  According to a seventh aspect of the present invention, there is provided a motion estimation apparatus that includes an imaging unit that is attached to a moving body and images a predetermined region, and a first image that is captured when the imaging unit is in a first position and a first posture. The imaging means extracts a feature point extracting means for extracting a plurality of feature points from the image and points corresponding to each of the feature points extracted from the first image by the feature point extracting means. Retrieval means for retrieving from the second image captured when the second position and the second posture are different from the first position and the first posture, and each of the feature points of the first image The image of the predetermined area including the feature point of the first image is more similar to the image of the predetermined area including the feature point of the second image corresponding to the feature point of the first image. Corresponding to weighting factor determination means to determine a large weighting factor A change in the position and orientation of the imaging means estimated based on a plurality of sets of the feature point of the first image and the point of the second image, and the position and orientation change A change in the position and orientation at which the sum of the weighting factors of the set of the feature point and the point corresponding to the first image and the second image is a predetermined value or more or a maximum value, And estimation means for estimating the change in the position and orientation of the imaging means between the first position and the first orientation and the second position and the second orientation.

  According to an eighth aspect of the present invention, there is provided a program from a first image captured when an imaging unit that is attached to a moving body and images a predetermined area is in a first position and a first posture. A feature point extracting unit that extracts a plurality of feature points, and the imaging unit selects a point corresponding to each of the feature points extracted from the first image by the feature point extracting unit. Search means for searching from the second image captured when the second position and the second position are different from the posture of the first image, and the first image for each of the feature points of the first image A larger weight coefficient is determined so that the image of the predetermined area including the feature point of the second image and the image of the predetermined area including the feature point of the second image corresponding to the feature point of the first image are similar to each other. Weight coefficient determination means and corresponding A change in the position and orientation of the imaging means estimated based on a plurality of sets of the feature point of the first image and the point of the second image, and the position and orientation change The change in the position and orientation at which the sum of the weighting factors of the set of the feature point and the point corresponding to each other between the first image and the second image becomes a predetermined value or more or a maximum value, A program for functioning as an estimation unit that estimates a change in position and posture of the imaging unit between a first position and the first posture and the second position and the second posture.

  According to the seventh invention and the eighth invention, when the imaging means is in the first position and the first posture, the imaging means takes a first image, and the imaging means takes the first position and When the second position and the second posture are different from the first posture, the second image is picked up by the photographing means.

  Then, a plurality of feature points are extracted from the first image by the feature point extraction means, and points corresponding to each of the feature points extracted from the first image by the feature point extraction means are obtained by the search means. Search from 2 images.

  In addition, for each of the feature points of the first image, an image of a predetermined area including the feature points of the first image and a second image corresponding to the feature points of the first image are obtained by the weighting factor determination unit. A weighting factor that is larger as the image is similar to an image in a predetermined area including a point is determined.

  And the change of the position and orientation of the imaging means estimated by the estimation means based on the plurality of pairs of the feature points of the corresponding first image and the points of the second image, the position and orientation change The position and orientation change at which the sum of the weighting factors of the pair of feature points corresponding to the first image and the second image is equal to or greater than a predetermined value or the maximum value when the And a change in the position and orientation of the imaging means between the first orientation and the second and second orientations.

  Thus, for the feature points, a weighting factor that is so large that the predetermined region including the feature points and the predetermined region including the corresponding second image point are similar to each other is determined. The position and orientation of the imaging means can be determined with a simple configuration by estimating the change in position and orientation in which the sum of the weighting factors of pairs of corresponding feature points between the images increases when the position and orientation change. Can be estimated with high accuracy.

  In order to avoid setting a high weight for a pair of feature points corresponding to a possibility of erroneous correspondence, in the fifth and sixth inventions, the degree of change in density or luminance between the feature point and surrounding points is set. A larger weighting coefficient is set as the feature amount to be shown is larger. In the seventh and eighth inventions, an image of a predetermined region including the feature point of the first image and a second image corresponding to the feature point A larger weighting coefficient is set as the image of the predetermined area including the point becomes more similar.

  A point where the size of the feature point is large is easy to obtain a correspondence between images. Therefore, a weight is set based on a low possibility of miscorresponding, or a point corresponding between two images. If the images of each predetermined area including the image are similar, a high weight is set for a set of feature points that are likely to be miscorresponding by setting a weight based on the low possibility of miscorrespondence Can be prevented, and correct motion estimation becomes possible.

  As described above, according to the motion estimation device and the program of the present invention, the weighting coefficient is determined for the feature point, and when the position and orientation change as the change in the position and orientation of the imaging means, the correspondence is made between images. By estimating the change in the position and orientation at which the sum of the weighting factors of the feature point-point pairs increases, it is possible to estimate the change in the position and orientation of the imaging means with high accuracy with a simple configuration. An effect is obtained.

  Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. The case where the present invention is applied to a motion estimation device mounted on a vehicle will be described as an example.

  As shown in FIG. 1, a motion estimation device 10 according to the present embodiment is attached to a vehicle (not shown), captures the front of the vehicle, and generates an image. And a computer 14 that estimates the amount of movement and the amount of rotation as a change in the position and orientation of the image capturing device 12 based on the image obtained from the image capturing device 12.

  The image capturing device 12 captures an image of the front of the vehicle and generates an image signal of the image, an A / D converter 20 that performs A / D conversion on the image signal generated by the image capturing unit 18, and an A / D And an image memory 22 for temporarily storing the D-converted image signal.

  The computer 14 includes a CPU, a RAM, and a ROM that stores a program for executing a later-described motion estimation processing routine, and is functionally configured as follows. The computer 14 extracts a plurality of feature points that are easy to track on the image from the first image obtained by the image capturing device 12, and the first image extracted by the feature point extraction unit 30. A second point obtained by the image pickup device 12 by picking up a point corresponding to the feature point at a second position and a second posture different from the first position and the first posture at which the first image is picked up. A corresponding point search unit 32 for searching and tracking on the image of the image and detecting the image coordinates of the point corresponding to the feature point of the first image, and a plurality of the first image extracted by the feature point extraction unit 30 Using the image coordinates of the feature points as input, the feature point weight determination unit 34 that determines the weighting coefficient for each feature point, the image coordinates of the feature points of the corresponding first image obtained by the corresponding point search unit 32, and the second Sets of image coordinates of points of the image and feature point weight determination unit 4, the weighting coefficient for each feature point determined in 4 is input, and the change from the first position when the first image is captured to the second position when the second image is captured 12 movements of the three movements in the three-axis direction are estimated, and the change to the second posture when the second image is taken with reference to the first posture when the first image is taken, A motion estimation unit 36 that estimates the amount of rotation with reference to the three axes of motion of the image capturing device 12 and an output unit 38 that outputs the result of the motion estimation unit 36 are provided.

  The feature point extraction unit 30 extracts a plurality of feature points from the first image obtained from the image capturing device 12. A feature point refers to a point that can be distinguished from surrounding points and that makes it easy to obtain a correspondence between different images. The feature points are automatically extracted by using a method (for example, a Harris operator) that detects pixels in which the gradient value of the change in shading increases two-dimensionally. In the method using the Harris operator, feature points are extracted as described below.

  First, the matrix M is calculated by the following equation (1), where the luminance of the point (u, v) of the image is I (u, v).

However, I _u and I _v represent horizontal and vertical differentiation, respectively, and G _σ represents smoothing by a Gaussian distribution with a standard deviation σ.

Then, using the eigenvalues λ ₁ and λ ₂ of the matrix M calculated by the above equation (1), the corner strength is calculated by the following equation (2).

  However, k is a preset constant, and a value of 0.04 to 0.06 is generally used. Then, a point at which the corner strength is equal to or greater than the threshold value and is maximized is selected, and the selected point is extracted as a feature point.

  The corresponding point search unit 32 associates each of the feature points extracted from the first image by the feature point extraction unit 30 between the first image and the second image, and A point corresponding to the feature point is searched from the second image. When searching for corresponding points in the first image and the second image, it is assumed that the luminance of the corresponding points in the first image and the second image and the brightness of the surrounding points do not change. The luminance matching between the first image and the second image may be performed. For example, using the Lucas-Kanade method, a point corresponding to the feature point on the first image is detected from the second image. Search for.

In the Lucas-Kanade method, the luminance value of the coordinates (x, y) of the first image is I (x, y), and the luminance value of the coordinates (x, y) of the second image is J (x, y). When representing the coordinates (p _x + u, _py + v) of the second image corresponding to the coordinates (p _x , p _y ) of the first image, the function represented by the following expression (3) is minimized. Is obtained by obtaining (u, v).

Here, Ω is a window centered on (p _x , p _y ), and w (x, y) is a function (such as a Gaussian function) that takes a larger value as the center of the window is a weight function. is there. I _x (x, y) is a value obtained by differentiating I (x, y) in the x-axis direction, and I _y (x, y) is a value obtained by differentiating in the y-axis direction.

The feature point weight determination unit 34 determines a weight coefficient for each of the plurality of feature points extracted by the feature point extraction unit 30 as described below. First, as shown in FIG. 2, a window centered on the feature point (see window w1 set around feature point p1 and window w2 around feature point p2 in FIG. 2) is set and set. The number of other feature points in the window is counted (for example, one point is counted for the window w1 in FIG. 2 and four points are counted for the window w2). Then, the smaller the number of other feature points in the window, the smaller the weighting factor is determined, and the smaller the number of other feature points, the larger the weighting factor is determined in accordance with the counted number of feature points. For example, when the number of feature points in the set window is n, the weighting coefficient is determined according to the following equation (4).
(Weighting factor) = k · exp (−n) (4)

However, k is a constant. Note that the equation for determining the weighting factor is not limited to the above equation (5), and may be determined according to the following equation (5), for example.
(Weighting factor) = k / (n + 1) (5)

  The motion estimation unit 36 includes image coordinates of a plurality of sets of corresponding points in the first image and the second image obtained from the corresponding point search unit 32, and a weight coefficient of each feature point obtained from the feature point weight determination unit 34. As described below, the change in the position and orientation of the image capturing device 12 when each of the first image and the second image is captured (based on the movement amount in the XYZ axis direction and the XYZ axis) Rotation amount) is calculated.

As shown in FIG. 3, the change of the position and orientation is based on the rotation matrix R from the first image to the second image (the rotation amount based on the X axis, the rotation amount based on the Y axis, and the Z axis based on And a translation vector t (a movement amount t _{x in} the X-axis direction, a movement amount t _{y in} the Y-axis direction, and a movement amount t _{z in the} Z-axis direction). Note that the elements of the rotation matrix R and the translation vector t are physical quantities representing conversion of image coordinates between two images.

Here, a calculation method of the rotation matrix R and the translation vector t from the first image to the second image will be described. For the first image coordinates of the corresponding point of the n points in the image I _i and image coordinates I _i of the corresponding point of n points in the second image _'(n ≧ 8), correspondence is correct errors of corresponding points Otherwise, there is a 3 × 3 matrix F that satisfies the following expression (6) as a matrix indicating changes in position and orientation.

However, I _i = (u _i , v _i , 1) ^T , I _i ′ = (u _i ′, v _i ′, 1) ^T , and the image coordinates (u _i , v _i ) in the first image The image coordinates of the point in the second image corresponding to the point are (u _i ′, v _i ′).

Here, the matrix F satisfying the above equation (6) has a constant multiple indefiniteness. That is, when F _s satisfies the above equation (6), αF _s also satisfies the above equation (6) (where α is a real number). Therefore, the matrix F can be expressed as the following equation (7).

  Further, from the above formulas (6) and (7), the following formula (8) is obtained.

Here, if there are eight or more pairs of corresponding points I _i and I _i ′, at least eight of the above equations (8) are obtained, so that eight variables f _{11 to} f ₃₂ can be obtained.

  In addition, when the correspondence between the feature points is obtained from the real image, the corresponding image coordinates include an error, and among the feature points, there are feature points associated with an incorrect point. Therefore, the motion estimation unit 36 maximizes the sum Z of the weighting factors represented by the following equation (9) based on the image coordinates of the plurality of sets of corresponding points and the determined weighting factors of each feature point. A matrix F such that

Here, w _i is a weighting coefficient of the i-th feature point determined by the feature point weight determination unit 34, and δ (x) is a function that takes 1 if x is true and 0 otherwise. D (a, b) represents the distance between a and b, and θ is a preset threshold value.

  A matrix F that is calculated based on a plurality of pairs of corresponding points by specifying a matrix that maximizes the total sum Z of the weighting coefficients in the above equation (9). A matrix F is specified that maximizes the sum of the weighting factors determined for the feature points of the set of corresponding points between the first image and the second image.

  When the calibration matrix K of the imaging unit 18 is known, the rotation matrix R and the translation vector according to the following equations (10) and (11) based on the matrix F specified as described above. t is calculated and output as a change in the position and orientation of the image capturing device 12 when each of the first image and the second image is captured.

  With the above method, the motion estimation unit 36 determines the position and orientation of the image capturing device 12 based on the image coordinates of a plurality of corresponding points in the first image and the second image and the weighting coefficient of each feature point. Calculate the change in.

  Next, the principle of this embodiment will be described. When estimating motion using multiple images, it is necessary to determine the correspondence of feature points between multiple images. However, if the correspondence between feature points is determined by automatic processing, incorrect features will be obtained. A point correspondence is included. Therefore, a method of selecting a motion according to the correspondence of as many feature points as possible is generally used so as not to affect the estimation of the motion due to the correspondence of erroneous feature points.

  If correct motion is estimated, the correspondence of feature points on objects at various distances should follow the estimated motion, but when using an image taken of the road environment with a camera mounted on the vehicle, the distance There are few feature points extracted from the road surface part near, while many feature points are extracted from the background part such as a distant building, so selecting the motion that maximizes the number of feature points to follow is incorrect. There is a possibility of estimating the movement.

  For example, when three feature points that are close to each other are extracted, 30 feature points that are at a similar distance in the distance are extracted, and five of the feature points in the far 30 points are incorrectly matched If the motion that maximizes the number of feature points to be followed is selected, the motion that matches the correspondence of the 30 feature points in the distance including the wrong correspondence is estimated, although it does not match the correspondence of the three nearby feature points. The In other words, the motion that matches the correspondence of 30 feature points including the erroneous correspondence of far away has more correspondence of the feature points that follow the motion than the correct motion that follows the correspondence of 28 feature points excluding the false correspondence. Therefore, correct motion is not estimated.

  From the above, it is desirable to select a motion according to the correspondence of feature points at various distances, but it is difficult to know in advance the distance between feature points extracted from the image. Therefore, feature points that are close to each other on the image use the knowledge that the distance is close even in a three-dimensional space, weight the feature points according to the density of feature points on the image from which the feature points are extracted, Correct motion estimation is realized by selecting the motion that maximizes the sum of the weights of the feature points of the corresponding points that follow the motion.

  In the previous example, the feature point density is low for the three feature points that are close to each other, so the weighting factor is increased, and the feature point density is high for the 30 feature points that are far away. To do. As a result, the weighting factors of the three neighboring points are larger than the weighting factors of the five far points, so that the motion according to the correspondence of the feature points including the erroneously corresponding five points is not selected. In this way, by increasing the weighting coefficient for the feature points in the region where the feature point density is low on the image and estimating the motion that maximizes the sum of the weights of the feature points of the corresponding points according to the motion, Even if the distribution of the distance between feature points extracted from inside is biased, correct motion estimation is possible.

  Next, the operation of the motion estimation apparatus 10 according to the first embodiment will be described. An example in which the motion of the image capturing device 12 is estimated while the vehicle on which the motion estimation device 10 is mounted will be described.

  First, the image capturing device 12 is directed to the front of the host vehicle, and the image capturing device 12 uses the first position and the first posture of the image capturing device 12 at the start of the motion to be estimated. Is captured to generate a first image. Then, the front region of the host vehicle is imaged by the image capturing device 12 at a second position and a second posture different from the first position and the first posture of the image capturing device 12 at the end of the estimated motion. When the second image is generated, the motion estimation processing routine shown in FIG.

  First, in step 100, a first image is acquired from the image pickup device 12, and in step 102, a plurality of feature points are extracted from the first image acquired in step 100. Then, in step 104, a second image is acquired from the image capturing device 12, and in the next step 106, points corresponding to each of the plurality of feature points on the first image extracted in step 102 are determined. The second image obtained in step 104 is searched for and tracked, and the image coordinates of the point on the second image corresponding to each feature point are detected.

In step 108, a variable i for identifying a feature point is set to an initial value 1, and the number of feature points extracted in step 102 is set to a constant N representing the number of feature points. In the next step 110, a predetermined window centered on the feature point p _i is set on the first image, the number of feature points other than the feature point p _i existing in the window is counted, in 112, based on the number of other feature points in the window that is counted in step 110, according to the above (4), determines the weight coefficient w _i for the feature point p _i.

  In step 114, it is determined whether or not the variable i is less than the constant N. If there is a feature point in which the variable i is less than the constant N and the weighting factor has not been determined, the variable i is determined in step 116. Increment i and return to step 110. On the other hand, if the variable i is equal to or greater than the constant N in step 114 and the weighting coefficients have been determined for all feature points, the process proceeds to step 118.

By executing the above steps 100 to 116, n sets of corresponding feature points on the first image and points on the second image as shown in the following expression (12), and n Data consisting of the weight coefficients of the individual feature points is obtained.
(I _i , I _i ′, w _i ), i = 1,..., N (12)

Here, I _i is a homogeneous coordinate vector of image coordinates in the first image, and I _i = (u _i , v _i , 1) ^T. Also, _{I i} 'is the homogeneous coordinate vector of the image coordinates of the second _{image, I i' = (u i} ', v i', 1) is ^T. Further, the image coordinates of the point in the second image corresponding to the image coordinates (u _i , v _i ) of the feature point in the first image are (u _i ′, v _i ′).

In step 118, a variable r indicating the number of repetitions is set to an initial value of 1. In step 120, eight sets of corresponding points are selected at random from the N sets of corresponding points. Then, in step 122, the eight pairs of corresponding points image coordinates of selected in step 120, according to the above (8), calculates an element _f 11 _{~f 32} of the matrix F, which is calculated element _{f 11} A matrix F _r is generated from ~ f ₃₂ .

In the next step 124, based on the matrix F _r generated in step 120, the image coordinates of the N corresponding points, and the weighting factor of each feature point determined in step 112, according to (9) above. When the matrix F _r is applied, the sum Z _r of the weight coefficients of the feature points of the set of corresponding points between the first image and the second image is calculated.

  In step 126, it is determined whether or not the variable r is less than a constant T indicating the maximum number of repetitions. If the variable r is less than the constant T, in step 128, the variable r is incremented, Return to step 120. On the other hand, if the variable r is greater than or equal to the constant T in step 126, the process proceeds to step.

In step 130, the matrix F _r that maximizes the sum Z _{r of the} weighting coefficients calculated in step 124 is specified. In step 132, based on the matrix F _r specified in step 130, (10) The first position of the image capturing device 12 when the first image is captured and the first position of the image capturing device 12 when the second image is captured according to the equations (11) and (11) As the change from the second posture, the movement amount of the movement of the image pickup device 12 in the XYZ axis direction and the rotation amount based on the XYZ axis are calculated.

  In the next step 134, the movement amount and the rotation amount of the image capturing device 12 calculated in step 132 are displayed on the display (not shown) of the computer 14 as the motion estimation result, and the motion estimation processing routine is terminated.

  As described above, according to the motion estimation device according to the present embodiment, for each extracted feature point, the number of other feature points existing in the window centered on the feature point on the first image. The smaller the is, the larger the weighting coefficient is determined, and when the matrix indicating the change in the position and orientation of the image capturing device is applied, the feature points on the first image and the points on the second image corresponding between the images By identifying a matrix that indicates a change in the position and orientation at which the sum of the weighting factors of the pair is maximum, and estimating the change in the position and orientation of the image pickup device, the change in the position and orientation of the image pickup device is highly accurate. Can be estimated.

  In addition, the motion of the image pickup apparatus can be estimated with high accuracy with a simple configuration without using any other sensor besides the image pickup apparatus.

  When estimating motion based on an image, if feature points are extracted from the entire image, many feature points are extracted from a part of the image area where objects with many patterns exist. Fewer feature points are extracted from a region with less. If the motion is estimated based on such feature point correspondence, the accuracy deteriorates. In the present embodiment, motions are estimated by applying different weighting factors to feature points extracted from regions with many patterns and feature points extracted from regions with few patterns, regardless of the number of feature points. The motion can be estimated so as to minimize the error of points at various distances, and the motion estimation accuracy is improved.

  Next, a motion estimation apparatus according to the second embodiment will be described. In addition, since the structure of the motion estimation apparatus which concerns on 2nd Embodiment is the same as that of 1st Embodiment, it attaches | subjects the same code | symbol and abbreviate | omits description.

  The second embodiment is different from the first embodiment in that the weighting coefficient of the feature point is determined according to the distance from other feature points.

  In the motion estimation apparatus according to the second embodiment, the feature point weight determination unit 34 calculates the distance on the first image from the feature point to other feature points around it, and sets the distance values in ascending order. The average value d of the distance values up to the m-th when arranged is determined, a smaller weight coefficient is determined as d is smaller, and a weight coefficient is determined as d is larger.

  For example, as shown in FIG. 5, when the weight values of feature points p1 are determined when using the distance values up to the third when the distance values are arranged in ascending order when determining the weight coefficients of the feature points, Then, three feature points that are close to the feature point p1 are obtained, and an average value d of the distances d1, d2, and d3 between the three feature points and the feature point p1 to be determined is calculated. For the feature point p1 to be determined, a smaller weight coefficient is determined as the average distance d is smaller, and a larger weight coefficient is determined as the average distance is larger.

  In the motion estimation processing routine according to the second embodiment, first, a first image is acquired from the image capturing device 12, and a plurality of feature points are extracted from the first image. Then, a second image is acquired from the image capturing device 12, and points corresponding to each of the plurality of feature points on the first image are searched and tracked on the second image, and each feature point is detected. The image coordinates of the corresponding point on the second image are detected.

Then, with respect to the extracted N feature points p _i (i = 1 to N), three feature points that are close to the feature point p _i on the first image are extracted from the surroundings of the feature points p _i . extracted, calculates an average value of the distance between the extracted three feature points and the feature point p _i, in accordance with the magnitude of the average value of the distance calculated, determines a weight coefficient w _i for the feature point p _i To do.

Then, eight sets of corresponding points are randomly selected from the N sets of corresponding points, and the elements f _{11 to} f ₃₂ of the matrix F are calculated from the image coordinates of the selected eight sets of corresponding points. A matrix F _r is generated from the elements f _{11 to} f ₃₂ . Next, based on the generated matrix F _r , the image coordinates of the N corresponding points, and the determined weighting factor of each feature point, the first image and the second image when the matrix F _r is applied calculating the sum Z _r of the weighting coefficients of the set of feature points of the corresponding points between the images.

When the process of calculating the weight coefficient sum Z _r from the eight randomly selected points is repeated a predetermined number of times T, a matrix F _r that maximizes the calculated weight coefficient sum Z _r is obtained. Based on the identified matrix _Fr , the movement amount of the movement of the image pickup device 12 in the XYZ axis direction and the rotation amount based on the XYZ axis are calculated.

  In this way, for each extracted feature point, a larger weighting factor is determined as the average distance from the feature points around the feature point on the first image is longer, and the change in the position and orientation of the image capturing device is determined. Identifies a matrix that indicates a change in position and orientation that maximizes the sum of the weighting factors of pairs of feature points on the first image and points on the second image that correspond to each other when the matrix shown is applied Thus, by estimating the change in the position and orientation of the image capturing apparatus, it is possible to estimate the change in the position and orientation of the image capturing apparatus with high accuracy with a simple configuration.

  Next, a motion estimation apparatus according to the third embodiment will be described. In addition, since the structure of the motion estimation apparatus which concerns on 3rd Embodiment is the same as that of 1st Embodiment, it attaches | subjects the same code | symbol and abbreviate | omits description.

  The third embodiment is different from the first embodiment in that the weighting coefficient of the feature point is determined according to the feature amount indicating the degree of change in luminance with the surrounding points.

  In the motion estimation apparatus according to the third embodiment, the feature point weight determination unit 34 uses the Harris operator in the feature point extraction unit 30 as a feature amount indicating the degree of change in luminance with the surrounding points. The corner strength obtained at the time of extraction is acquired, and the larger the corner strength of the feature point, the larger the weighting factor is determined. The smaller the corner strength, the smaller the weighting factor is determined.

  In the motion estimation processing routine according to the third embodiment, first, the first image and the second image are acquired from the image capturing device 12, and a plurality of feature points are extracted from the first image. A point corresponding to each of the plurality of feature points on one image is searched and tracked on the second image, and the image coordinates of the point on the second image corresponding to each feature point are detected.

Then, for the extracted N feature points p _i (i = 1 to N), the corner strength obtained when extracting the feature points p _i is acquired, and according to the size of the acquired corner strength. , to determine the weight coefficient _{w i} for the feature point _{p i.}

Then, eight sets of corresponding points are selected at random from the N sets of corresponding points, and the elements f _{11 to} f ₃₂ of the matrix F are calculated from the image coordinates of the selected eight sets of corresponding points. _A matrix F _r is generated from _{11 to} f ₃₂ . Next, based on the generated matrix F _r , the image coordinates of the N corresponding points, and the determined weighting factor of each feature point, the first image and the second image when the matrix F _r is applied calculating the sum Z _r of the weighting coefficients of the set of feature points of the corresponding points between the images.

When the process of calculating the weight coefficient sum Z _r from the eight randomly selected points is repeated a predetermined number of times T, a matrix F _r that maximizes the calculated weight coefficient sum Z _r is obtained. Based on the identified matrix _Fr , the movement amount of the movement of the image pickup device 12 in the XYZ axis direction and the rotation amount based on the XYZ axis are calculated.

  Thus, for each extracted feature point, a larger weighting factor is determined as the corner strength of the feature point on the first image is larger, and a matrix indicating changes in the position and orientation of the image capturing device is applied. A matrix indicating a change in position and orientation at which the sum of the weighting factors of a set of feature points on the first image and points on the second image corresponding to each other between images is maximized is determined. By estimating the change in position and orientation, it is possible to estimate the change in position and orientation of the image capturing apparatus with high accuracy with a simple configuration.

  In the above embodiment, the case where the corner strength is used as the feature amount indicating the degree of change in luminance with the surrounding points is described as an example. However, the present invention is not limited to this. The feature amount indicating the degree of change in the density or luminance of the image may be used. For example, the edge or the second derivative is calculated by calculating the edge or the second derivative that is the feature amount indicating the degree of change in the density or the luminance with the surrounding points. A larger weighting factor may be determined as the amount increases.

  Next, a motion estimation apparatus according to the fourth embodiment will be described. In addition, since the structure of the motion estimation apparatus which concerns on 4th Embodiment is the same as that of 1st Embodiment, it attaches | subjects the same code | symbol and abbreviate | omits description.

  The fourth embodiment is different from the first embodiment in that a weighting coefficient of a feature point is determined depending on whether images in a peripheral area of corresponding points in two images are similar.

  In the motion estimation apparatus according to the fourth embodiment, in the feature point weight determination unit 34, the small region around the feature point on the first image obtained by the corresponding point search unit 32 and the second image For example, a value of SSD (Sum of Squared Distances), which is a value obtained by summing squares of luminance differences for each pixel in the small area, is calculated as an index indicating luminance similarity with respect to the small area around the corresponding point. A smaller weighting factor is determined as the SSD value is larger, and a larger weighting factor is determined as the SSD value is smaller.

  For example, as shown in FIGS. 6A and 6B, when the feature point p of the first image and the point p ′ of the second image are detected as corresponding points, they are set around the feature point p. The SSD is calculated based on the brightness of the small area w and the brightness of the small area w ′ having the same size set around the corresponding point p ′. Here, if the feature point of the first image and the corresponding point of the second image correspond to each other correctly between the two images, the change in the surrounding pattern is also small, so the SSD value is small. Therefore, if the SSD value is small, the small area around the feature point of the first image is similar to the small area around the point of the corresponding second image, and it is highly possible that the correspondence is correct. Since the reliability of correspondence is high, a large weighting factor is determined. On the other hand, if the calculated SSD value is large, the small area around the feature point of the first image is not similar to the small area around the point of the corresponding second image, and the correspondence is incorrect. Since the reliability of correspondence is low, a small weighting factor is determined. Thereby, it is possible to reduce the weight coefficient of the feature point that has a high possibility of erroneous correspondence.

  In the motion estimation processing routine according to the fourth embodiment, first, a first image and a second image are acquired from the image capturing device 12, and a plurality of feature points are extracted from the first image. A point corresponding to each of the plurality of feature points on one image is searched and tracked on the second image, and the image coordinates of the point on the second image corresponding to each feature point are detected.

Then, for the extracted N feature points p _i (i = 1 to N), a small area around the feature point p _i of the first image is extracted, and the second corresponding to the feature point p _i is extracted. Extract a small area around a point in the image. Then, based on the first second luminance small region around the point of images around the small region of luminance, and extraction of the feature point p _i of the images extracted to calculate the SSD, it is calculated depending on the magnitude of the value of the SSD, to determine the weight coefficient w _i for the feature point p _i.

Then, eight sets of corresponding points are selected at random from the N sets of corresponding points, and the elements f _{11 to} f ₃₂ of the matrix F are calculated from the image coordinates of the selected eight sets of corresponding points. _A matrix F _r is generated from _{11 to} f ₃₂ . Next, based on the generated matrix F _r , the image coordinates of the N corresponding points, and the determined weighting factor of each feature point, the first image and the second image when the matrix F _r is applied calculating the sum Z _r of the weighting coefficients of the set of feature points of the corresponding points between the images.

When the process of calculating the weight coefficient sum Z _r from the eight randomly selected points is repeated a predetermined number of times T, a matrix F _r that maximizes the calculated weight coefficient sum Z _r is obtained. Based on the identified matrix _Fr , the movement amount of the movement of the image pickup device 12 in the XYZ axis direction and the rotation amount based on the XYZ axis are calculated.

  Thus, for each extracted feature point, the smaller the SSD value based on the small region around the feature point on the first image and the small region around the corresponding point on the second image, the smaller the feature point. When a large weighting factor is determined and a matrix indicating changes in the position and orientation of the image capturing device is applied, the corresponding feature points on the first image and the second between the first image and the second image By specifying a matrix indicating a change in position and orientation at which the sum of weight coefficients of a pair with a point on the image of the image becomes the maximum, and estimating a change in the position and orientation of the image capturing device, with a simple configuration, Changes in the position and orientation of the image capturing apparatus can be estimated with high accuracy.

  In the above-described embodiment, the case where the SSD is used as an index indicating whether or not the images of the peripheral areas of corresponding points in the two images are similar is described. However, the present invention is not limited to this. Instead of this, another index indicating the similarity between the images in the peripheral areas of the corresponding points in the two images may be used. In this case, based on the obtained index, a weighting factor that is so large that the images in the peripheral areas of corresponding points in the two images are similar may be determined for the feature points.

  In the first to fourth embodiments, the case where the point corresponding to the feature point of the first image is searched from the second image has been described as an example. It is also possible to extract feature points from the first image and search for points corresponding to the feature points of the first image from the feature points on the second image.

  In addition, when the matrix F is applied, a matrix F that maximizes the sum of the weighting factors for the feature points of the pair of corresponding points between the first image and the second image is identified, and the identified matrix F However, the present invention is not limited to this. For example, when the matrix F is applied, the first image and the second image The matrix F in which the sum of the weighting coefficients of the feature points of the pair of corresponding points is greater than or equal to a predetermined value is specified, and the change in the position and orientation of the image pickup device is obtained using the specified matrix F Also good.

  The program according to the present invention may be provided by being stored in a storage medium such as a CDROM.

It is the schematic which shows the structure of the motion estimation apparatus which concerns on the 1st Embodiment of this invention. It is an image figure which shows a mode that the number of the feature points which exist in the window centering on the feature point is counted. It is a figure for demonstrating the translation vector and rotation matrix showing the change of a position and orientation. It is a flowchart which shows the content of the motion estimation process routine in the motion estimation apparatus which concerns on the 1st Embodiment of this invention. It is an image figure which shows a mode that the distance with the other feature point which exists around a feature point is calculated | required. (A) The image figure which shows a mode that the small area | region around the feature point on a 1st image is extracted, (B) The image figure which shows a mode that a small area | region around the corresponding point on a 2nd image is extracted. is there.

Explanation of symbols

DESCRIPTION OF SYMBOLS 10 Motion estimation apparatus 12 Image pick-up apparatus 14 Computer 30 Feature point extraction part 32 Corresponding point search part 34 Feature point weight determination part 36 Motion estimation part

Claims (8)

An imaging means attached to the moving body for imaging a predetermined area;
Feature point extracting means for extracting a plurality of feature points from a first image captured when the imaging means is in a first position and a first posture;
A point corresponding to each of the feature points extracted from the first image by the feature point extraction unit is a second position and a second point where the imaging unit is different from the first position and the first posture. Search means for searching from a second image captured when in a posture;
For each of the feature points of the first image, a weighting factor determination unit that determines a larger weighting factor as the number of the feature points existing in a predetermined region including the feature point of the first image decreases.
A change in the position and orientation of the imaging means estimated based on a plurality of sets of the corresponding feature point of the first image and the point of the second image, the position and orientation being changed Sometimes the change in position and orientation is such that the sum of the weighting factors of the set of feature points and points corresponding between the first image and the second image is greater than or equal to a predetermined value or a maximum value. Estimating means for estimating a change in position and orientation of the imaging means between the first position and the first attitude and the second position and the second attitude;
A motion estimation device including: An imaging means attached to the moving body for imaging a predetermined area;
Feature point extracting means for extracting a plurality of feature points from a first image captured when the imaging means is in a first position and a first posture;
A point corresponding to each of the feature points extracted from the first image by the feature point extraction unit is a second position and a second point where the imaging unit is different from the first position and the first posture. Search means for searching from a second image captured when in a posture;
For each of the feature points of the first image, a weighting factor determination unit that determines a larger weighting factor as the distance between the feature point of the first image and another feature point is longer;
A change in the position and orientation of the imaging means estimated based on a plurality of sets of the corresponding feature point of the first image and the point of the second image, the position and orientation being changed Sometimes the change in position and orientation is such that the sum of the weighting factors of the set of feature points and points corresponding between the first image and the second image is greater than or equal to a predetermined value or a maximum value. Estimating means for estimating a change in position and orientation of the imaging means between the first position and the first attitude and the second position and the second attitude;
A motion estimation device including: An imaging means attached to the moving body for imaging a predetermined area;
Feature point extracting means for extracting a plurality of feature points from a first image captured when the imaging means is in a first position and a first posture;
A point corresponding to each of the feature points extracted from the first image by the feature point extraction unit is a second position and a second point where the imaging unit is different from the first position and the first posture. Search means for searching from a second image captured when in a posture;
For each of the feature points of the first image, a larger weighting factor is determined as the feature amount indicating the degree of change in density or luminance between the feature points of the first image and surrounding points is larger. A weight coefficient determination means;
A change in the position and orientation of the imaging means estimated based on a plurality of sets of the corresponding feature point of the first image and the point of the second image, the position and orientation being changed Sometimes the change in position and orientation is such that the sum of the weighting factors of the set of feature points and points corresponding between the first image and the second image is greater than or equal to a predetermined value or a maximum value. Estimating means for estimating a change in position and orientation of the imaging means between the first position and the first attitude and the second position and the second attitude;
A motion estimation device including: An imaging means attached to the moving body for imaging a predetermined area;
Feature point extracting means for extracting a plurality of feature points from a first image captured when the imaging means is in a first position and a first posture;
A point corresponding to each of the feature points extracted from the first image by the feature point extraction unit is a second position and a second point where the imaging unit is different from the first position and the first posture. Search means for searching from a second image captured when in a posture;
For each of the feature points of the first image, an image of a predetermined area including the feature points of the first image and a point of the second image corresponding to the feature points of the first image. A weighting factor determining means for determining a weighting factor that is larger as the image of the predetermined area including the image is similar;
A change in the position and orientation of the imaging means estimated based on a plurality of sets of the corresponding feature point of the first image and the point of the second image, the position and orientation being changed Sometimes the change in position and orientation is such that the sum of the weighting factors of the set of feature points and points corresponding between the first image and the second image is greater than or equal to a predetermined value or a maximum value. Estimating means for estimating a change in position and orientation of the imaging means between the first position and the first attitude and the second position and the second attitude;
A motion estimation device including: Computer
A feature point extraction unit that extracts a plurality of feature points from a first image captured when the imaging unit attached to the moving body captures a predetermined area is in the first position and the first posture;
A point corresponding to each of the feature points extracted from the first image by the feature point extraction unit is a second position and a second point where the imaging unit is different from the first position and the first posture. Search means for searching from a second image captured when in a posture;
For each of the feature points of the first image, a weighting factor determination unit that determines a larger weighting factor as the number of the feature points existing in a predetermined region including the feature point of the first image decreases, and A change in the position and orientation of the imaging means estimated based on a plurality of sets of the corresponding feature point of the first image and the point of the second image, the position and orientation being changed Sometimes the change in position and orientation is such that the sum of the weighting factors of the set of feature points and points corresponding between the first image and the second image is greater than or equal to a predetermined value or a maximum value. A program for functioning as an estimation unit that estimates a change in position and orientation of the imaging unit between the first position and the first posture and the second position and the second posture. Computer
A feature point extraction unit that extracts a plurality of feature points from a first image captured when the imaging unit attached to the moving body captures a predetermined area is in the first position and the first posture;
A point corresponding to each of the feature points extracted from the first image by the feature point extraction unit is a second position and a second point where the imaging unit is different from the first position and the first posture. Search means for searching from a second image captured when in a posture;
For each of the feature points of the first image, a weighting factor determination unit that determines a larger weighting factor as the distance between the feature point of the first image and another feature point is longer, and the corresponding first Change in the position and orientation of the imaging means estimated based on a plurality of sets of the feature points of the image and the points of the second image when the position and orientation change A change in the position and orientation at which the sum of the weighting factors of the set of the feature points and the points corresponding to each other between the image and the second image is equal to or greater than a predetermined value or a maximum value; A program for functioning as an estimation unit that estimates a change in the position and orientation of the imaging unit between a position and the first orientation and the second position and the second orientation. Computer
A feature point extraction unit that extracts a plurality of feature points from a first image captured when the imaging unit attached to the moving body captures a predetermined area is in the first position and the first posture;
A point corresponding to each of the feature points extracted from the first image by the feature point extraction unit is a second position and a second point where the imaging unit is different from the first position and the first posture. Search means for searching from a second image captured when in a posture;
For each of the feature points of the first image, a larger weighting factor is determined as the feature amount indicating the degree of change in density or luminance between the feature points of the first image and surrounding points is larger. A change in position and orientation of the imaging means estimated on the basis of a plurality of sets of weight coefficient determination means and the corresponding feature points of the first image and points of the second image, When the position and orientation change, the sum of the weighting factors of the set of feature points and points corresponding between the first image and the second image is greater than or equal to a predetermined value or a maximum value. A function for estimating a change in position and orientation as a change in the position and orientation of the imaging means between the first position and the first orientation and the second position and the second orientation Program to let you. Computer
A feature point extraction unit that extracts a plurality of feature points from a first image captured when the imaging unit attached to the moving body captures a predetermined area is in the first position and the first posture;
A point corresponding to each of the feature points extracted from the first image by the feature point extraction unit is a second position and a second point where the imaging unit is different from the first position and the first posture. Search means for searching from a second image captured when in a posture;
For each of the feature points of the first image, an image of a predetermined area including the feature points of the first image and a point of the second image corresponding to the feature points of the first image. A weighting factor determining unit that determines a weighting factor that is larger as the image of the predetermined region is more similar, and an estimation based on a plurality of sets of the feature point of the corresponding first image and the point of the second image A change in the position and orientation of the imaging means, and a set of the feature point and the point corresponding to each other between the first image and the second image when the position and orientation change The change in the position and posture at which the sum of the weighting coefficients is equal to or greater than a predetermined value or the maximum value is determined between the first position and the first posture and the second position and the second posture. Estimating means for estimating changes in position and orientation of the imaging means Program to function.

Images