CN112857215B - Monocular 6D pose estimation method based on regular icosahedron - Google Patents

Monocular 6D pose estimation method based on regular icosahedron Download PDF

Info

Publication number
CN112857215B
CN112857215B CN202110023412.7A CN202110023412A CN112857215B CN 112857215 B CN112857215 B CN 112857215B CN 202110023412 A CN202110023412 A CN 202110023412A CN 112857215 B CN112857215 B CN 112857215B
Authority
CN
China
Prior art keywords
regular icosahedron
aruco
icosahedron
current frame
code
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110023412.7A
Other languages
Chinese (zh)
Other versions
CN112857215A (en
Inventor
孙昊
段伦辉
崔睿
吴梦坤
谭英伦
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hebei University of Technology
Original Assignee
Hebei University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hebei University of Technology filed Critical Hebei University of Technology
Priority to CN202110023412.7A priority Critical patent/CN112857215B/en
Publication of CN112857215A publication Critical patent/CN112857215A/en
Application granted granted Critical
Publication of CN112857215B publication Critical patent/CN112857215B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • G01B11/002—Measuring arrangements characterised by the use of optical techniques for measuring two or more coordinates
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • G01B11/02—Measuring arrangements characterised by the use of optical techniques for measuring length, width or thickness
    • G01B11/03—Measuring arrangements characterised by the use of optical techniques for measuring length, width or thickness by measuring coordinates of points
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • G01B11/26—Measuring arrangements characterised by the use of optical techniques for measuring angles or tapers; for testing the alignment of axes

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Image Analysis (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

The invention discloses a monocular 6D pose estimation method based on a regular icosahedron. The method combines the Aruco code and the regular icosahedron, then uses a monocular camera to collect images of the regular icosahedron marked by the Aruco code, calculates the spatial three-dimensional coordinates of the central point of the Aruco code, obtains sparse point cloud of an inscribed sphere of the Aruco code, further compares the virtual regular icosahedron at the camera coordinates by utilizing multivariate nonlinear fitting and an iterative closest point algorithm, further calculates the spatial displacement and the rotation angle of the current regular icosahedron, and indirectly calculates the pose of a measured object. The method overcomes the high requirements of the existing method on illumination, environment and equipment, the detection is rapid, and the stability is improved; a large amount of effort is not required to be invested in template acquisition or model training in the early stage, the pose estimation cost is obviously reduced, the pose estimation precision is ensured, the universality is enhanced, and the pose estimation of large-amplitude object motion can be carried out.

Description

Monocular 6D pose estimation method based on regular icosahedron
Technical Field
The invention belongs to the field of image recognition pose detection, and particularly relates to a monocular 6D pose estimation method based on a regular icosahedron.
Background
The pose estimation of the target plays an important role in both production and life, and is a precondition for realizing the flexible and effective motion of the robot. At present, the industrial automation industry is developed rapidly, the requirements of real-time detection and feedback of the state of the robot are continuously improved, and the target pose estimation based on vision has important significance for improving the performance of the robot. The monocular vision system only uses one camera, has a simple structure and low cost, and is widely applied. The main methods for estimating the pose of the target at present comprise:
in documents LEPEIT V, PILET J, FUA P.Point matching as a classification scheme for fast and robust object position estimation [ C ]// Proceedings of the 2004IEEE Computer Society Conference on Computer Vision and Pattern Recognition,2004.CVPR 2004. volume 2.IEEE,2004: II-II. However, the pose estimation method based on the feature points is very susceptible to the influence of illumination and environment, so that the tracking loss of the feature points is caused, and the estimation quality of the target pose is reduced.
In the documents PAVLAKOS G, ZHOU X, CHAN A, et al.6-dof object position from a semantic key points [ C ]//2017IEEE international conference on robotics and automation (ICRA), IEEE,2017:2011 2018, a template-based method is adopted, a two-dimensional or three-dimensional template of an object to be detected is constructed, and a template closest to an actual pose is searched by comparing the relationship between an actual pose image of the current object and the template, so that the current pose of the object is estimated. However, the method needs to construct a large number of template matching libraries for two-dimensional and three-dimensional mapping in the prior period, the workload is huge, the result precision depends on the size of the template matching libraries, and one template library is only suitable for pose estimation of a single object and cannot meet the universality.
The method based on deep learning is used, the deep learning is widely applied in recent years, and the effect of identifying the pose of an object in an image is achieved by training a deep neural network and constructing a loss function. In the documents "REDMON J, DIVVAL S, GIRSHICK R, et al. you only look on:. Unifield, real-time object detection [ C ]// Proceedings of the IEEE conference on computer vision and pattern recognition.2016:779 and 788", the YoLO network is adopted, but the method based on deep learning needs a large amount of sample resources to construct a large amount of sample data and label the real pose, needs a large amount of computing resources to train the network, is difficult to control the cost, is mostly purposefully developed, and cannot meet the universality.
Disclosure of Invention
Aiming at the defects of the prior art, the invention aims to solve the technical problem of providing a monocular 6D pose estimation method based on a regular icosahedron.
The technical scheme for solving the technical problem is to provide a monocular 6D pose estimation method based on a regular icosahedron, and the method is characterized by comprising the following steps:
firstly, marking a regular icosahedron by using an Aruco code to obtain the Aruco code marked regular icosahedron; calibrating a monocular camera, and observing the regular icosahedron marked by the Aruco code through the calibrated monocular camera;
secondly, positioning a plurality of observed Aruco codes in the current frame and the initial frame respectively, and then obtaining the coding id of all the observed Aruco codes in the current frame and the initial frame respectivelyi1,2,3, n and the spatial coordinates a of the central point of the ArUco codei(xi,yi,zi) 1,2,3, n, n represents the number of observed ArUco codes for a frame;
thirdly, performing multivariate nonlinear fitting on the spatial coordinates of the central point of the Aruco code of the current frame obtained in the second step to calculate the coordinates of the spherical center of the regular icosahedron inscribed sphere of the current frame as the spatial coordinates t of the body center of the regular icosahedron of the current frame0Performing multivariate nonlinear fitting on the spatial coordinates of the central point of the Aruco code of the initial frame to calculate the spherical center coordinates of the positive icosahedron inscribed sphere of the initial frame as the spatial coordinates t of the body center of the positive icosahedron of the initial frameO(ii) a Then by the formula trel＝t0-tOObtaining the relative space coordinate t of the body center of the regular icosahedron of the current frame relative to the initial framerel；
Constructing a virtual regular icosahedron at the origin of a camera coordinate system, wherein the center of the virtual regular icosahedronCoinciding with the origin of a camera coordinate system, marking the virtual regular icosahedron by using the Aruco codes and obtaining the codes of all the Aruco codes of the virtual regular icosahedron and the space coordinate A of the central point of the codesWi(xWi,yWi,zWi,idWi) 1,2,3, ·, 20; then the space coordinate of the central point of the Aruco code of the current frame obtained in the second step and the code thereof are compared with AWi(xWi,yWi,zWi,idWi) 1,2, 3.. 20, calculating a rotation matrix R of a current frame regular icosahedron by an iterative closest point method, and combining the spatial coordinates of a central point of an Aruco code of an initial frame and the coding thereof with AWi(xWi,yWi,zWi,idWi) 1,2,3, 20 calculates a rotation matrix R of the regular icosahedron of the initial frame by an iterative closest point methodOThen by the formula
Figure BDA0002889386430000021
Calculating to obtain a relative rotation matrix R of the current frame regular icosahedron relative to the initial framerel；
According to the relative rotation matrix RrelAnd relative spatial coordinate trelObtaining a position matrix T of the current frame regular icosahedron relative to the initial frame regular icosahedronrel：
Figure BDA0002889386430000022
In the formula (2), SE (3) represents an attribute of the matrix;
fourthly, firstly, a measured object coordinate system is constructed, the regular icosahedron is fixed on the measured object, and the relative space coordinate t of the regular icosahedron relative to the measured object is calculatedrefAnd relative rotation matrix RrefObtaining a pose matrix T of the regular icosahedron relative to the measured objectref：
Figure BDA0002889386430000031
And then the relative pose matrix T of the measured object is as follows:
Figure BDA0002889386430000032
and obtaining the spatial attitude change of the measured object of the current frame relative to the initial frame.
Compared with the prior art, the invention has the beneficial effects that:
(1) the method creatively combines the Aruco code with the regular icosahedron, expands the pose identification from two-dimensional label identification to three-dimensional space body identification, greatly improves the pose identification performance, uses a monocular camera to collect images of the regular icosahedron marked by the Aruco code, calculates the spatial three-dimensional coordinate of the central point of the Aruco code by using a PnP algorithm, obtains sparse point cloud of an inscribed sphere, compares the virtual regular icosahedron at the camera coordinate position by using multivariate nonlinear fitting and an iterative closest point algorithm, converts the position solving and rotation matrix solving problem into an optimization problem, further calculates the spatial displacement and the rotation angle of the current regular icosahedron, and indirectly calculates the pose of a measured object.
(2) The method overcomes the high requirements of the existing method on illumination, environment and equipment, the detection is rapid, the stability is improved, and the average image recognition time of each frame is about 10 ms; a large amount of effort is not required to be invested in template acquisition or model training in the early period, so that the pose estimation cost is obviously reduced, the pose estimation precision is ensured, and the universality is enhanced; the three-dimensional object is used as a mark, and the pose estimation of the object large-amplitude motion in the space can be carried out.
(3) The regular icosahedron adopted by the invention is a regular polyhedron with the largest number of spatial surfaces, which is an important factor for realizing the identification of any spatial angle and large-amplitude motion pose, and has higher precision compared with other regular polyhedrons.
(4) Compared with a method based on feature points, the method for identifying the Aruco codes by the monocular camera overcomes the sensitivity of the method to factors such as illumination, environment and the like, and can stably operate in most environments.
(5) Compared with a template-based method, the method provided by the invention has the advantages that the target pose is indirectly measured by using the regular icosahedron, the universality is stronger, a large amount of template libraries do not need to be constructed, and the memory space and the workload are saved.
(6) Compared with a deep learning-based method, the algorithm does not need relatively strong hardware performance, saves cost and meets the universality.
Drawings
FIG. 1 is a schematic representation of an Aruco code-tagged regular icosahedron of the present invention;
FIG. 2 is a schematic diagram of a regular icosahedron and its inscribed sphere of the present invention;
FIG. 3 is a schematic diagram of the position relationship between the virtual regular icosahedron of the present invention and the actual regular icosahedron marked by Aruco code.
Detailed Description
Specific examples of the present invention are given below. The specific examples are only intended to illustrate the invention in further detail and do not limit the scope of protection of the claims of the present application.
The invention provides a monocular 6D pose estimation method (short for method) based on a regular icosahedron, which is characterized by comprising the following steps:
firstly, marking the icosahedron by using an Aruco code to obtain the icosahedron marked by the Aruco code (shown in figure 1); calibrating a monocular camera, and observing the regular icosahedron marked by the Aruco code through the calibrated monocular camera;
preferably, in the first step, the process of labeling the n-icosahedron using the ArUco code is: the method comprises the steps of generating 20 Aruco codes by using an OpenCV image processing library, respectively attaching the 20 Aruco codes to 20 faces of a regular icosahedron according to the encoding sequence, and enabling the geometric center of the Aruco codes to coincide with the geometric midpoint of the face to which the Aruco codes are attached.
Preferably, in the first step, the monocular camera is calibrated by using a checkerboard standard calibration board and by using an OpenCV image processing library to calibrate the internal parameters of the camera, so as to generate a camera internal parameter matrix K;
the camera internal reference matrix K is a matrix for converting the coordinates in the space into plane coordinates, and the standardized formula is as follows:
Figure BDA0002889386430000041
in the formula (1), fx、fyIs the focal length parameter of the camera; c. Cx、cyIs the amount of translation of the pixel.
Secondly, positioning a plurality of observed Aruco codes in the current frame and the initial frame respectively by using an image processing algorithm, and obtaining the coding ids of all observed Aruco codes in the current frame and the initial frame respectively by using the image processing algorithmi1,2,3, n and obtaining the spatial coordinates A of the center points of all observed Aruco codes in the current frame and the initial frame by using a pose calculation methodi(xi,yi,zi) 1,2,3, n, n represents the number of observed ArUco codes for a frame;
the current frame is an image of the current position of the regular icosahedron, and the initial frame is an image of the initial position of the regular icosahedron shot by the monocular camera;
preferably, in the second step, the positioning process of the ArUco code is as follows: carrying out gray processing, median filtering and self-adaptive threshold segmentation on the image acquired by the monocular camera in sequence, and extracting a candidate region meeting the requirement from the segmented image.
Preferably, in the second step, the coding of the ArUco code is obtained: firstly, applying perspective transformation to a candidate area to obtain a standard square mark, separating black and white color positions, and dividing the mark into different cells according to the size of the mark; and determining the color of each cell according to the color corresponding to the pixel with the largest number on each cell, and finally converting the color into a binary value to determine the code of the mark.
Preferably, in the second step, the spatial coordinates of the central point of the ArUco code are obtained: the spatial coordinates of the central point of the Aruco code are obtained according to the four corner point information of the candidate area, the pose calculation function of the Aruco library function in the OpenCV framework is used, the four corner point information is used as input, and the spatial coordinates of the central point of the Aruco code under a camera coordinate system can be obtained by using a PnP mode (namely a 2D-3D matching algorithm).
Thirdly, performing multivariate nonlinear fitting on the space coordinates of the center points of all Aruco codes of the current frame obtained in the second step to calculate the coordinates of the sphere center of the regular icosahedron inscribed sphere of the current frame as the space coordinates t of the body center of the regular icosahedron of the current frame0＝(x0,y0,z0) Performing multivariate nonlinear fitting on the spatial coordinates of the central point of the Aruco code of the initial frame to calculate the spherical center coordinates of the positive icosahedron inscribed sphere of the initial frame as the spatial coordinates t of the body center of the positive icosahedron of the initial frameO＝(xO,yO,zO) (ii) a Then the space coordinate t of the body center of the current frame regular icosahedron is used0＝(x0,y0,z0) Subtracting the spatial coordinate t of the body center of the regular icosahedron of the initial frameO＝(xO,yO,zO) By the formula trel(x,y,z)＝t0(x0,y0,z0)-tO(xO,yO,zO) Obtaining the relative space coordinate t of the body center of the regular icosahedron of the current frame relative to the initial framerel；
Constructing a virtual regular icosahedron at the origin of a camera coordinate system, wherein the position relation between the virtual regular icosahedron and the actual regular icosahedron is shown in figure 3, the body center of the virtual regular icosahedron is coincided with the origin Oc of the camera coordinate system, then using Aruco codes to mark the virtual regular icosahedron and obtaining the codes of all Aruco codes of the virtual regular icosahedron and the space coordinates A of the center point of the Aruco codesWi(xWi,yWi,zWi,idWi) 1,2,3, ·, 20; then the space coordinate of the central point of the Aruco code of the current frame obtained in the second step and the code A thereofi(xi,yi,zi,idi) 1,2,3, n and aWi(xWi,yWi,zWi,idWi) 1,2, 3.. 20, calculating a rotation matrix R of a current frame regular icosahedron by an iterative closest point method, and combining the spatial coordinates of a central point of an Aruco code of an initial frame and the coding thereof with AWi(xWi,yWi,zWi,idWi) 1,2,3, 20 calculates the initial point by the iterative closest point methodRotation matrix R of start frame regular icosahedronOThen by the formula
Figure BDA0002889386430000052
Calculating to obtain a relative rotation matrix R of the current frame regular icosahedron relative to the initial framerel；
According to the relative rotation matrix RrelAnd relative spatial coordinate trelObtaining a position matrix T of the current frame regular icosahedron relative to the initial frame regular icosahedronrel：
Figure BDA0002889386430000051
In the formula (2), SE (3) represents a plum group, is a mathematical symbolic expression and represents the attribute of a matrix;
preferably, in the third step, the spatial coordinates of the current frame regular icosahedron body center are obtained: since the geometric center of the Aruco code coincides with the geometric midpoint of the surface to which the Aruco code is attached, and the intersection point of the inscribed sphere of the regular icosahedron and the regular icosahedron is exactly positioned at the geometric center of each surface of the regular icosahedron (as shown in FIG. 2), the spatial coordinate A of the central point of each Aruco code on the partial surface of the regular icosahedron observed by the monocular camera obtained in the second step at the current frame can be used for determining the spatial coordinate A of the central point of each Aruco code on the partial surface of the regular icosahedroni(xi,yi,zi) 1,2,3, n is regarded as the current frame regular icosahedron inscribed sphere point (x)i,yi,zi) The sparse point cloud coordinates of (1); when the monocular camera obtains the space coordinates larger than three central points of the Aruco codes, the data condition of multivariate nonlinear fitting is met (under the general condition, the monocular camera can obtain 6-8 central point coordinates of the Aruco codes in each observation, namely the condition can be met in each observation), and the current frame is processed into the regular icosahedron inscribed sphere points (x)i,yi,zi) And the radius of the inscribed sphere is taken as a parameter, the standard equation of the inscribed sphere is as follows:
(xi-x0)2+(yi-y0)2+(zi-z0)2-Rinner part 2＝0 (3)
In the formula (3), RInner partIs the radius of the tangent sphere of a regular icosahedron,
Figure BDA0002889386430000061
a is the edge length of a regular icosahedron;
the spherical center of the spherical multivariate nonlinear function is fitted to the formula (3) by using nonlinear least squares, and a loss function is constructed as follows:
Figure BDA0002889386430000062
since the requirement of multivariate nonlinear fitting on the initial point is high, the initial point is easy to fall into a local minimum value, and therefore the known spherical point coordinate (x) is utilizedi,yi,zi) Generating a group of initial values R of multivariate nonlinear fitting for the coordinates of the spherical center of the inscribed sphereFirst stage：
Figure BDA0002889386430000063
Then R is putFirst stageObtaining a value of the loss function J in the formula (4), and then continuously changing RFirst stageThe value of (a) is that the value of the loss function J in the formula (4) is continuously reduced, and a group of (x) which enables the loss function J to be minimum can be obtained after multiple iterative solution0,y0,z0) That is, the coordinates of the current frame inscribed sphere center (i.e. the space coordinates t of the current frame regular icosahedron body center under the camera coordinate system)0＝(x0,y0,z0))。
Preferably, in the third step, the spatial coordinates of the positive icosahedron body center of the initial frame are obtained: taking the spatial coordinates of the central point of each Aruco code on the partial surface of the regular icosahedron observed by the monocular camera in the initial frame as the sparse point cloud coordinates of the spherical surface point of the regular icosahedron inscribed sphere of the initial frame, and obtaining the spatial coordinates of the body center of the regular icosahedron of the initial frame in the same way as the spatial coordinates of the body center of the regular icosahedron of the current frame, namely obtaining the spatial coordinates t of the body center of the regular icosahedron of the initial frame in the camera coordinate systemO＝(xO,yO,zO)。
Preferably, in the third step, the rotation matrix of the current frame regular icosahedron is obtained:
search AWiAnd matching the codes of the virtual regular icosahedron Aruco code corresponding to the code of the actual regular icosahedron Aruco code of the current frame obtained in the second step, so as to obtain a group of matching points in the space:
(AWi,Ai),i＝1,2,3,...,n (6)
the group of matching points consists of the spatial coordinates of the central point of the Aruco code observed on the actual regular icosahedron and the spatial coordinates of the central point of the Aruco code corresponding to the actual regular icosahedron, and centroiding is respectively carried out on the spatial coordinates through a formula (7) (namely, the spatial coordinates of the central point of the Aruco code observed on the actual regular icosahedron and the spatial coordinates of the central point of the Aruco code corresponding to the actual regular icosahedron are translated to the original points):
Figure BDA0002889386430000064
obtained qiDe-centrode, q, of the spatial coordinates of the center point of the actual regular icosahedral Aruco codei' is a centroid removing point of the space coordinate of the central point of the virtual regular icosahedron Aruco code, and then q is obtainediAnd q isiThe relationship of' is as follows:
Figure BDA0002889386430000071
defining an error term:
Figure BDA0002889386430000072
the rotation matrix R of the current frame regular icosahedron, which minimizes the error term E, is found according to equation (9), the first term in equation (9)
Figure BDA0002889386430000076
Independent of the optimization objective, R in the second termTI is also independent of the optimization objective, the error term becomes:
Figure BDA0002889386430000073
in the formula (10), tr represents a trace of the matrix;
to solve the optimization objective R in equation (10), a matrix is defined:
Figure BDA0002889386430000074
performing singular value decomposition on W in equation (11) to obtain:
W＝UΣVT (12)
in equation (12), Σ is the eigenvalue matrix of W, U and V are diagonal matrices, and then the rotation matrix R of the current frame regular icosahedron is:
R＝UVT (13)。
preferably, in the third step, the rotation matrix of the positive icosahedron of the initial frame is obtained: search AWiThe coding of the virtual regular icosahedron Aruco code corresponding to the coding of the actual regular icosahedron Aruco code of the initial frame obtained in the second step is matched with the same coding, and the rotation matrix R of the initial frame regular icosahedron can be obtained by the same method as the obtaining of the rotation matrix of the current frame regular icosahedronO。
And fourthly, indirectly calculating the 6D pose of the measured object according to the space relative position relation between the regular icosahedron and the measured object.
Preferably, the fourth step is specifically: firstly, a measured object coordinate system is constructed, the regular icosahedron is fixed on the measured object, and the relative position and posture relation between the regular icosahedron and the measured object is calculated (namely, the relative space coordinate t of the regular icosahedron relative to the measured object is calculatedrefAnd relative rotation matrix Rref) Obtaining a pose matrix T of the regular icosahedron relative to the measured objectref：
Figure BDA0002889386430000075
And then the relative pose matrix T of the measured object is as follows:
Figure BDA0002889386430000081
and obtaining the space attitude change of the measured object of the current frame relative to the initial frame.
The camera calibration method, the Aruco code generation method, the image processing algorithm and the pose calculation method are known methods in the field.
Nothing in this specification is said to apply to the prior art.

