CN112734843A - Monocular 6D pose estimation method based on regular dodecahedron - Google Patents

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

Info

Publication number
CN112734843A
CN112734843A CN202110022822.XA CN202110022822A CN112734843A CN 112734843 A CN112734843 A CN 112734843A CN 202110022822 A CN202110022822 A CN 202110022822A CN 112734843 A CN112734843 A CN 112734843A
Authority
CN
China
Prior art keywords
regular dodecahedron
aruco
current frame
dodecahedron
monocular
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.)
Granted
Application number
CN202110022822.XA
Other languages
Chinese (zh)
Other versions
CN112734843B (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 CN202110022822.XA priority Critical patent/CN112734843B/en
Publication of CN112734843A publication Critical patent/CN112734843A/en
Application granted granted Critical
Publication of CN112734843B publication Critical patent/CN112734843B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30204—Marker
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (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 dodecahedron. The method combines the ArUco code with the regular dodecahedron, then uses a monocular camera to collect images of the regular dodecahedron 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, compares the virtual regular dodecahedron 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 dodecahedron, 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 dodecahedron
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 dodecahedron.
Background
Pose estimation of a target object is one of key technologies for ensuring that the robot realizes random operation in a working space. 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:
and estimating the pose by using a feature point matching method. The specific method comprises the steps of firstly extracting characteristic points in the image, matching the characteristic points in different images, and estimating the change of the target posture according to the motion of the characteristic points. A method for estimating the pose of an Autonomous vehicle using feature point extraction and matching techniques is proposed in the documents G.Lu, X.Wong and J.McBride.from Mapping to Localization A Complex frame to visual Estimate Position and Attitude for Autonomous Vehicles [ J ].2019 IEEE International Conference on Image Processing (ICIP), Taipei, China,2019pp.3103-3107, doi:10.1109/ICIP.2019.8803326. However, the feature point matching technique has its own limitations, and the feature point tracking will be greatly affected in the case of poor lighting conditions.
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.
And constructing a three-dimensional template by using a template-based method, and searching the position closest to the actual position for estimating the current target position. For example, in AUBRY M, MATURANA D, EFROS A, et al, setting 3D channels, expressor part-based 2D-3D alignment using large data set of CAD models [ C ] Proceedings of the IEEE conference on computer vision and pattern registration.2014, 3762 and 3769, a large number of three-dimensional CAD models are used to locate the classification of the target in the image as an alignment problem from two-dimensional image to three-dimensional model. However, the template accuracy depends on the size of the template library, and one template is suitable for one scene, and the universality is poor.
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 dodecahedron.
The technical scheme for solving the technical problem is to provide a monocular 6D pose estimation method based on a regular dodecahedron, and the method is characterized by comprising the following steps of:
firstly, marking the regular dodecahedron by using an Aruco code to obtain the Aruco code marked regular dodecahedron; calibrating a monocular camera, and observing the regular dodecahedron 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 inscribed sphere of the regular dodecahedron of the current frame as the spatial coordinates t of the body center of the regular dodecahedron of the current frame0Performing multivariate nonlinear fitting on the space coordinate of the central point of the Aruco code of the initial frame to calculate the spherical center coordinate of the regular dodecahedron inscribed sphere of the initial frame as the initialSpatial coordinate t of body center of frame regular dodecahedronO(ii) a Then by the formula trel＝t0-tOObtaining the relative space coordinate t of the body center of the regular dodecahedron of the current frame relative to the initial framerel；
Constructing a virtual regular dodecahedron at the origin of a camera coordinate system, enabling the body center of the virtual regular dodecahedron to coincide with the origin of the camera coordinate system, marking the virtual regular dodecahedron by using the Aruco codes and obtaining the codes of all the Aruco codes of the virtual regular dodecahedron and the space coordinate A of the center point of the Aruco codesWi(xWi,yWi,zWi,idWi) 1,2,3, ·, 12; 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.. 12, calculating a rotation matrix R of the regular dodecahedron of the current frame by an iterative closest point method, and combining the spatial coordinates of the central point of the Aruco code of the initial frame and the coding thereof with AWi(xWi,yWi,zWi,idWi) 1,2,3, 12 calculates the rotation matrix R of the regular dodecahedron of the initial frame by the iterative closest point methodOThen by the formula
Figure BDA0002889184010000022
Calculating to obtain a relative rotation matrix R of the regular dodecahedron of the current frame relative to the initial framerel；
According to the relative rotation matrix RrelAnd relative spatial coordinate trelObtaining a pose matrix T of the current frame regular dodecahedron relative to the initial frame regular dodecahedronrel：
Figure BDA0002889184010000021
In the formula (2), SE (3) represents an attribute of the matrix;
fourthly, firstly, a measured object coordinate system is constructed, the regular dodecahedron is fixed on the measured object, and the relative space coordinate t of the regular dodecahedron relative to the measured object is calculatedrefAnd relative rotation matrix RrefObtaining a pose matrix T of the regular dodecahedron relative to the measured objectref：
Figure BDA0002889184010000031
And then the relative pose matrix T of the measured object is as follows:
Figure BDA0002889184010000032
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 dodecahedron, 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 dodecahedron 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 dodecahedron at the coordinate of the camera by using multivariate nonlinear fitting and an iterative closest point algorithm, converts the position solving and rotation matrix solving problems into an optimization problem, further calculates the space displacement and the rotation angle of the current regular dodecahedron, 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 dodecahedron adopted by the invention is a three-dimensional object, and can realize the position and posture identification of any spatial angle and large-amplitude motion. Each surface area of the regular dodecahedron is larger than that of the regular icosahedron with the same volume, and each surface can put down larger Aruco codes for computer visual detection, so that the identification effectiveness can be ensured at a farther position.
(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 dodecahedron, 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 dodecahedron of the present invention;
FIG. 2 is a schematic view of a regular dodecahedron and its inscribed sphere in accordance with the present invention;
FIG. 3 is a schematic diagram showing the position relationship between the virtual regular dodecahedron and the actual regular dodecahedron marked by Aruco code according to the present invention.
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 method) based on a regular dodecahedron, which is characterized by comprising the following steps:
firstly, marking the regular dodecahedron by using an Aruco code to obtain the Aruco code marked regular dodecahedron (shown in figure 1); calibrating a monocular camera, and observing the regular dodecahedron marked by the Aruco code through the calibrated monocular camera;
preferably, in the first step, the process of labeling the dodecahedron by using the ArUco code is as follows: 12 Aruco codes are generated through C + + codes by using an OpenCV image processing library, the 12 Aruco codes are respectively attached to 12 faces of the regular dodecahedron in the coding sequence, and the geometric center of the Aruco codes is overlapped with the geometric midpoint of the attached faces.
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 BDA0002889184010000041
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 dodecahedron, and the initial frame is an image of the initial position of the regular dodecahedron 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 an image acquired by a monocular camera, extracting an Aruco code contour from the segmented image, and filtering non-convex and non-square images to further obtain a candidate region meeting the requirement.
Preferably, in the second step, the coding of the ArUco code is obtained: firstly, perspective transformation is applied to a candidate region to obtain a standard square mark, an Otsu threshold processing algorithm is used for separating black and white color positions, and the black and white color positions are divided 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 spherical center of the regular dodecahedron inscribed sphere of the current frame as the space coordinates t of the body center of the regular dodecahedron 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 regular dodecahedron inscribed sphere of the initial frame as the spatial coordinates t of the body center of the regular dodecahedron of the initial frameO＝(xO,yO,zO) (ii) a Then the space coordinate t of the body center of the regular dodecahedron of the current frame is used0＝(x0,y0,z0) Subtracting the spatial coordinate t of the body center of the regular dodecahedron 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 dodecahedron of the current frame relative to the initial framerel；
Constructing a virtual regular dodecahedron at the origin of a camera coordinate system, wherein the position relationship between the virtual regular dodecahedron and the actual regular dodecahedron is shown in figure 3, the body center of the virtual regular dodecahedron is superposed with the origin of the camera coordinate system Oc, and then using ArUco codes to mark the virtual regular dodecahedron to obtain the codes of all ArUco codes of the virtual regular dodecahedron and the space coordinates A of the center point of the ArUco codesWi(xWi,yWi,zWi,idWi),i＝12,3, ·, 12; 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.. 12, calculating a rotation matrix R of the regular dodecahedron of the current frame by an iterative closest point method, and combining the spatial coordinates of the central point of the Aruco code of the initial frame and the coding thereof with AWi(xWi,yWi,zWi,idWi) 1,2,3, 12 calculates the rotation matrix R of the regular dodecahedron of the initial frame by the iterative closest point methodOThen by the formula
Figure BDA0002889184010000052
Calculating to obtain a relative rotation matrix R of the regular dodecahedron of the current frame relative to the initial framerel；
According to the relative rotation matrix RrelAnd relative spatial coordinate trelObtaining a pose matrix T of the current frame regular dodecahedron relative to the initial frame regular dodecahedronrel：
Figure BDA0002889184010000051
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 body center of the regular dodecahedron of the current frame 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 dodecahedron and the regular dodecahedron is exactly positioned at the geometric center of each surface of the regular dodecahedron (as shown in FIG. 2), the spatial coordinate A of the central point of each Aruco code on the partial surface of the regular dodecahedron observed by the monocular camera obtained in the second step at the current frame can be usedi(xi,yi,zi) 1,2,3, n is regarded as the tangent sphere point (x) of the regular dodecahedron of the current framei,yi,zi) The sparse point cloud coordinates of (1); when the monocular camera obtains more than three ArUco codesThe spatial coordinates of the central point satisfy the data condition of multivariate nonlinear fitting (generally, the monocular camera can obtain 6-8 Aruco code central point coordinates for each observation, namely, the condition can be satisfied for each observation), and the spherical point (x) of the inscribed sphere of the regular dodecahedron of the current framei,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 inscribed sphere of the regular dodecahedron,
Figure BDA0002889184010000061
a is the edge length of a regular dodecahedron;
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 BDA0002889184010000062
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 BDA0002889184010000063
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 (i.e. the space of the body center of the regular dodecahedron of the current frame under the coordinate system of the camera)Inter-coordinate t0＝(x0,y0,z0))。
Preferably, in the third step, the spatial coordinates of the regular dodecahedron body center of the initial frame are obtained: the space coordinate of each ArUco code central point on the partial surface of the regular dodecahedron observed by the monocular camera in the initial frame is taken as the sparse point cloud coordinate of the spherical surface point of the inscribed sphere of the regular dodecahedron in the initial frame, and the space coordinate t of the body center of the regular dodecahedron in the initial frame under the camera coordinate system can be obtained in the same way as the space coordinate of the body center of the regular dodecahedron in the current frameO＝(xO,yO,zO)。
Preferably, in the third step, the rotation matrix of the current frame regular dodecahedron is obtained:
search AWiAnd matching the codes of the virtual regular dodecahedron ArUco codes corresponding to the codes of the actual regular dodecahedron ArUco codes 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 set of matching points consists of the spatial coordinates of the central point of the ArUco code observed on the actual regular dodecahedron and the spatial coordinates of the central point of the ArUco code corresponding to the actual regular dodecahedron, and the centroiding of the matching points is respectively carried out through a formula (7) (namely, the spatial coordinates of the central point of the ArUco code observed on the actual regular dodecahedron and the centroiding of the spatial coordinates of the central point of the ArUco code corresponding to the virtual regular dodecahedron are translated to the original points):
Figure BDA0002889184010000071
obtained qiDe-centrode, q, of the spatial coordinates of the center point of the actual regular dodecahedral ArUco codei' is a centroid removing point of the space coordinate of the central point of the virtual regular dodecahedral ArUco code, and then q is obtainediAnd q isiThe relationship of' is as follows:
Figure BDA0002889184010000072
defining an error term:
Figure BDA0002889184010000073
the rotation matrix R of the current frame regular dodecahedron with the error term E minimized is obtained according to the formula (9), the first term in the formula (9)
Figure BDA0002889184010000074
Independent of the optimization objective, R in the second termTI is also independent of the optimization objective, the error term becomes:
Figure BDA0002889184010000075
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 BDA0002889184010000076
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 the rotation matrix R of the regular dodecahedron of the current frame is:
R＝UVT (13)。
preferably, in the third step, the rotation matrix of the regular dodecahedron of the initial frame is obtained: search AWiThe coding of the virtual regular dodecahedron ArUco code corresponding to the coding of the actual regular dodecahedron 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 dodecahedron can be obtained in the same way as the obtaining of the rotation matrix of the current frame regular dodecahedronO。
And fourthly, indirectly calculating the 6D pose of the measured object according to the space relative position relation between the regular dodecahedron and the measured object.
Preferably, the fourth step is specifically: firstly, a measured object coordinate system is constructed, the regular dodecahedron is fixed on the measured object, and the relative position and posture relation between the regular dodecahedron and the measured object is calculated (namely, the relative space coordinate t of the regular dodecahedron relative to the measured object is calculatedrefAnd relative rotation matrix Rref) Obtaining a pose matrix T of the regular dodecahedron relative to the measured objectref：
Figure BDA0002889184010000081
And then the relative pose matrix T of the measured object is as follows:
Figure BDA0002889184010000082
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, the Otsu threshold processing algorithm and the pose calculation method are known in the field.
Nothing in this specification is said to apply to the prior art.

