JP2010127819A - Device of detecting position of polyhedral body and method for detection - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a device of detecting a position of a three-dimensional object capable of precisely recognizing a position and an attitude of an object even when the object has a glossy face, a face without a pattern or a black face, and to provide a method for detection. <P>SOLUTION: Model images of respective faces forming an object to be a measuring target are stored in a model image storage section. An image capturing device captures images of the measuring target as multi-viewpoint images. The imaged three-dimensional information is stored in an image information storage section. A plane face extracting section extracts a plane face from the stored three-dimensional image information. A normal line image conversion section converts the extracted plane face into a normal line image by using a first conversion matrix. A model correspondence image conversion section performs pattern matching between the converted normal line image and each of model images and determines a correspondence model image so as to convert the determined model image into a model correspondence image on the basis of a second conversion matrix. The position and the attitude of the target are recognized on the basis of the operated first and second conversion matrixes. <P>COPYRIGHT: (C)2010,JPO&INPIT

Description

  The present invention relates to a polyhedron position detection device and a detection method for imaging a target object including a plurality of planar portions as a multi-viewpoint image with an imaging device and detecting the posture and position of the target object from the captured multi-viewpoint image. .

  As a conventional apparatus for recognizing the three-dimensional position / orientation of a target object, for example, a shape of a cylindrical object from one viewpoint is measured by a three-dimensional measuring apparatus (laser scanner), and the point group measured by the three-dimensional measuring apparatus is used. A normal vector related to a plurality of arbitrary points in the data is calculated, and a direction (vector) of the rotation axis of the cylindrical object is calculated by the rotation axis straight line calculation unit based on the plurality of normal vectors. A three-dimensional object position detection device has been proposed in which a point through which a rotation axis line passes on a coordinate plane is obtained to obtain a rotation axis line, and a position of a cylindrical object is specified by a target position specifying unit based on the rotation axis line. (For example, refer to Patent Document 1).