Claims (7)

1. A monocular 6D pose estimation method based on a regular icosahedron is characterized by comprising the following steps:
firstly, marking a regular icosahedron by using an Aruco code to obtain the Aruco code marked regular icosahedron; calibrating a monocular camera, and observing the regular icosahedron marked by the Aruco code through the calibrated monocular camera;
secondly, positioning a plurality of observed Aruco codes in the current frame and the initial frame respectively, and then obtaining the coding id of all the observed Aruco codes in the current frame and the initial frame respectivelyi1,2,3, n and the spatial coordinates a of the central point of the ArUco codei(xi,yi,zi) 1,2,3, n, n represents the number of observed ArUco codes for a frame;
thirdly, performing multivariate nonlinear fitting on the spatial coordinates of the central point of the Aruco code of the current frame obtained in the second step to calculate the coordinates of the spherical center of the regular icosahedron inscribed sphere of the current frame as the spatial coordinates t of the body center of the regular icosahedron of the current frame0Performing multivariate nonlinear fitting on the spatial coordinates of the central point of the Aruco code of the initial frame to calculate the spherical center coordinates of the positive icosahedron inscribed sphere of the initial frame as the positive icosahedron body of the initial frameSpatial coordinates t of the heartO(ii) a Then by the formula trel＝t0-tOObtaining the relative space coordinate t of the body center of the regular icosahedron of the current frame relative to the initial framerel；
Constructing a virtual regular icosahedron at the origin of a camera coordinate system, enabling the body center of the virtual regular icosahedron to coincide with the origin of the camera coordinate system, marking the virtual regular icosahedron by using the Aruco codes and obtaining the codes of all the Aruco codes of the virtual regular icosahedron and the space coordinates A of the center points of the codesWi(xWi,yWi,zWi,idWi) 1,2,3, ·, 20; then the space coordinate of the central point of the Aruco code of the current frame obtained in the second step and the code thereof are compared with AWi(xWi,yWi,zWi,idWi) 1,2, 3.. 20, calculating a rotation matrix R of a current frame regular icosahedron by an iterative closest point method, and combining the spatial coordinates of a central point of an Aruco code of an initial frame and the coding thereof with AWi(xWi,yWi,zWi,idWi) 1,2,3, 20 calculates a rotation matrix R of the regular icosahedron of the initial frame by an iterative closest point methodOThen by the formula
Figure FDA0003413263500000012
Calculating to obtain a relative rotation matrix R of the current frame regular icosahedron relative to the initial framerel；
According to the relative rotation matrix RrelAnd relative spatial coordinate trelObtaining a position matrix T of the current frame regular icosahedron relative to the initial frame regular icosahedronrel：
Figure FDA0003413263500000011
In the formula (2), SE (3) represents an attribute of the matrix;
in the third step, obtaining a rotation matrix of the current frame regular icosahedron:
search AWiCoding pair of actual regular icosahedron Aruco codes of current frame obtained in the middle and second stepsAnd (3) the corresponding codes of the Aruco code of the virtual regular icosahedron are matched with each other, so that a group of matching points in the space are obtained:
(AWi,Ai),i＝1,2,3,...,n (6)
the group of matching points consists of the spatial coordinates of the central point of the Aruco code observed on the actual regular icosahedron and the spatial coordinates of the central point of the Aruco code corresponding to the actual regular icosahedron, and the centroiding of the Aruco code is respectively removed by the formula (7):
Figure FDA0003413263500000021
obtained qiDe-centrode, q, of the spatial coordinates of the center point of the actual regular icosahedral Aruco codei' is a centroid removing point of the space coordinate of the central point of the virtual regular icosahedron Aruco code, and then q is obtainediAnd q isiThe relationship of' is as follows:
Figure FDA0003413263500000022
defining an error term:
Figure FDA0003413263500000023
the rotation matrix R of the current frame regular icosahedron, which minimizes the error term E, is found according to equation (9), the first term in equation (9)
Figure FDA0003413263500000024
Independent of the optimization objective, R in the second termTI is also independent of the optimization objective, the error term becomes:
Figure FDA0003413263500000025
in the formula (10), tr represents a trace of the matrix;
to solve the optimization objective R in equation (10), a matrix is defined:
Figure FDA0003413263500000026
performing singular value decomposition on W in equation (11) to obtain:
W＝UΣVT (12)
in equation (12), Σ is the eigenvalue matrix of W, U and V are diagonal matrices, and then the rotation matrix R of the current frame regular icosahedron is:
R＝UVT (13)
fourthly, firstly, a measured object coordinate system is constructed, the regular icosahedron is fixed on the measured object, and the relative space coordinate t of the regular icosahedron relative to the measured object is calculatedrefAnd relative rotation matrix RrefObtaining a pose matrix T of the regular icosahedron relative to the measured objectref：
Figure FDA0003413263500000027
And then the relative pose matrix T of the measured object is as follows:
Figure FDA0003413263500000031
and obtaining the spatial attitude change of the measured object of the current frame relative to the initial frame.
2. The method for estimating the monocular 6D pose based on a regular icosahedron as claimed in claim 1, wherein in the first step, the process of marking the regular icosahedron by using Aruco code is as follows: the method comprises the steps of generating 20 Aruco codes by using an OpenCV image processing library, respectively attaching the 20 Aruco codes to 20 faces of a regular icosahedron according to the encoding sequence, and enabling the geometric center of the Aruco codes to coincide with the geometric center of the face to which the Aruco codes are attached.
3. The monocular 6D pose estimation method based on a regular icosahedron as claimed in claim 1, wherein in the first step, a chessboard grid standard calibration plate is used for calibration of the monocular camera, and an OpenCV image processing library is used for calibration of camera internal parameters to generate a camera internal parameter matrix K;
the camera internal reference matrix K is a matrix for converting the coordinates in the space into plane coordinates, and the standardized formula is as follows:
Figure FDA0003413263500000032
in the formula (1), fx、fyIs the focal length parameter of the camera; c. Cx、cyIs the amount of translation of the pixel.
4. The monocular 6D pose estimation method based on a regular icosahedron as claimed in claim 1, wherein in the second step, the positioning process of the ArUco code is: carrying out gray processing, median filtering and self-adaptive threshold segmentation on the image acquired by the monocular camera in sequence, and extracting a candidate region meeting the requirement from the segmented image.
5. The method for estimating monocular 6D pose based on regular icosahedron according to claim 4, wherein in the second step, the coded acquisition of ArUco code: firstly, applying perspective transformation to a candidate area to obtain a standard square mark, separating black and white color positions, and dividing the mark into different cells according to the size of the mark; and determining the color of each cell according to the color corresponding to the pixel with the largest number on each cell, and finally converting the color into a binary value to determine the code of the mark.
6. The monocular 6D pose estimation method based on a regular icosahedron as claimed in claim 4, wherein in the second step, the spatial coordinates of the central point of the ArUco code are obtained: and solving the spatial coordinates of the central point of the Aruco code according to the four corner point information of the candidate area, using a pose calculation function of an Aruco library function in an OpenCV frame, taking the four corner point information as input, and obtaining the spatial coordinates of the central point of the Aruco code under a camera coordinate system by using a PnP mode.
7. The monocular 6D pose estimation method based on a regular icosahedron as claimed in claim 1, characterized in that, in the third step, the spatial coordinates of the body center of the regular icosahedron of the current frame are obtained: the space coordinate of the central point of the Aruco code on the partial surface of the regular icosahedron observed by the current frame obtained in the second step is taken as the inscribed sphere spherical point (x) of the regular icosahedron of the current framei,yi,zi) The sparse point cloud coordinates of (1); when the monocular camera obtains the space coordinate which is larger than the central point of three Aruco codes, the spherical surface point (x) of the tangent sphere of the regular icosahedron of the current frame is obtainedi,yi,zi) And the radius of the inscribed sphere is taken as a parameter, the standard equation of the inscribed sphere is as follows:
(xi-x0)2+(yi-y0)2+(zi-z0)2-Rinner part 2＝0 (3)
In the formula (3), RInner partIs the radius of the tangent sphere of a regular icosahedron,
Figure FDA0003413263500000041
a is the edge length of a regular icosahedron;
the spherical center of the spherical multivariate nonlinear function is fitted to the formula (3) by using nonlinear least squares, and a loss function is constructed as follows:
Figure FDA0003413263500000042
since the requirement of multivariate nonlinear fitting on the initial point is high, the initial point is easy to fall into a local minimum value, and therefore the known spherical point coordinate (x) is utilizedi,yi,zi) Generating a group of initial values R of multivariate nonlinear fitting for the coordinates of the spherical center of the inscribed sphereFirst stage：
Figure FDA0003413263500000043
Then R is putFirst stageObtaining a value of the loss function J in the formula (4), and then continuously changing RFirst stageThe value of (a) is that the value of the loss function J in the formula (4) is continuously reduced, and a group of (x) which enables the loss function J to be minimum can be obtained after multiple iterative solution0,y0,z0) I.e. the coordinates of the spherical center of the inscribed sphere of the current frame.
CN202110023412.7A 2021-01-08 2021-01-08 Monocular 6D pose estimation method based on regular icosahedron Active CN112857215B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110023412.7A CN112857215B (en) 2021-01-08 2021-01-08 Monocular 6D pose estimation method based on regular icosahedron

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110023412.7A CN112857215B (en) 2021-01-08 2021-01-08 Monocular 6D pose estimation method based on regular icosahedron