Claims (8)

1. A monocular 6D pose estimation method based on a regular dodecahedron is characterized by comprising the following steps:
firstly, labeling the regular dodecahedron by using Aruco morphine to obtain the Aruco morpholine labeled regular dodecahedron; calibrating a monocular camera, and observing the regular dodecahedron marked by the Aruco through the calibrated monocular camera;
secondly, respectively positioning a plurality of observed Aruco in the current frame and the initial frame, and respectively obtaining the coding ids of all the observed Aruco in the current frame and the initial framei，i＝1，2，3，.., spatial coordinates A of the n and Aruco center pointsi(xi，yi，zi) N, n represents the number of ArUco observed for a frame;
thirdly, performing multivariate nonlinear fitting on the spatial coordinates of the central point of the Aruco moleculae of the current frame obtained in the second step to calculate the coordinates of the spherical center of the inscribed sphere of the regular dodecahedron of the current frame as the spatial coordinates t of the body center of the regular dodecahedron of the current frame0Performing multivariate nonlinear fitting on the spatial coordinates of the central point of the Aruco molars of the initial frame to calculate the spherical center coordinates of the tangent sphere of the dodecahedron of the initial frame as the spatial coordinates t of the body center of the dodecahedron 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 dodecahedron of the current frame relative to the initial framerel；
Constructing a virtual regular dodecahedron at the origin of a camera coordinate system, enabling the body center of the virtual regular dodecahedron to coincide with the origin of the camera coordinate system, marking the virtual regular dodecahedron by using Aruco to obtain all the programs of the Aruco of the virtual regular dodecahedron and the space coordinates A of the center points of the ArucoWi(xWi，yWi，zWi，idWi) 1,2,3, ·, 12; then the space coordinate of the central point of the current frame Aruco obtained in the second step and the coding thereof are compared with AWi(xWi，yWi，zWi，idWi) 1,2,3, 12 calculating a rotation matrix R of a regular dodecahedron of the current frame by an iterative closest point method, and compiling the spatial coordinates of the central point of the initial frame Aruco and AWi(xWi，yWi，zWi，idWi) 1,2,3, 12 calculates the rotation matrix R of the regular dodecahedron of the initial frame by the iterative closest point methodOThen by the formula
Figure FDA0002889182000000013
Calculating to obtain a relative rotation matrix R of the regular dodecahedron of the current frame relative to the initial framerel；
According to the relative rotation matrix RrelAnd relativeSpatial coordinate trelObtaining a pose matrix T of the current frame regular dodecahedron relative to the initial frame regular dodecahedronrel：
Figure FDA0002889182000000011
In the formula (2), SE (3) represents an attribute of the matrix;
fourthly, firstly, a measured object coordinate system is constructed, the regular dodecahedron is fixed on the measured object, and the relative space coordinate t of the regular dodecahedron relative to the measured object is calculatedrefAnd relative rotation matrix RrefObtaining a pose matrix T of the regular dodecahedron relative to the measured objectref：
Figure FDA0002889182000000012
And then the relative pose matrix T of the measured object is as follows:
Figure FDA0002889182000000021
and obtaining the spatial attitude change of the measured object of the current frame relative to the initial frame.
2. The monocular 6D pose estimation method based on a regular dodecahedron as claimed in claim 1, wherein in the first step, the process of marking the regular dodecahedron by using ArUco code is: 12 Aruco codes are generated by using an OpenCV image processing library, the 12 Aruco codes are respectively attached to 12 faces of the regular dodecahedron in the coding sequence, and the geometric center of the Aruco codes is coincident 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 dodecahedron as claimed in claim 1, wherein in the first step, a chessboard grid standard calibration board is used for calibrating monocular cameras, and an OpenCV image processing library is used for calibrating internal parameters of the cameras 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 FDA0002889182000000022
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 dodecahedron 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 adaptive threshold segmentation on the image acquired by the monocular camera in sequence, extracting an Aruco code contour from the segmented image, and filtering the non-convex and non-square images to further obtain a candidate region meeting the requirement.
5. The monocular pose estimation method based on regular dodecahedron of claim 4, wherein in the second step, the coded acquisition of Aruco code: firstly, perspective transformation is applied to a candidate region to obtain a standard square mark, an Otsu threshold processing algorithm is used for separating black and white color positions, and the black and white color positions are divided 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 dodecahedron 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 dodecahedron as claimed in claim 1, wherein in the third step, the spatial coordinates of the body center of the regular dodecahedron of the current frame are obtained: the space coordinate of the central point of the Aruco code on the partial surface of the regular dodecahedron observed by the current frame obtained in the second step is taken as the inscribed sphere spherical point (x) of the regular dodecahedron of the current framei，yi，zi) The sparse point cloud coordinates of (1); when the monocular camera obtains the space coordinate more than three central points of the ArUco code, the spherical surface point (x) of the inscribed sphere of the regular dodecahedron 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 inscribed sphere of the regular dodecahedron,
Figure FDA0002889182000000031
a is the edge length of a regular dodecahedron;
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 FDA0002889182000000032
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 FDA0002889182000000033
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.
8. The monocular 6D pose estimation method based on a regular dodecahedron as claimed in claim 1, wherein in the third step, the rotation matrix of the regular dodecahedron of the current frame is obtained:
search AWiAnd matching the codes of the virtual regular dodecahedron ArUco codes corresponding to the codes of the actual regular dodecahedron ArUco codes 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 dodecahedron and the spatial coordinates of the central point of the Aruco code corresponding to the actual regular dodecahedron, and the centroiding of the matching points is respectively removed by the following formula (7):
Figure FDA0002889182000000034
obtained qiDe-centrode, q, of the spatial coordinates of the center point of the actual regular dodecahedral ArUco codei' is a centroid removing point of the space coordinate of the central point of the virtual regular dodecahedral ArUco code, and then q is obtainediAnd q isiThe relationship of' is as follows:
Figure FDA0002889182000000035
defining an error term:
Figure FDA0002889182000000041
the rotation matrix R of the current frame regular dodecahedron with the error term E minimized is obtained according to the formula (9), the first term in the formula (9)
Figure FDA0002889182000000042
Independent of the optimization objective, R in the second termTI is also independent of the optimization objective, the error term becomes:
Figure FDA0002889182000000043
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 FDA0002889182000000044
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 regular dodecahedron of the current frame is:
R＝UVT (13)。
CN202110022822.XA 2021-01-08 2021-01-08 Monocular 6D pose estimation method based on regular dodecahedron Active CN112734843B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110022822.XA CN112734843B (en) 2021-01-08 2021-01-08 Monocular 6D pose estimation method based on regular dodecahedron

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110022822.XA CN112734843B (en) 2021-01-08 2021-01-08 Monocular 6D pose estimation method based on regular dodecahedron