In addition, a method has been proposed in which a measurement target object is imaged with stereo vision, and a position / orientation is recognized from a distance image of the captured measurement target object (see, for example, Patent Documents 2 to 4).
Further, there is also an image processing apparatus that recognizes the posture of a measurement object by associating a two-dimensional image captured from one camera with a two-dimensional image obtained by photographing a plurality of postures assumed in advance by two-dimensional pattern matching. It has been proposed (see, for example, Patent Document 5).
JP 2006-139713 A Japanese Patent No. 2961264 Japanese Patent Laid-Open No. 2007-183908 Japanese Patent Laid-Open No. 2001-143073 JP 2004-333422 A

  As described above, in the conventional examples described in Patent Documents 1 to 5 for recognizing the three-dimensional position / orientation of an object, roughly classified, from the three-dimensional point cloud data of the object measured by a laser scanner. A method for recognizing position / posture, a method for recognizing position / posture from a distance image of an object measured by stereo vision, and a two-dimensional image taken from one camera and photographing a plurality of presumed postures. There is a method of recognizing the posture by associating the two-dimensional images made by two-dimensional pattern matching.

  Here, in the method of recognizing the position / orientation from the 3D point cloud data of the object measured by the laser scanner, it is difficult to measure the 3D point cloud data by the laser scanner, such as a glossy surface (metal etc.). It is difficult to measure point cloud data for objects that reflect laser light with high reflectivity, such as), or objects that absorb laser light, such as black, so that position and orientation cannot be recognized. There are unresolved issues.

  Further, in the method of recognizing the position / orientation from the distance image of the object measured by stereo vision, the object is difficult to measure the distance by stereo vision, for example, it is photographed like a glossy surface (metal etc.). When the pattern of the target object is not the pattern of the target object itself but the surrounding pattern that appears on the target object, and there is no correspondence between multiple images, or there is no pattern and the correspondence between multiple images cannot be taken However, since it is difficult to create a distance image, there is an unsolved problem that position / posture recognition cannot be performed. Such an object can measure the distance of an edge portion or the like, but it is difficult to perform position / posture recognition based only on the edge portion because the amount of information is small.

  Moreover, in the above two methods, 3D feature values are extracted from measurement data (3D point cloud data and distance data), a 3D model, a 3D feature dictionary, and 3D features are prepared in advance. This is a method of performing position / posture recognition by obtaining a plurality of three-dimensional corresponding points between measurement data and a model by performing quantity matching, and obtaining a transformation matrix relating the two. For example, in Patent Document 2, a three-dimensional geometric feature and a three-dimensional contour line generated by extracting and segmenting edges are used as feature values, and in Patent Document 3, a corner of an object is used as a three-dimensional feature value. In 4, the straight line group is detected from the edge, and the intersection of the straight line group is used as the feature amount. Since these feature quantities are based on the premise that the 3D point cloud data and the distance image are correctly measured, there is a common unresolved problem that the 3D point cloud data and the distance image cannot be recognized if they cannot be measured. is there.

  In a method of recognizing a posture by associating a two-dimensional image photographed from one camera with a two-dimensional image obtained by photographing a plurality of postures assumed in advance by a two-dimensional pattern match, a laser scanner or stereo vision is used. Since distance measurement is not performed, an object having no glossy surface or pattern can be photographed, and posture recognition is possible. However, since the position (distance) of the object is estimated from the magnitude relationship of the images, there is an unsolved problem that it is difficult to detect the position (distance) with high accuracy. In addition, there is an unsolved problem that an enormous amount of images must be prepared as two-dimensional images obtained by photographing a plurality of assumed postures.

  Therefore, the present invention has been made paying attention to the above-mentioned unsolved problems of the conventional example, and the position / posture can be set even when the object has a glossy surface, no pattern, or a black object. It is an object of the present invention to provide a three-dimensional object position detection apparatus and detection method that can be recognized with high accuracy.

  In order to achieve the above object, a polyhedron position detection device according to one aspect stores a model image that stores in advance, as a model image, an image captured from the normal direction of each surface constituting a known object to be measured. A storage unit; an imaging device that captures the object as a multi-viewpoint image and outputs three-dimensional image information; and an image information storage unit that stores the three-dimensional image information output from the imaging device. Then, a plane is extracted by the plane extraction unit based on the three-dimensional image information stored in the image information storage unit. Next, in the normal direction image conversion unit, the position of the object in the normal direction image which is an image from the normal direction of the plane obtained by extracting the position and orientation of the object of the three-dimensional image information by the plane extraction unit Then, a first parameter representing a parameter to be converted into a posture is obtained, and the image of the three-dimensional image information is converted into a normal direction image. The normal image converted by the normal image conversion unit and each model image stored in the model image storage unit are compared by the model image determination unit to determine a corresponding model image. The model corresponding image conversion unit obtains a second parameter representing the position and orientation of the object at the corresponding position in the normal direction image of the model image determined by the model image determining unit, and the model image is determined as the corresponding position. Is converted into a model-corresponding image, which is an image at. Then, the position and orientation recognition unit recognizes the position and orientation of the object measured based on the calculated first and second parameters.

  Further, the polyhedron position detection device according to another aspect includes a model image storage unit that stores in advance, as a model image, an image captured from a normal direction of each surface constituting a known object to be measured, and the target An imaging apparatus that captures an object as a multi-viewpoint image and outputs three-dimensional image information, and an image information storage unit that stores the three-dimensional image information output from the imaging apparatus are provided. Then, a plane is extracted by the plane extraction unit based on the three-dimensional image information stored in the image information storage unit. Next, in the normal direction image conversion unit, the position of the object in the normal direction image which is an image from the normal direction of the plane obtained by extracting the position and orientation of the object of the three-dimensional image information by the plane extraction unit Then, a first parameter representing a parameter to be converted into a posture is obtained, and the image of the three-dimensional image information is converted into a normal direction image. The normal image converted by the normal image conversion unit and each model image stored in the model image storage unit are compared by the model image determination unit to determine a corresponding model image. The model corresponding image conversion unit obtains a second parameter representing the position and orientation of the object at the corresponding position in the normal direction image of the model image determined by the model image determining unit, and the model image is determined as the corresponding position. Is converted into a model-corresponding image, which is an image at. The error calculation unit uses the image based on the model image and the image based on the three-dimensional image information, and obtains a third parameter representing an error correction amount between the two by iterative processing. Then, the position and orientation recognition unit recognizes the position and orientation of the object measured based on the calculated first, second, and third parameters.

  Furthermore, in the polyhedral position detection method according to one aspect, each surface constituting the object is imaged using an imaging device that images a known object to be measured as a multi-viewpoint image and outputs three-dimensional image information. A model image storing step for storing in advance an image captured from the normal direction of the image as a model image in a model image storage unit, an image information storing step for storing three-dimensional image information output from the imaging device, and A plane extraction step for extracting a plane from the three-dimensional image information, and a normal direction image that is an image from the normal direction of the plane extracted by the plane extraction unit with the position and orientation of the object of the three-dimensional image information. A normal direction image conversion step of obtaining a first parameter representing a parameter to be converted into the position and orientation of the object and converting the image of the three-dimensional image information into a normal direction image A model image determination step for determining a corresponding model image by comparing the converted normal direction image and the model image stored in the model image storage step, and the method of the model image determined in the model image determination step A model-corresponding image conversion step for obtaining a second parameter representing the position and orientation of the object at the corresponding position in the line direction image, and converting the model image into a model-corresponding image that is an image at the corresponding position; A position and orientation recognition step for recognizing the position and orientation of the object measured based on the first and second parameters.

  Further, in the polyhedral position detection method according to another embodiment, each surface constituting the object is imaged using an imaging device that images a known object to be measured as a multi-viewpoint image and outputs three-dimensional image information. A model image storing step for storing in advance an image captured from the normal direction of the image as a model image in a model image storage unit, an image information storing step for storing three-dimensional image information output from the imaging device, and A plane extraction step for extracting a plane from the three-dimensional image information, and a normal direction image that is an image from the normal direction of the plane extracted by the plane extraction unit with the position and orientation of the object of the three-dimensional image information. A normal direction image conversion step of obtaining a first parameter representing a parameter to be converted into the position and orientation of the object, and converting the image of the three-dimensional image information into a normal direction image; A model image determining step for determining a corresponding model image by comparing the converted normal direction image and each model image stored in the model image storing step, and the method of the model image determined in the model image determining step A model-corresponding image conversion step of obtaining a second parameter representing the position and orientation of the object at the corresponding position in the line direction image and converting the model image into a model-corresponding image that is an image at the corresponding position; Using the image based on the image and the image based on the three-dimensional image information, an error calculating step for obtaining a third parameter representing the correction amount of the error between the two by repetitive processing, and the calculated first, second and third A position and posture recognition step for recognizing the position and posture of the object measured based on these parameters.

  According to the present invention, a normal direction image is obtained by specifying a target object as an object having a plurality of planes and calculating a first parameter based on three-dimensional image information output from an imaging device that captures a multi-viewpoint image. And the converted normal direction image and each stored model image are compared to determine a corresponding model image, and the second parameter is determined from the corresponding position of the determined model image in the normal direction image. By converting the model image into a model-corresponding image and recognizing the position and orientation of the object based on the first and second parameters, the object can be obtained with fewer three-dimensional measurement points that cannot be recognized by the three-dimensional feature. This has the effect that the position / posture of an object can be detected.

  Further, a third parameter representing an error between the two is obtained by iterative processing using the image based on the determined model image and the image based on the three-dimensional image information, and the object is obtained based on the first to third parameters. The position and orientation recognition errors due to various conversions, measurement errors, or quantum errors can be corrected with high accuracy.