Publications (2)

Publication Number Publication Date
CN112857215A CN112857215A (en) 2021-05-28
CN112857215B true CN112857215B (en) 2022-02-08

Family

ID=76005415

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110023412.7A Active CN112857215B (en) 2021-01-08 2021-01-08 Monocular 6D pose estimation method based on regular icosahedron

Country Status (1)

Country Link
CN (1) CN112857215B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115690607B (en) * 2023-01-04 2023-04-11 西湖大学 Rainfall inversion method and device based on infrared remote sensing and deep learning
CN117351091A (en) * 2023-09-14 2024-01-05 成都飞机工业（集团）有限责任公司 Camera array calibration device and use method thereof

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104457640A (en) * 2014-09-05 2015-03-25 合肥工业大学 Common virtual geometrical characteristic standard component used for calibration of joint-class coordinate measuring machine
JP2016058043A (en) * 2014-09-12 2016-04-21 キヤノン株式会社 Information processing device, information processing method, and program
JP2018032111A (en) * 2016-08-22 2018-03-01 株式会社ソニー・インタラクティブエンタテインメント Information processing device, information processing system, object for operation, and information processing method
CN108491880A (en) * 2018-03-23 2018-09-04 西安电子科技大学 Object classification based on neural network and position and orientation estimation method
CN109345588A (en) * 2018-09-20 2019-02-15 浙江工业大学 A kind of six-degree-of-freedom posture estimation method based on Tag
CN110390694A (en) * 2019-07-19 2019-10-29 中兵勘察设计研究院有限公司 A kind of positioning of photography method for article three-dimensional reconstruction
CN110827359A (en) * 2019-10-29 2020-02-21 武汉大学 Checkerboard trihedron-based camera and laser external reference checking and correcting method and device
CN111179347A (en) * 2019-12-26 2020-05-19 广东星舆科技有限公司 Positioning method, positioning device and storage medium based on regional characteristics
TWI708591B (en) * 2019-12-06 2020-11-01 財團法人金屬工業研究發展中心 Three-dimensional real-time positioning method for orthopedic surgery
CN111899301A (en) * 2020-06-02 2020-11-06 广州中国科学院先进技术研究所 Workpiece 6D pose estimation method based on deep learning

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9418480B2 (en) * 2012-10-02 2016-08-16 Augmented Reailty Lab LLC Systems and methods for 3D pose estimation

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104457640A (en) * 2014-09-05 2015-03-25 合肥工业大学 Common virtual geometrical characteristic standard component used for calibration of joint-class coordinate measuring machine
JP2016058043A (en) * 2014-09-12 2016-04-21 キヤノン株式会社 Information processing device, information processing method, and program
JP2018032111A (en) * 2016-08-22 2018-03-01 株式会社ソニー・インタラクティブエンタテインメント Information processing device, information processing system, object for operation, and information processing method
CN108491880A (en) * 2018-03-23 2018-09-04 西安电子科技大学 Object classification based on neural network and position and orientation estimation method
CN109345588A (en) * 2018-09-20 2019-02-15 浙江工业大学 A kind of six-degree-of-freedom posture estimation method based on Tag
CN110390694A (en) * 2019-07-19 2019-10-29 中兵勘察设计研究院有限公司 A kind of positioning of photography method for article three-dimensional reconstruction
CN110827359A (en) * 2019-10-29 2020-02-21 武汉大学 Checkerboard trihedron-based camera and laser external reference checking and correcting method and device
TWI708591B (en) * 2019-12-06 2020-11-01 財團法人金屬工業研究發展中心 Three-dimensional real-time positioning method for orthopedic surgery
CN111179347A (en) * 2019-12-26 2020-05-19 广东星舆科技有限公司 Positioning method, positioning device and storage medium based on regional characteristics
CN111899301A (en) * 2020-06-02 2020-11-06 广州中国科学院先进技术研究所 Workpiece 6D pose estimation method based on deep learning

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Stereo-based real-time 6-DoF work tool tracking for robot programing by demonstration;Marcos Ferreira等;《Int J Adv Manuf Technol》;20140613;第57–69页 *
基于三维标识物的虚实配准方法研究;卢韶芳等;《吉林大学学报( 信息科学版)》;20170531;第35卷(第3期);第288-295页 *