Publications (2)

Publication Number Publication Date
CN112734843A true CN112734843A (en) 2021-04-30
CN112734843B CN112734843B (en) 2023-03-21

Family

ID=75591304

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110022822.XA Active CN112734843B (en) 2021-01-08 2021-01-08 Monocular 6D pose estimation method based on regular dodecahedron

Country Status (1)

Country Link
CN (1) CN112734843B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117351091A (en) * 2023-09-14 2024-01-05 成都飞机工业（集团）有限责任公司 Camera array calibration device and use method thereof

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107291093A (en) * 2017-07-04 2017-10-24 西北工业大学 Unmanned plane Autonomous landing regional selection method under view-based access control model SLAM complex environment
CN107292927A (en) * 2017-06-13 2017-10-24 厦门大学 A kind of symmetric motion platform's position and pose measuring method based on binocular vision
CN108168504A (en) * 2017-12-26 2018-06-15 中国人民解放军战略支援部队信息工程大学 A kind of antenna rotation center Calibration Method based on fitting centre of sphere method
CN108734737A (en) * 2018-06-14 2018-11-02 哈尔滨工业大学 The method that view-based access control model SLAM estimation spaces rotate noncooperative target shaft
CN109345588A (en) * 2018-09-20 2019-02-15 浙江工业大学 A kind of six-degree-of-freedom posture estimation method based on Tag
US20190370983A1 (en) * 2017-02-10 2019-12-05 SZ DJI Technology Co., Ltd. System and method for real-time location tracking of a drone
CN110962127A (en) * 2019-12-10 2020-04-07 南京航空航天大学 Auxiliary calibration device for tail end pose of mechanical arm and calibration method thereof
CN111307146A (en) * 2020-03-02 2020-06-19 北京航空航天大学青岛研究院 Virtual reality wears display device positioning system based on binocular camera and IMU
CN111981982A (en) * 2020-08-21 2020-11-24 北京航空航天大学 Multi-directional cooperative target optical measurement method based on weighted SFM algorithm

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190370983A1 (en) * 2017-02-10 2019-12-05 SZ DJI Technology Co., Ltd. System and method for real-time location tracking of a drone
CN107292927A (en) * 2017-06-13 2017-10-24 厦门大学 A kind of symmetric motion platform's position and pose measuring method based on binocular vision
CN107291093A (en) * 2017-07-04 2017-10-24 西北工业大学 Unmanned plane Autonomous landing regional selection method under view-based access control model SLAM complex environment
CN108168504A (en) * 2017-12-26 2018-06-15 中国人民解放军战略支援部队信息工程大学 A kind of antenna rotation center Calibration Method based on fitting centre of sphere method
CN108734737A (en) * 2018-06-14 2018-11-02 哈尔滨工业大学 The method that view-based access control model SLAM estimation spaces rotate noncooperative target shaft
CN109345588A (en) * 2018-09-20 2019-02-15 浙江工业大学 A kind of six-degree-of-freedom posture estimation method based on Tag
CN110962127A (en) * 2019-12-10 2020-04-07 南京航空航天大学 Auxiliary calibration device for tail end pose of mechanical arm and calibration method thereof
CN111307146A (en) * 2020-03-02 2020-06-19 北京航空航天大学青岛研究院 Virtual reality wears display device positioning system based on binocular camera and IMU
CN111981982A (en) * 2020-08-21 2020-11-24 北京航空航天大学 Multi-directional cooperative target optical measurement method based on weighted SFM algorithm

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
李云雷: "形貌视觉测量中立体拼接靶标的设计及应用", 《仪器仪表学报》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117351091A (en) * 2023-09-14 2024-01-05 成都飞机工业（集团）有限责任公司 Camera array calibration device and use method thereof

Also Published As

Publication number Publication date
CN112734843B (en) 2023-03-21

Similar Documents

Publication Publication Date Title
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
CN107590836B (en) Kinect-based charging pile dynamic identification and positioning method and system
CN112734844B (en) Monocular 6D pose estimation method based on octahedron
He et al. Sparse template-based 6-D pose estimation of metal parts using a monocular camera
Azad et al. Stereo-based 6d object localization for grasping with humanoid robot systems
Lin et al. Robotic grasping with multi-view image acquisition and model-based pose estimation
CN107977996B (en) Space target positioning method based on target calibration positioning model
CN112857215B (en) Monocular 6D pose estimation method based on regular icosahedron
Lambrecht et al. Towards the usage of synthetic data for marker-less pose estimation of articulated robots in rgb images
CN109766903B (en) Point cloud model curved surface matching method based on curved surface features
Zheng et al. Industrial part localization and grasping using a robotic arm guided by 2D monocular vision
Lambrecht Robust few-shot pose estimation of articulated robots using monocular cameras and deep-learning-based keypoint detection
Hu et al. Pipe pose estimation based on machine vision
CN106815585B (en) High-precision visual positioning method for hole characteristics of complex dynamic environment
CN112734843B (en) Monocular 6D pose estimation method based on regular dodecahedron
Sun et al. Automatic targetless calibration for LiDAR and camera based on instance segmentation
Liu et al. Robust 3-d object recognition via view-specific constraint
CN115760984A (en) Non-cooperative target pose measurement method based on monocular vision by cubic star
Wu et al. Object Pose Estimation with Point Cloud Data for Robot Grasping
Ngo et al. Development of a Color Object Classification and Measurement System Using Machine Vision.
Zhang et al. High-precision pose estimation method of the 3C parts by combining 2D and 3D vision for robotic grasping in assembly applications
Tellaeche et al. 6DOF pose estimation of objects for robotic manipulation. A review of different options
Li et al. Real-time CAD-level surface of revolution reconstruction on visual slam
Ko et al. 3D point cloud matching based on Its 2D representation for visual odometry

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