Hereinafter, embodiments of the present invention will be described with reference to the drawings.
1 is a system configuration diagram of the present invention, FIG. 2 is a block diagram showing a specific configuration of the arithmetic processing apparatus of FIG. 1, and FIG. 3 is a perspective view of an object to be measured.
In FIG. 1, reference numeral 1 denotes stereo vision as an imaging device that captures multi-viewpoint images. In this stereo vision 1, the correspondence of each pixel is searched between stereo images obtained by capturing an object with a pair of cameras having two or more known relative positional relationships, and a multi-viewpoint image is obtained by the principle of triangulation. Three-dimensional image information as information is restored. The three-dimensional image information of the object imaged by the stereo vision 1 is supplied to the arithmetic processing unit 2 constituted by, for example, a microcomputer.

  The arithmetic processing device 2 is connected to a model image database 3 and a display device 4 as a model image storage unit. In the model image database 3, each known object to be measured, for example, an object composed of a hexahedron having at least six plane parts on the top, bottom, left, and right is imaged with one camera from the normal direction of each plane part. 24 types of normal direction image information as model image information is obtained by combining the two-dimensional normal direction image information and the image information rotated by 90 degrees, 180 degrees, and 270 degrees for each of the two-dimensional normal direction image information. It is remembered. Note that the two-dimensional normal direction image information includes information representing the position and orientation of the object in the three-dimensional coordinate system at the time of imaging.

In addition, as shown in FIG. 2 in a functional block diagram, the arithmetic processing device 2 includes a three-dimensional measurement unit 21, an information storage unit 22 including an image information storage unit, a point / edge selection unit 23, a plane extraction unit 24, A normal direction image conversion unit 25, a model-corresponding image generation unit 26, a correction conversion matrix calculation unit 27 as an error calculation unit, and a conversion matrix calculation unit 28 are provided.
Here, it is assumed that the condition of the object in the present embodiment is a polyhedron that is mainly configured by a flat portion and an angle formed by the flat portion is known. Examples of the polyhedron include a cube, a rectangular parallelepiped, a polygonal column, a polygonal pyramid, a regular polyhedron, a truncated polyhedron, and an oblique polyhedron. Each surface of the polyhedron need not be a complete plane, but is a plane portion. A plane part means the case where a protrusion, a hole, an unevenness | corrugation, etc. exist with respect to the plane used as a foundation. The known angle formed by the plane portion means that the angle formed by the edge portion detected by the edge detection is known in a two-dimensional image captured from the normal direction of the plane of the object.

  Furthermore, the definition of 3D position / posture recognition is as follows. For example, when the origin of the three-dimensional coordinate system is the imaging origin of stereovision 1, the position and orientation of the imaged object is a three-dimensional rigid transformation of the object located at the origin of the three-dimensional coordinate system. It is. Therefore, obtaining this rigid body transformation matrix (degree of freedom of rotation 3 and degree of freedom of translation 3) is three-dimensional position / posture recognition. Note that the three-dimensional coordinate system is not limited to the one having the imaging origin of the stereo vision 1 as the origin. That is, the rigid body transformation matrix is an expression form for recognizing the three-dimensional position / orientation of the object, and is not limited to the rigid body transformation matrix as long as the three-dimensional position / orientation of the object can be represented as a parameter. Therefore, it is not necessary to express with three degrees of freedom of rotation and three degrees of freedom of parallel movement, and may be expressed with mathematical expressions such as equations.

  In the three-dimensional measurement by the stereo vision 1, first, distance measurement is performed for points that can be distance-measured by stereo vision, which is a known technique. The purpose is to measure a small number of points with high accuracy, not to measure a dense three-dimensional shape such as three-dimensional point cloud data or distance images, even for objects without metal surfaces or patterns. It is possible to measure a small number of points that are edges constituting the plane.

  When the image information reading request is input from an input device such as a keyboard or a mouse, the three-dimensional measuring unit 21 operates the stereo vision 1 to form the rectangular parallelepiped protrusion 11 on the upper surface of the rectangular parallelepiped 10 shown in FIG. The three-dimensional image information based on the multi-viewpoint image information obtained by imaging the target object 12 is read, and the read three-dimensional image information is stored in the three-dimensional image information storage area of the information storage unit 22. Here, the multi-viewpoint image captured by the stereo vision 1 is as shown in FIG.

The information storage unit 22 has a three-dimensional image information storage area for storing the three-dimensional image information measured by the three-dimensional measurement unit 21, and the calculation result in the plane extraction unit 24, the normal direction image conversion unit 25, and the model correspondence The calculation results of the image generation unit 26 and the correction conversion matrix calculation unit 27 are stored.
The point / edge selection unit 23 searches for corresponding points using a known stereo vision image processing technique based on the three-dimensional image information read by the three-dimensional measurement unit 21, and the distances (three-dimensional coordinates) of a plurality of points. Ask for. Here, the points for obtaining the distance are desirably uniform on each plane, but may be a sufficient number for detecting the plane, and may be several. Further, the edge of each plane is obtained, and the line segment is obtained by a known technique such as Hough transform. That is, as shown in FIG. 5, the point / edge selection unit 23 three-dimensionally measures the point indicated by ● and the straight line of the edge indicated by a thick line.

The plane extraction unit 24 includes a normal vector calculation unit 24a, a plane detection unit 24b, and a plane selection unit 24c.
In the normal vector calculation unit 24a, since the points and line segments selected by the point / edge selection unit 23 have three-dimensional coordinates, the method of the plane including each point represented by these three-dimensional coordinates is included. Calculate the line vector. Since the calculation of the normal vector has a small number of measurement points, it is difficult to obtain a plane by the method of least squares. Therefore, as shown in FIG. 6, first, a point ▲ serving as a reference point and two neighboring points are used. ● is selected, and the outer product of two vectors determined by the reference point ▲ and the two points ● is defined as a normal vector. Next, normal vectors are obtained for all combinations of the reference point ▲ and the other two points. The normal vector having the largest frequency is adopted as the normal vector of the plane including the reference point ▲ from the obtained plurality of normal vectors.

  Specifically, as shown in FIG. 7, a three-dimensional vector (xi, yi, zi) is defined as (θi, φi), which is an inclination angle θi on the XY plane and an inclination angle φi with respect to the Z axis from the XY plane. To a two-dimensional representation. Here, θ is 0 to 360 degrees, and φ is 0 to 180 degrees. Generality is not lost by making all the vectors upward (Z> 0). All vectors (xi, yi, zi) are divided into predetermined angles (for example, 5 degree sections) shown in FIG. 8, and the two-dimensional frequency distribution of θ and φ is counted. In the counted two-dimensional frequency distribution, the normal vector belonging to the section with the highest frequency is adopted as the normal vector of the plane including the reference point.

The plane detection unit 24b detects the normal vector having the highest frequency from the normal vectors of the plane including the reference points calculated by the normal vector calculation unit 24a. The points constituting the extracted normal vector are a set of points that may constitute a plane.
When the normal vector with the highest frequency is (a, b, c), the plane equation is
ax + by + cz + d = 0
However, d is not obtained at this stage.

  The plane selection unit 24c calculates −d by the plane equation ax + by + cz = −d for all points (xi, yi, zi) constituting the normal vector extracted by the plane detection unit 24b, and the frequency of −d A distribution is created, and the points belonging to the category with the highest frequency are determined as a set of “points belonging to a plane” to be obtained. This is a plane having the largest number of points belonging to the group of parallel planes.

  Specifically, as shown in FIG. 9A, when the plane equation is calculated, the constant d is, for example, d1, d2, and d3, and five points belonging to the plane d1 belong to the plane d2. When the number of points is two and the number of points belonging to d3 is three, the frequency distribution has the largest frequency of the constant d1, as shown in FIG. 9B, so that a plane expressed as ax + by + cz + d1 = 0 is extracted. Is done.

  The normal direction image conversion unit 25 determines the position / posture of the object of the 3D image information captured by the stereo vision 1 in the image from the normal direction of the plane extracted by the plane extraction unit 24. A conversion matrix as a first parameter that is converted into a posture is obtained, and the measurement image is converted into a normal direction image using this conversion matrix. The normal direction image refers to an image when viewed from the normal direction of the plane extracted by the plane extraction unit. This will be specifically described below.

  First, the normal direction image conversion matrix (P) calculation unit 25a will be described. As shown in FIGS. 10A and 10B, in the two-dimensional coordinate system on the two-dimensional image, two vectors V1 and V2 formed by arbitrary three extracted points on the same plane before and after the conversion, Any one vector V3 other than the extracted three points can be uniquely indicated by a linear combination of the vectors V1 and V2 and is represented identically before and after the conversion. By using this condition, all the points on the extracted plane in the image of the three-dimensional image information shown in FIG. 10A become points in the normal direction image as shown in FIG. Association of two-dimensional coordinates can be obtained.

  Considering that an image of 3D image information is converted into a normal direction image, a matrix representing this conversion is a perspective conversion matrix. Therefore, obtaining the perspective transformation matrix means obtaining a transformation matrix for converting the position / orientation of the object in the measured three-dimensional image information into the position / orientation of the object in the normal direction image. The normal direction image transformation matrix P as the first parameter can be obtained by using the above-described associated points and using, for example, a well-known technique such as a DLT (Direct Linear Transform) method.

  A normal direction image conversion matrix P for converting the coordinates (X0, Y0, Z0) to the coordinates (X1, Y1, Z1) is expressed by the following equation. In normal direction conversion, the input uses (X0, Y0, Z0), and the output side uses (X1, Y1, Z1).

Here, in order to calculate the normal direction image transformation matrix P by the DLT method, when a point correspondence {x _i ⇔x _i ′} before and after the two-dimensional transformation is given, x _i ′ = Hx _i. A plane projection transformation matrix H is obtained. Here, the matrix H is a matrix equal to the matrix P.
The meaning of the equation x _i '= Hx _i indicates that x _i ' and Hx _i are the same direction vectors having different sizes. That is, it is equivalent to x _i ′ × Hx _i = 0.

Therefore, first, the following matrix A _i is calculated for each point corresponding x _i ⇔x _i ′.

Note that the third line of the example line A _{i in} Equation 2 is a linear combination of the first and second lines. Therefore, the matrix A _i can be handled substantially as a 2 × 9 matrix.
Next, when the number of points corresponding to n is n, n 2 × 9 matrices A _i are obtained to form a 2n × 9 matrix A.

The matrix A is subjected to singular value decomposition, and the singular vector corresponding to the minimum singular value is set to h.
h = (h11, h12, h13, h21, h22, h23, h31, h32, h33)

  The normal direction image conversion unit 25 performs the above-described calculation of Equation 1 for each point using the normal direction image conversion matrix P obtained by the normal direction image conversion matrix (P) calculation unit 25a, and based on the calculation result. The normal direction image information is generated, and the generated normal direction image information is displayed on the display device 4 and stored in the information storage unit 22.

  Here, the plane extracted as the same plane in the normal direction image is converted into a correct shape when viewed from the normal direction of the extracted plane, but the other planes are in a deformed state that is not actually possible. There is no problem. Further, as described above, the point / edge selection unit 23 obtains a line segment from each edge, and these line segments should have known angles. For this reason, when obtaining the association of the two-dimensional coordinates, the association is selected as shown in FIG. 11 by selecting the association so that the line segment having a known angle has a direction that can be taken in the normal direction image. An accurate normal direction image can be obtained for the plane, and an inappropriate normal direction image can be prevented as shown in FIG.

The model corresponding image generation unit 26 includes a model image matching unit 26a as a model image determination unit and a rigid body transformation matrix (T1) calculation unit 26b as a model corresponding image conversion unit.
The model image matching unit 26a is known between the normal direction image information generated by the normal direction image conversion unit 25 and the model image information of each surface shown in FIG. 13 stored in the model image database 3. Two-dimensional pattern matching processing is performed to determine corresponding model image information.

  Here, the image size (one pixel size) of the model image information and the image size of the normal direction image information are adjusted to be equal. That is, as shown in FIG. 14A, when the size of one pixel of the model image information stored in the model image database 3 is a (millimeter), the actual size of the object is A (millimeter). In this case, the number of pixels on the image is A / a pixels.

As shown in FIG. 14B, the three-dimensional image information before the normal direction image conversion is expressed by the three-dimensional distance between the two-dimensional coordinates (αx1, αy1) and (αx2, αy2) of any two feature points. Known. If this three-dimensional distance is B (millimeters), the number of pixels in the normal direction image is not necessarily B / a pixels.
Therefore, in the normal direction image information after the normal direction image conversion, as shown in FIG. 14C, the feature points corresponding to the two-dimensional coordinates (αx1, αy1) and (αx2, αy2) of the feature points are shown. The normal direction image is determined so that the number of pixels of the three-dimensional distance B (millimeter) between the two-dimensional coordinates (βx1, βy1) and (βx2, βy2) becomes B / a pixels.

Therefore, when the feature point coordinates (βxi, βyi) after normal direction image conversion corresponding to the feature point coordinates (αxi, αyi) before conversion are obtained, the feature points are set so that the number of pixels becomes B / a pixels. By setting the two-dimensional coordinates to γ (βxi, βyi), the normal direction image information and the model image information can be compared and pattern matched with the same size.
Since the rigid body transformation matrix (T1) calculation unit 26b knows where the model image information determined by the model image matching unit 26a exists in the two-dimensional image, the rigid body transformation matrix (T1) calculation unit 26b determines the position and orientation of the object in the model image information. Based on this, the position / posture of the object at the matching position can be grasped. Therefore, it is possible to obtain a rigid body transformation matrix as a second parameter for converting the object at the origin of the three-dimensional coordinate system into the position / posture of the object at the matching position. The three-dimensional coordinates (X1, Y1, Z1) of the object at the matching position with respect to the three-dimensional coordinates (Xorg, Yorg, Zorg) of the object at the origin of the three-dimensional coordinate system can be expressed by the following equations.

Here, a specific example of the calculation method of the rigid transformation matrix is that the input (number of points = N) is
A set of points on the target (three-dimensional coordinates): {P′i}, P′i = (p′i (x), p′i (y), p ′ (z))
Corresponding points on the model: {Pi}, Pi = (pi (x), pi (y), pi (z))
The problem is to obtain a rigid transformation matrix that transforms {P′i} into {Pi}.

  In order to obtain a least square solution, the sum of squares of differences between {P′i} and {Pi} subjected to rigid body transformation is minimized.

In Equation 5, R represents a rotation matrix and T represents a translation vector.
Assuming that the center point coordinates are equal, the following relationship is established in the point set moved to the origin. The last equation is the target to be minimized.

First, the rotation matrix R is obtained, and then the translation vector T is obtained.
In order to obtain the rotation matrix R, the matrix H is used.
H = Σq _i q ′ _i ^t
The matrix H is subjected to singular value decomposition (Singular Value Decomposition).

H = UΛV ^t
Here, in singular value decomposition, U and V are orthonormal matrices, and Λ is a diagonal matrix having singular values as diagonal components.
Subsequently, the matrix X is calculated.
X = VU ^t
When the determinant of the matrix X is 1, the matrix X is equal to the rotation matrix R. When the determinant is -1, this problem cannot be solved, but it is rarely -1.

In calculating the determinant, the matrix X is subjected to LU decomposition, and the product sum of diagonal elements is obtained. This sum of products is equal to the determinant.
Subsequently, the translation vector T can be obtained by the following equation.
T = p′−Rp
Then, based on the position / posture of the object at the matching position based on the T1 matrix obtained by the rigid body transformation matrix (T1) calculation unit 26b, and the position / posture of the object in the model image information stored in the model image database. The model image is generated by converting the model image into a model-corresponding image that is an image at the matching position, so that the generated model-corresponding image information is displayed on the display device 4 and stored in the information storage unit 22.

  The correction transformation matrix (T2) calculation unit 27 assumes that the normal direction image information is not converted in the correct direction due to conversion, measurement error, or quantization error. The model-corresponding image generated by converting the image based on the conversion matrix T1 and the normal direction image generated by converting the image of the three-dimensional image information by the conversion matrix P are optimized by an iterative method (ICP: Iterative Closest Points). A correction amount to match the position is calculated. Here, “match” means obtaining a rigid transformation matrix T2 as a third parameter representing the relationship between the two. Note that the correction amount is not limited to the relationship between the model-corresponding image and the normal direction image as described above, but the relationship between the image based on the model image and the image based on the image of the three-dimensional image information. If it is good. Therefore, for example, a relationship between a model image and an image obtained by converting the image of the three-dimensional image information into an image corresponding to the model image by conversion may be used.

  In order to obtain the rigid transformation matrix T2, the three-dimensional point group M = {Mi} = {(mxi, myi) corresponding to the model correspondence image that is an image in which the object exists at the position and orientation specified by the transformation matrix T1. , Mzi)} and a three-dimensional point group S = {Si} = {(sxi, syi, szi)} corresponding to a normal direction image obtained by transforming the image of the three-dimensional image information by the transformation matrix P. When the points are not necessarily equal and the point correspondence between the two is not known, a rigid transformation matrix for converting the point group S to the point group M is obtained.

For each point of the three-dimensional point group S corresponding to the normal direction image, a point of the three-dimensional point group M corresponding to the model corresponding image at the closest distance is searched, and both are used as corresponding points. The Euclidean distance is generally used as the distance, but other distance measures can be applied.
A rigid transformation matrix T2 that associates the two is obtained. At this stage, since the corresponding points are known and the scores of both are equal, the calculation method of the rigid transformation matrix of Formula 4 described above is used.

Next, all the points of the three-dimensional point group S corresponding to the normal direction image information corresponding to the measurement image are subjected to rigid transformation.
Next, if the difference between the rigidly transformed 3D point group S and the 3D point group M is equal to or smaller than a predetermined threshold value, the correction process is terminated. Otherwise, the rigidly transformed 3D point group S is replaced with a new 3D point group. The above correction process is repeated for the group S.

  Here, the general iterative method ICP is a method for obtaining a rigid transformation matrix between three-dimensional point groups, but in the present embodiment, it is applied so as to obtain a rigid transformation matrix between two images. In addition, the iterative method requires a large amount of calculation and falls into a local minimum value, so there is a problem that the initial value dependency is strong. Therefore, approximate alignment (initial value of rigid transformation matrix) can be performed by other methods. It is common to apply the iterative method with the initial value of a state in which both are obtained and substantially combined.

Specifically, in the present embodiment, as shown in FIG. 15A, the normal direction image S shown by the solid line and the model corresponding image M shown by the broken line almost match each other.
For this reason, first, as shown in FIG. 15B, points are selected from the normal direction image S in, for example, a lattice shape (may be random or limited to points having features such as edges). Effective).

Subsequently, as shown in FIG. 15C, a rectangle centered on the coordinates (i, j) of the model corresponding image M with respect to the coordinates (i, j) of the points extracted in the normal direction image S. Region 1 is made to correspond.
Subsequently, as shown in FIG. 16, a rectangular area 2 smaller than the rectangular area 1 around the point (i, j) in the normal direction image S is provided, and the rectangular area 1 of one model-corresponding image M has a rectangular shape. Two-dimensional pattern matching of the size of the region 2 is performed, the most similar point is extracted, and this is set as a point corresponding to the coordinate (i, j) of the normal direction image S.

Subsequently, a plurality of corresponding points are obtained, and a rigid transformation matrix T2 of both is obtained.
Subsequently, the process ends when the difference between the point of the normal direction image S that has undergone rigid transformation and the point of the model-corresponding image M is less than or equal to the threshold value, otherwise the front view image that has undergone rigid transformation has been converted to a new normal direction image S. Then, the rigid body transformation process is repeated.
The conversion matrix calculation unit 28 converts the three-dimensional coordinates (Xorg, Yorg, Xorg) ^t when there is an object at the origin of the three-dimensional coordinates into the three-dimensional coordinates (X0, Y0, Z0) ^t of the measurement image. P ⁻¹ · T2 · T1 is calculated.

That is, when the relationship of the above-described conversion formula is summarized,
3D coordinates of measurement image (X0, Y0, Z0) ^t
Three-dimensional coordinates P (X0, Y0, Z0) ^{t of} normal direction image
3D coordinates (Xorg, Yorg, Xorg) of the object at the origin of the 3D coordinates ^t
Three-dimensional coordinates T1 (Xorg, Yorg, Zorg) ^{t of} the model corresponding image that matches the normal direction image information
Similarly, the three-dimensional coordinates T2 · T1 (Xorg, Yorg, Zorg) ^{t of} the error-corrected image
It becomes. Since the 3D coordinates of the normal direction image and the 3D coordinates of the error corrected image are equal,
P (X0, Y0, Z0) ^t = T2 · T1 (Xorg, Yorg, Zorg) ^t
(X0, Y0, Z0) ^t = P ⁻¹ · T2 · T1 (Xorg, Yorg, Zorg) ^t
It becomes.

Therefore, the conversion formula for converting the three-dimensional coordinates (Xorg, Yorg, Zorg) ^t of the object at the origin of the three-dimensional coordinates into the three-dimensional coordinates (X0, Y0, Z0) ^t of the measurement image is P ^−1. It can be obtained as T1 and T2, and the position and orientation of the object in the three-dimensional image information can be recognized.
Next, the operation of the above embodiment will be described.

  Now, an object 12 having a rectangular parallelepiped protrusion 11 formed on the upper surface of the rectangular parallelepiped 10 shown in FIG. 3 is present in the visual field range of the stereo vision 1, and in order to recognize the position and orientation of the object 12, a model is used in advance. In the image database 3, model images picked up from normal directions of planes in six planes of the target object 12 shown in FIG. 13 on the top, bottom, left, and right sides, and rotated by 90 degrees, 180 degrees, and 270 degrees of each model image 24. A model image of a type is stored.

  In this state, the three-dimensional measuring unit 21 of the arithmetic processing device 2 outputs an imaging command to the stereo vision 1. In the stereo vision 1, the multi-viewpoint image information (stereo image) of the object 12 shown in FIG. 4 is imaged, and in the arithmetic processing device 2, the 3D image information based on the captured multi-viewpoint image information is read and read 3 The three-dimensional image information is stored in the three-dimensional image information storage area of the information storage unit 22. The point / edge selection unit 23 searches for corresponding points using a known stereo vision image processing technique based on the three-dimensional image information read by the three-dimensional measurement unit 21, and the distances (three-dimensional coordinates) of a plurality of points. Ask for. The obtained three-dimensional image information is stored in the three-dimensional image information storage area of the information storage unit 22.

  Then, the plane extraction unit 24 reads out the 3D image information stored in the 3D image information storage area, and the normal vector calculation unit 24a uses two or three planes represented by the 3D image information. A predetermined number of feature points are selected, and a plane normal vector is obtained from the outer product of the vector of the feature point serving as a reference for calculating the plane normal vector and the other two feature points. Next, a normal vector of the plane is obtained for the combination of the reference feature point and all the two nearby points. Then, the calculated normal vector (xi, yi, xi) of the plane is converted into a two-dimensional representation (θi, Φi).

Then, for example, the two-dimensional frequency distribution of θ and Φ is counted by dividing into five-degree sections, and in the counted two-dimensional frequency distribution, the plane method including the feature points based on the normal vector of the section with the highest frequency is included. Adopt as a line vector.
Then, the plane detection unit 24b calculates an equation of a plane represented by a normal vector of the plane including the adopted reference feature point. Based on the calculated plane equation, the plane selection unit 24c creates a frequency distribution of the constant term -d of the plane equation, and determines the point belonging to the highest frequency category as a set of points belonging to the plane to be observed. To do.

  In this way, when the detection of the plane to be watched is completed, the normal direction image conversion unit 25 uses the plane direction extraction unit to calculate the position / posture of the object in the three-dimensional image information in the normal direction image conversion matrix calculation unit 25a. A normal direction image conversion matrix P that is a first conversion matrix that is converted into the position and orientation of the object in the extracted normal direction image of the plane is obtained by using, for example, the DLT method. Then, based on the normal direction image transformation matrix P, the calculation of Formula 1 is performed, and the three-dimensional coordinates (x1, y1, z1) of the normal direction image from the three-dimensional coordinates (x0, y0, z0) of the three-dimensional image information. Is further generated to generate normal direction image information shown in FIG. 11, and the normal direction image information is displayed on the display device 4.

Then, the model-corresponding image generation unit 26 includes normal direction image information and 24 types of model image information corresponding to images captured from the normal direction of each plane of the object stored in the model image database 3. The model image matching unit 26a performs two-dimensional pattern matching to determine corresponding model image information surrounded by a square in FIG.
As a result of this two-dimensional pattern matching, it is determined in which position in the two-dimensional image the determined model image exists. Therefore, based on the position / posture of the target in the model image information, Know your posture. Accordingly, it is possible to obtain the rigid body transformation matrix T1 for converting the position / posture of the target at the origin of the three-dimensional coordinates into the position / posture of the target at the matching position. Thereby, the three-dimensional coordinates (X1, Y1, Z1) of the model corresponding image with respect to the three-dimensional coordinates (Xorg, Yorg, Zorg) when the target is at the origin of the three-dimensional coordinates can be calculated from Equation 4. Further, the model correspondence image can be generated by converting the model image based on the position / posture of the object in the model image information and the rigid body transformation matrix T1.

From this calculation result, when the normal direction image and the model corresponding image are displayed, as shown in FIG. 15A, the normal direction image S and the model corresponding image M substantially coincide with each other.
At this time, the normal direction image S and the model-corresponding image M substantially coincide with each other. However, as shown in FIG. In the case of rotation, correction using the third transformation matrix T2 is performed by an iterative method (ICP), and the difference between the normal direction image S obtained by rigid transformation of the correction and the model corresponding image M is less than a predetermined threshold value. By repeating until the normal direction image S and the model corresponding image M, which have been subjected to high-precision correction and subjected to rigid body transformation, can be accurately matched.

Then, the normal direction transformation matrix (first transformation matrix) P for transforming into a normal direction image and the three-dimensional coordinates in the model-corresponding image when the object is at the origin of the three-dimensional coordinate system Three-dimensional coordinates when the object is at the origin of the three-dimensional coordinate system based on a rigid transformation matrix (second transformation matrix) T1 for transformation into coordinates and a correction transformation matrix (third transformation matrix) T2 Can be obtained as the aforementioned P ⁻¹ · T1 · T2, and the position and orientation of the object 12 can be accurately recognized.

  Further, when the plane extraction unit 24 extracts a plane from the three-dimensional image information, the normal vector is calculated from the three feature points, and the plane is extracted based on the calculated normal vector. There is no need to select a large number of feature points as in the case of applying the least squares method, and there are few features even if the target object has a glossy surface that reflects light, no pattern, or a black object A plane can be extracted by selecting a point. Moreover, when generating the normal direction image information, a line segment having a known angle formed by the straight line corresponding to the edge is detected. Therefore, when the normal direction image information is generated, the main line is formed. Since the angle is known, the normal direction image can be accurately generated by performing conversion to the normal direction image so that the main straight line has a known angle.

Then, when performing the two-dimensional pattern matching between the 24 types of model images stored in the model image database 3 and the normal direction image, the size matching process shown in FIGS. 14A to 14C is executed. Thus, the normal direction image and the model image can be compared and pattern matched with the same size.
In the above embodiment, after calculating the normal direction image conversion matrix P, the rigid body conversion matrix T1 is calculated, and then the correction conversion matrix T2 is calculated, and the position and orientation of the object are recognized based on each matrix. Explained when to do. However, when the normal direction image S and the model corresponding image M match, the calculation process of the correction conversion matrix T2 is omitted, and the object is based on the normal direction conversion matrix P and the rigid body conversion matrix T1. The position and orientation may be recognized.

  In the above embodiment, the case where the DLT method is applied to the calculation of the normal direction image conversion matrix P has been described. However, the present invention is not limited to this, and normalization with higher accuracy than the Gold Standard algorithm or the DLT method is performed. A DLT algorithm may be applied. Further, the DLT method described above is based on the assumption that Gaussian noise is included in the point correspondence but no incorrect correspondence is included. ) Method, a maximum likelihood estimation (LIM) method, a LMS (Least Median of Sauer) method, and the like.

  Furthermore, in the above-described embodiment, the case where two-viewpoint image information is captured by two stereo cameras as stereovision 1 has been described. However, the present invention is not limited to this, and multiple viewpoints can be applied by applying three or more cameras. You may make it image-capture image information.

1 is a system configuration diagram showing an outline of a system configuration of the present invention. It is a functional block diagram which shows the functional block of an arithmetic processing unit. It is a perspective view of the target object used as a measuring object. It is explanatory drawing which shows the multi-viewpoint image imaged with the stereo vision. It is explanatory drawing which shows the selection condition of a feature point and an edge. It is explanatory drawing which shows a normal vector. It is explanatory drawing at the time of expressing a normal vector two-dimensionally. It is explanatory drawing which shows the frequency distribution of a normal vector. It is explanatory drawing showing the equation and frequency display of a plane. It is explanatory drawing showing before and after the coordinate conversion to a normal line direction image. It is explanatory drawing which shows an exact normal line direction image. It is explanatory drawing which shows an inappropriate normal line direction image. It is explanatory drawing which shows the model image memorize | stored in the model image database. It is explanatory drawing which shows the size adjustment process of a normal direction image. It is explanatory drawing which shows the relationship between the normal line direction image S and the model corresponding | compatible image M. FIG. It is explanatory drawing for demonstrating correction | amendment with the front view image S and the front view model image M. FIG.

Explanation of symbols

  DESCRIPTION OF SYMBOLS 1 ... Stereovision, 2 ... Arithmetic processing apparatus, 3 ... Model image database, 4 ... Display apparatus, 10 ... Rectangular parallelepiped, 11 ... Protrusion, 12 ... Object, 21 ... Three-dimensional measurement part, 22 ... Information storage part, 23 ... Point / edge selection unit, 24 ... plane extraction unit, 24a ... normal vector calculation unit, 24b ... plane detection unit, 24c ... plane selection unit, 25 ... normal direction image conversion unit, 25a ... normal direction image conversion matrix calculation , 26 ... model-corresponding image generation unit, 26a ... model image matching unit, 26b ... rigid transformation matrix calculation unit, 27a ... corresponding point search unit, 27b ... T2 matrix calculation unit, 28 ... transformation matrix calculation unit

Claims (4)

A model image storage unit that stores in advance, as a model image, an image captured from the normal direction of each surface constituting a known object to be measured;
An imaging device that images the object as a multi-viewpoint image and outputs three-dimensional image information;
An image information storage unit for storing three-dimensional image information output from the imaging device;
A plane extraction unit that extracts a plane from the three-dimensional image information stored in the image information storage unit;
A first parameter representing the position and orientation of the object in the three-dimensional image information, which is converted into the position and orientation of the object in a normal direction image that is an image from the normal direction of the plane extracted by the plane extraction unit. A normal direction image conversion unit for obtaining a parameter and converting the image of the three-dimensional image information into a normal direction image;
A model image determining unit that compares the normal direction image converted by the normal direction image converting unit with each model image stored in the model image storage unit and determines a corresponding model image;
A second parameter representing the position and orientation of the object at the corresponding position in the normal direction image of the model image determined by the model image determining unit is obtained, and the model image corresponding to the model image is an image at the corresponding position A model-compatible image converter that converts to
A polyhedron position detection device comprising: a position and posture recognition unit for recognizing a position and posture of an object measured based on the calculated first and second parameters. A model image storage unit that stores in advance, as a model image, an image captured from the normal direction of each surface constituting a known object to be measured;
An imaging device that images the object as a multi-viewpoint image and outputs three-dimensional image information;
An image information storage unit for storing three-dimensional image information output from the imaging device;
A plane extraction unit that extracts a plane from the three-dimensional image information stored in the image information storage unit;
A first parameter representing the position and orientation of the object in the three-dimensional image information converted into the position and orientation of the object in the normal direction image that is an image from the normal direction of the plane extracted by the plane extraction unit. A normal direction image converting unit for obtaining a parameter and converting the image of the three-dimensional image information into a normal direction image;
A model image determining unit that compares the normal direction image converted by the normal direction image converting unit with each model image stored in the model image storage unit and determines a corresponding model image;
A second parameter representing the position and orientation of the object at the corresponding position in the normal direction image of the model image determined by the model image determining unit is obtained, and the model image corresponding to the model image is an image at the corresponding position A model-compatible image converter that converts to
Using an image based on the model image and an image based on the three-dimensional image information, an error calculation unit for obtaining a third parameter representing a correction amount of an error between the two by iterative processing;
A polyhedron position detection device comprising: a position and posture recognition unit for recognizing the position and posture of an object measured based on the calculated first, second, and third parameters. An image captured from a normal direction of each surface constituting the object is used as a model image by using an imaging device that captures a known object to be measured as a multi-viewpoint image and outputs three-dimensional image information. A model image storage step for storing in advance in the model image storage unit;
An image information storage step for storing three-dimensional image information output from the imaging device;
A plane extraction step of extracting a plane from the stored three-dimensional image information;
A first parameter representing the position and orientation of the object in the three-dimensional image information, which is converted into the position and orientation of the object in a normal direction image that is an image from the normal direction of the plane extracted by the plane extraction unit. A normal direction image conversion step of obtaining a parameter and converting the image of the three-dimensional image information into a normal direction image;
A model image determination step for determining a corresponding model image by comparing the converted normal direction image and the model image stored in the model image storage step;
A second parameter representing the position and orientation of the object at the corresponding position in the normal direction image of the model image determined in the model image determining step is obtained, and the model corresponding image is an image at the corresponding position. A model-compatible image conversion step for converting to
A polyhedron position detection method comprising: a position and posture recognition step for recognizing the position and posture of an object measured based on the calculated first and second parameters. An image captured from a normal direction of each surface constituting the object is used as a model image by using an imaging device that captures a known object to be measured as a multi-viewpoint image and outputs three-dimensional image information. A model image storage step for storing in advance in the model image storage unit;
An image information storage step for storing three-dimensional image information output from the imaging device;
A plane extraction step of extracting a plane from the stored three-dimensional image information;
A first parameter representing the position and orientation of the object in the three-dimensional image information, which is converted into the position and orientation of the object in a normal direction image that is an image from the normal direction of the plane extracted by the plane extraction unit. A normal direction image conversion step of obtaining a parameter and converting the image of the three-dimensional image information into a normal direction image;
A model image determining step for comparing the converted normal direction image and each model image stored in the model image storing step to determine a corresponding model image;
A second parameter representing the position and orientation of the object at the corresponding position in the normal direction image of the model image determined in the model image determining step is obtained, and the model corresponding image is an image at the corresponding position. A model-compatible image conversion step for converting to
Using an image based on the model image and an image based on the three-dimensional image information, an error calculation step for obtaining a third parameter representing a correction amount of an error between the two by iterative processing;
A polyhedron position detection method comprising: a position and posture recognition step for recognizing the position and posture of an object measured based on the calculated first, second, and third parameters.

Images