Also Published As

Publication number Publication date
CN112857215A (en) 2021-05-28

Similar Documents

Publication Publication Date Title
CN108549873B (en) Three-dimensional face recognition method and three-dimensional face recognition system
CN111775152B (en) Method and system for guiding mechanical arm to grab scattered stacked workpieces based on three-dimensional measurement
CN109344882B (en) Convolutional neural network-based robot control target pose identification method
CN112734844B (en) Monocular 6D pose estimation method based on octahedron
CN107481284A (en) Method, apparatus, terminal and the system of target tracking path accuracy measurement
CN112857215B (en) Monocular 6D pose estimation method based on regular icosahedron
Duncan et al. Multi-scale superquadric fitting for efficient shape and pose recovery of unknown objects
CN107705322A (en) Motion estimate tracking and system
CN111476841B (en) Point cloud and image-based identification and positioning method and system
Lin et al. Robotic grasping with multi-view image acquisition and model-based pose estimation
CN108171102A (en) A kind of part method for quickly identifying of view-based access control model
Zheng et al. Industrial part localization and grasping using a robotic arm guided by 2D monocular vision
Wunsch et al. Real-Time pose estimation of 3D objects from camera images using neural networks
Lambrecht et al. Towards the usage of synthetic data for marker-less pose estimation of articulated robots in rgb images
Ma et al. Binocular vision object positioning method for robots based on coarse-fine stereo matching
Sun et al. Robotic grasping using semantic segmentation and primitive geometric model based 3D pose estimation
CN108090476A (en) It is a kind of to be directed to the external 3D face identification methods blocked
CN113256789A (en) Three-dimensional real-time human body posture reconstruction method
Sun et al. Automatic targetless calibration for LiDAR and camera based on instance segmentation
CN112734843B (en) Monocular 6D pose estimation method based on regular dodecahedron
Liu et al. Robust 3-d object recognition via view-specific constraint
CN108694348B (en) Tracking registration method and device based on natural features
Li et al. Online workpieces recognition for the robotic spray-painting production line with a low-cost RGB-D camera
CN115760984A (en) Non-cooperative target pose measurement method based on monocular vision by cubic star
Sanchez-Lopez et al. Visual detection, tracking and pose estimation of a robotic arm end effector

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant