CN103278138B - Method for measuring three-dimensional position and posture of thin component with complex structure - Google Patents

Abstract

The invention discloses a method for measuring three-dimensional position and posture of a thin component with complex structure. The method comprises the following steps of: establishing a mapping relation between a three-dimensional world coordinate system and a two-dimensional image coordinate in a binocular vision measurement system; extracting the features on a reference posture thin component binocular image, establishing a prior feature library, extracting the features on a to-be-measured posture thin component binocular image, matching the extracted features with the prior feature library so as to obtain the same amount of feature matched pair sets corresponding to the prior feature library; obtaining a three-dimensional point cloud of feature points; calculating a main plane of the reference posture thin component three-dimensional point cloud, calculating the main plane of three-dimensional point cloud of the to-be-measured thin component and calculating the spatial position relation with the main plane of the reference posture thin component so as to obtain the three-dimensional position and posture of the to-be-measured thin component. The measurement method provided by the invention is beneficial for accurate positioning for the thin component in the technological processes of automatic manufacturing, assembly, welding, and the like of an industrial mechanical arm. According to the method, the implementation is simple and easy, the positioning accuracy is high, the cost is low, and the operation is convenient.

Description

Method for measuring three-dimensional position and posture of thin part with complex structure

Technical Field

The invention belongs to the field of visual measurement in information technology, and particularly relates to a visual measurement method for three-dimensional positions and postures of complex thin parts.

Background

The binocular vision-based measurement method can realize three-dimensional size measurement, positioning and online quality control of parts on an industrial production line, is an important research field in the current information technology, and is already applied to the aspects of measurement, positioning and the like of common small-size parts. For three-dimensional position and attitude measurement of thin parts (such as side walls and doors of a body-in-white of an automobile) with large size and complex structure, no clear application is seen.

Disclosure of Invention

In order to solve the above problems, the present invention provides a method for measuring a three-dimensional position and posture of a thin component to be measured, comprising:

step 1: calibrating the internal parameters and the external parameters of the binocular camera and the spatial position relationship of the binocular camera, and establishing a mapping relationship between a three-dimensional world coordinate system and a two-dimensional image coordinate in a binocular vision measurement system;

step 2: extracting SIFT feature point pairs in the binocular image of the reference thin component by using an SIFT operator to generate an SIFT prior feature library, and using a three-dimensional plane where the SIFT feature point pairs are located as a reference plane where the reference thin component is located;

and step 3: respectively extracting a plurality of SIFT feature point pairs from the binocular stereo correction image pair of the thin component to be detected, and matching the extracted SIFT feature point pairs with SIFT feature point pairs in the SIFT prior feature library to obtain an SIFT feature point pair set which has the same number and corresponds to the SIFT feature point pairs in the SIFT feature prior library;

and 4, step 4: obtaining three-dimensional point cloud of the thin part to be measured according to the obtained image coordinates of the feature points in the SIFT feature point pair set, and further obtaining a space plane of the thin part to be measured;

and 5: and determining the three-dimensional position and attitude information of the thin component to be measured by solving the transformation matrix between the space plane and the reference plane.

The invention adopts a binocular vision measurement technology: (1) selecting proper cameras and lenses according to an application scene and a visual field range, building a binocular vision system, respectively calibrating the left camera and the right camera to obtain internal parameters and lens distortion coefficients of the left camera and the right camera, obtaining external parameters of the left camera and the right camera according to a world coordinate system on a reference plane, and then performing binocular stereo calibration to further obtain a spatial position relation between the left camera and the right camera; (2) the method comprises the steps of sequentially carrying out lens distortion correction and stereo correction on collected binocular images, respectively obtaining a binocular distortion correction image pair and a binocular stereo correction image pair, extracting SIFT characteristics of the binocular stereo correction image pair of a reference pose thin component, manually selecting SIFT characteristics of specific positions (such as positions of a positioning hole, a positioning bolt and the like) of the thin component, obtaining a binocular characteristic matching pair set of the thin component, taking the binocular characteristic matching pair set as an SIFT priori characteristic library, extracting SIFT characteristics of the binocular stereo correction image pair of the thin component to be detected, carrying out characteristic matching on the SIFT characteristics and the priori characteristic library, and obtaining the same number of and corresponding characteristic matching pair sets; (3) obtaining three-dimensional coordinates of the feature matching pair set by a binocular measurement method, and obtaining three-dimensional point cloud of feature points; (4) and respectively calculating main planes of the three-dimensional point cloud of the reference pose thin part and the three-dimensional point cloud of the thin part to be measured according to a least square method, and obtaining the three-dimensional position and the pose of the thin part to be measured by calculating the spatial position relation between the two main planes. The three-dimensional position and posture measuring method for the complex thin part is beneficial to accurate positioning of the thin part in the technical processes of automatic manufacturing, assembling, welding and the like of an industrial robot, and is high in positioning accuracy, low in cost and convenient to operate.

Drawings

FIG. 1 is a general block diagram of a method for measuring three-dimensional position and attitude of a complex thin part according to the present invention;

FIG. 2 is a diagram of a binocular camera imaging model and a physical imaging and normalized imaging coordinate system in accordance with the present invention;

FIG. 3 is a schematic view of the binocular camera stereo calibration of the present invention;

FIG. 4 is a flow chart of feature matching in the present invention;

FIG. 5 is a schematic diagram of the acquisition of missing SIFT features in the present invention;

FIG. 6 is a flow chart of the calculation of the feature point three-dimensional point cloud in the present invention.

Detailed Description

In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to specific embodiments and the accompanying drawings.

The invention discloses a visual measurement method for three-dimensional positions and postures of complex thin parts.

Fig. 1 shows a general flow chart of a visual measurement method for three-dimensional position and posture of a complex thin part in the invention. As shown in FIG. 1, the method is divided into two stages of reference pose measurement and pose estimation to be measured. Wherein, benchmark position appearance measurement stage includes: the method comprises the following steps of camera calibration, binocular image synchronous acquisition, binocular image distortion correction and stereo correction, binocular image pair SIFT feature extraction, prior feature set acquisition, reference pose thin component three-dimensional point cloud acquisition, reference pose thin component three-dimensional pose estimation and the like; the to-be-detected pose estimation stage comprises the following steps: the method comprises the steps of binocular image synchronous acquisition, binocular image pair distortion correction and stereo correction, binocular image pair SIFT feature extraction, feature set acquisition of a pose thin component to be detected, three-dimensional point cloud acquisition of the pose thin component to be detected, pose estimation of the pose thin component to be detected and the like.

Because the implementation modes of the corresponding steps in the reference pose measurement and the pose estimation to be measured are the same, the reference pose measurement and the pose estimation to be measured are summarized and summarized into four steps for expansion description, namely camera calibration, feature extraction and thin component characterization, binocular vision measurement and three-dimensional point cloud acquisition and thin component three-dimensional position and pose estimation, and the steps of the invention are further described in detail by combining the related drawings.

The invention discloses a visual measurement method for three-dimensional positions and postures of complex thin parts, which specifically comprises the following steps:

step 1: and calibrating the camera. The method comprises the steps of calibrating the internal parameters of a camera, a lens distortion coefficient and the spatial position relation between binocular cameras to establish the mapping relation between a reference plane three-dimensional world coordinate system and two-dimensional image coordinates in a binocular vision measuring system.

The whole working range is set to be 5 m multiplied by 5 m, and a lens with a proper view field is selected for the view field. Taking these factors into consideration, it is preferable to select a lens having a focal length of 6mm (the angle of view is about 53 degrees). The binocular cameras were placed horizontally with the spacing set at 60cm, with the left camera as the reference camera.

Camera calibration mainly involves four coordinate systems, i.e. three-dimensional world coordinate system X_wY_wZ_w(origin on the reference plane, and Z_wAxis perpendicular to reference plane), three-dimensional camera coordinate system X_cY_cZ_c(origin is at the optical center of the lens, and Z_cAxis coincident with optical axis), two-dimensional image physical coordinate system xO₁y (origin at the center of the image, coordinates are physical coordinates), a two-dimensional image pixel coordinate system uOv (origin at the top left corner of the image, coordinates are pixel coordinates).

Figure 2 shows a schematic diagram of the various coordinate systems and their normalized coordinate systems of the present invention. As shown in FIG. 2, the coordinate of the space point P in the three-dimensional world coordinate system is defined as [ X ] by using the linear camera pinhole model_w Y_w Z_w]^TWith the corresponding homogeneous coordinate P ═ X_w Y_w Z_w 1]^T(ii) a Defining the coordinate of the point P in the three-dimensional camera coordinate system as [ X ]_c Y_c Z_c]^TWith homogeneous coordinate P_c＝[X_c Y_c Z_c 1]^T(ii) a Defining the projection point of the point P on the two-dimensional image plane as P', and the coordinate of the two-dimensional image in the physical coordinate system as [ x y ]]^T(unit: mm) having coordinates in a two-dimensional image pixel coordinate system of [ u v%]^TIts homogeneous coordinate is p' ═ u v1]^T. Intersection O of the optical axis of the camera and the image plane₁Has a pixel coordinate of [ u ]₀ v₀]^T(ii) a The physical dimensions of the image unit pixel in the x-axis and y-axis directions are dx and dy, respectively. The mapping relationship between the coordinates of the point P in the three-dimensional coordinate system and the coordinates of its projected point P' on the two-dimensional image plane in the two-dimensional image coordinates is:

$Z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{cccc}\frac{f}{\mathrm{dx}}& 0& u_0& 0\\ 0& \frac{f}{\mathrm{dy}}& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& t\\ O^t& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]---\left(1\right)$

$=\left[\begin{array}{cccc}{a}_x& 0& u_0& 0\\ 0& a_y& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& t\\ O^t& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=M_1{M}_{2}P=\mathrm{MP}$

wherein, P ═ X_w Y_w Z_w 1]^TIs a homogeneous coordinate under a three-dimensional world coordinate system; f is the focal length of the lens, a_x＝f/dx、a_yF/dy is the focal ratio of the camera, and R, t is the rotation matrix and translation vector between the three-dimensional camera coordinate system and the three-dimensional world coordinate system respectively; o is^t＝[0 0 0]，M₁、M₂Respectively, an intrinsic parameter matrix and an extrinsic parameter matrix (i.e., a homography matrix) of the camera, and M is an overall projection matrix of the camera.

In the whole calibration process, the internal parameter [ f dx dy u ] of the camera needs to be calibrated₀ v₀]^TDistortion coefficient [ k ] of camera lens₁ k₂ p₁ p₂ k₃]^TAnd external parameters, wherein the internal parameters comprise the focal ratio and the central point position of the camera, and the external parameters comprise a rotation matrix R and a translational vector t between a three-dimensional camera coordinate system and a three-dimensional world coordinate system. In the calibration process, a black and white square grid calibration plate is used for calibration, wherein a small calibration plate is used for calibrating internal parameters, the grid size is 50mm multiplied by 50mm, and the number of grids is 8 (width) multiplied by 12 (length); when external parameters are calibrated, a large calibration plate is used, the size of the grid is 100mm multiplied by 100mm, and the number of the grids is 4 (width) multiplied by 6 (length). And detecting each angular point on the calibration plate by adopting a Harris angular point detection algorithm, and acquiring the position of the angular point at the sub-pixel level.

The step 1 specifically comprises the following steps:

step 11: and respectively calibrating the left camera and the right camera to respectively obtain an internal parameter matrix and an external parameter matrix of each camera.

(a) Initial intrinsic parameters of the left and right cameras and the rotation matrix and translation vector of the cameras relative to the small calibration plate are first acquired.

Taking calibration of a reference camera as an example, in order to obtain an internal parameter matrix of the camera, lens distortion is not considered firstly, a calibration method based on a plane homography matrix is adopted, a small calibration plate is held by hand, and the calibration method is not carried outChanging the postures (at least 3 postures) of the small calibration plate, and calculating initial internal parameters of the camera and a rotation matrix and a translation vector of the camera relative to the small calibration plate by using plane matching (a direct linear transformation method) among different viewpoints; that is, the internal parameters of the camera and the rotation matrix R and translation vector t of the camera relative to the small calibration plate when the lens distortion is zero are obtained. Let the plane Z of the small calibration plate_wWhen the value is 0, then:

$Z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{cccc}{a}_x& 0& u_0& 0\\ 0& a_y& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& t\\ O^t& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ 0\\ 1\end{array}\right]=\left[\begin{array}{ccc}{a}_x& 0& u_0\\ 0& a_y& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cc}R& t\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ 0\\ 1\end{array}\right]\left[\mathrm{0,1}\right]$

$=\left[\begin{array}{ccc}{a}_x& 0& u_0\\ 0& a_y& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{r}_1& r_2& r_3& t\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ 0\\ 1\end{array}\right]=\left[\begin{array}{ccc}{a}_x& 0& u_0\\ 0& a_y& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{r}_1& r_2& t\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ 1\end{array}\right]---\left(2\right)$

<math> <mrow> <mo>=</mo> <mi>H</mi> <mo>&CenterDot;</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>X</mi> <mi>w</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Y</mi> <mi>w</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </math>

wherein R ═ R₁ r₂ r₃]。

Therefore, the calibration of the reference camera can be completed by solving the H matrix, and the internal parameters of the camera and the rotation matrix R and translation vector t of the camera relative to the small calibration plate are obtained.

(b) Based on the initial internal parameters of the camera and the rotation matrix R and translation vector t of the camera relative to the small calibration plate, the distortion coefficient of the camera lens is further obtained by considering the lens distortion, and the internal parameters of the camera are further optimized.

Distortion diagram of original lensThe image pixel coordinate of a corner point detected in the image is [ u ]_raw v_raw]^TThe image pixel coordinate of the angular point in the lens distortion-free image under the ideal pinhole imaging model is [ u [ ]_und v_und]^TAnd wherein, the original lens distortion image is an original collected image with lens distortion, then:

(1) and transforming the three-dimensional world coordinate of each corner point on the small calibration plate into a three-dimensional camera coordinate system, wherein one corner point on the small calibration plate is taken as an original point, the three-dimensional world coordinate system is established, and the three-dimensional world coordinate of each corner point on the calibration plate is calculated according to the length of black and white grids on the small calibration plate. Namely, it is

$\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\end{array}\right]=R\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]+t---\left(3\right)$

Where R and t are the rotation matrix and translation vector of the camera relative to the small calibration plate.

(2) Further projecting to an image plane to obtain the undistorted image physical coordinate [ x ] of the angular point in the image plane coordinate system_und y_und]^TAnd image pixel coordinate [ u ]_und v_und]^TI.e. by

$\left[\begin{array}{c}{x}_{\mathrm{und}}\\ y_{\mathrm{und}}\end{array}\right]=\left[\begin{array}{c}{\mathrm{fX}}_c/Z_c\\ f{Y}_c/Z_c\end{array}\right]---\left(4\right)$

$\left[\begin{array}{c}{u}_{\mathrm{und}}\\ v_{\mathrm{und}}\end{array}\right]\left[\begin{array}{ccc}{a}_x& 0& u_0\\ 0& a_y& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{\mathrm{und}}\\ y_{\mathrm{und}}\end{array}\right]---\left(5\right)$

(3) Image pixel coordinates [ u ] of corner points to be detected in an original lens distorted image_raw v_raw]^TConversion to physical coordinates [ x ] of the image_raw y_raw]^TAnd introducing a certain initially set lens distortion coefficient [ k ]₁ k₂ p₁ p₂ k₃]^TObtaining the pixel coordinates [ u 'of the corner image after distortion correction'_und v′_und]^TAnd image physical coordinates [ x'_und y′_und]^TI.e. by

$\left[\begin{array}{c}{x}_{\mathrm{raw}}\\ y_{\mathrm{raw}}\\ 1\end{array}\right]=\left[\begin{array}{ccc}\mathrm{dx}& 0& -u_0\mathrm{dx}\\ 0& \mathrm{dy}& -v_0\mathrm{dy}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{u}_{\mathrm{raw}}\\ v_{\mathrm{raw}}\\ 1\end{array}\right]---\left(6\right)$

<math> <mrow> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <msup> <mi>x</mi> <mo>&prime;</mo> </msup> <mi>und</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <msup> <mi>y</mi> <mo>&prime;</mo> </msup> <mi>und</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>+</mo> <msub> <mi>k</mi> <mn>1</mn> </msub> <msup> <mi>r</mi> <mn>2</mn> </msup> <mo>+</mo> <msub> <mi>k</mi> <mn>2</mn> </msub> <msup> <mi>r</mi> <mn>4</mn> </msup> <mo>+</mo> <msub> <mi>k</mi> <mn>3</mn> </msub> <msup> <mi>r</mi> <mn>6</mn> </msup> <mo>)</mo> </mrow> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>x</mi> <mi>raw</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>y</mi> <mi>raw</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mn>2</mn> <msub> <mi>p</mi> <mn>1</mn> </msub> <msub> <mi>x</mi> <mi>d</mi> </msub> <msub> <mi>y</mi> <mi>d</mi> </msub> <mo>+</mo> <msub> <mi>p</mi> <mn>2</mn> </msub> <mrow> <mo>(</mo> <msup> <mi>r</mi> <mn>2</mn> </msup> <mo>+</mo> <mn>2</mn> <msubsup> <mi>x</mi> <mi>d</mi> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>p</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <msup> <mi>r</mi> <mn>2</mn> </msup> <mo>+</mo> <mn>2</mn> <msubsup> <mi>y</mi> <mi>d</mi> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> <mo>+</mo> <mn>2</mn> <msub> <mi>p</mi> <mn>2</mn> </msub> <msub> <mi>x</mi> <mi>d</mi> </msub> <msub> <mi>y</mi> <mi>d</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> </mrow> </math>

<math> <mrow> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <msup> <mi>u</mi> <mo>&prime;</mo> </msup> <mi>und</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <msup> <mi>v</mi> <mo>&prime;</mo> </msup> <mi>und</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>a</mi> <mi>x</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>u</mi> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>a</mi> <mi>y</mi> </msub> </mtd> <mtd> <msub> <mi>v</mi> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <msup> <mi>x</mi> <mo>&prime;</mo> </msup> <mi>und</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <msup> <mi>y</mi> <mo>&prime;</mo> </msup> <mi>und</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow> </math>

Wherein,in the process, the image pixel coordinates of the corner points after distortion correction and the image pixel coordinates [ u ] of the corner points detected in the original lens distorted image are established according to the internal parameters and the distortion parameters of the camera_raw v_raw]^TLinear equation between.

(4) For the N angular points used in the calibration, an objective function is defined:

<math> <mrow> <mi>min</mi> <mi>F</mi> <mo>=</mo> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <mo>[</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>u</mi> <mi>und</mi> <mi>i</mi> </msubsup> <mo>-</mo> <msubsup> <mi>u</mi> <mi>und</mi> <mrow> <mo>&prime;</mo> <mi>i</mi> </mrow> </msubsup> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>v</mi> <mi>und</mi> <mi>i</mi> </msubsup> <mo>-</mo> <msubsup> <mi>v</mi> <mi>und</mi> <mrow> <mo>&prime;</mo> <mi>i</mi> </mrow> </msubsup> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>]</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow> </math>

the nonlinear least square problem is iterated for many times through a nonlinear optimization algorithm to obtain a parameter value which enables a target function to be minimum, and global optimization is obtainedCamera internal parameter of [ f dx dy u ]₀ v₀]^TAnd distortion parameter [ k₁ k₂ p₁ p₂k₃]^T。

(c) And calibrating external parameters of the camera, namely the spatial position relation between the image plane and the reference plane, and obtaining a rotation matrix and a translation vector of the camera plane relative to the reference plane.

The invention needs to measure the three-dimensional coordinates of the feature points at certain specific positions on the complex thin part, so that an external parameter matrix of the camera when the reference plane is taken as a world coordinate system needs to be obtained. Placing the large calibration plate on the reference plane, namely setting the reference plane in the scene as a world coordinate system X_WO_WY_WThe method comprises the following specific steps:

(1) determining a three-dimensional world coordinate system X_wY_wZ_wPlacing the large calibration plate on the reference plane, taking one angular point of the grid on the large calibration plate as the origin of the coordinate system, and making Z_wThe axis is vertical to the reference plane of the large calibration plate;

(2) acquiring a large calibration plate image at the moment, and carrying out distortion correction by using the previously acquired camera internal parameters and the lens distortion coefficient to acquire a corrected image;

(3) detecting the pixel coordinates of a two-dimensional image of each corner point in the image after distortion correction by using a Harris corner point detection algorithm;

(4) and calculating a rotation matrix and a translation vector of the camera relative to the large calibration plate, namely external parameters of the camera according to the three-dimensional world coordinates of each corner point on the large calibration plate and the three-dimensional camera coordinates of each corner point detected on the distortion correction image under the three-dimensional camera coordinate system.

Step 12: and (4) binocular stereo calibration, and establishing a spatial position relation between binocular cameras. Namely, a rotation matrix and a translation matrix, namely a homography matrix, of the right camera relative to the reference camera are obtained, and the method specifically comprises the following steps:

fig. 3 shows a schematic perspective view of a binocular camera according to the present invention. And solving the spatial position relation between the right camera and the reference camera when the large calibration plate is on the reference plane according to the rotation matrix and the translation vector of the left camera and the right camera relative to the reference plane, namely obtaining the rotation matrix and the translation vector of the right camera relative to the left camera when the target object is on the horizontal ground. As shown in fig. 3, the space point P in the three-dimensional world coordinate system is in the three-dimensional camera coordinate system of the left camera and the right camera (the optical center or the origin is O, respectively)_ClAnd O_CR) The lower spatial point is respectively P_ClAnd P_CRLet the extrinsic parameter matrices of the left and right cameras be [ R ] respectively_l t_l]And [ R ]_r t_r]Then according to

P_Cl＝R_lP+t_l

P_Cr＝R_rP+t_r

P_Cl＝R^T(P_Cr-t)

The spatial position relation [ R t ] of the right camera relative to the left camera is obtained as

$R=R_r{R}_l^T---\left(10\right)$

t＝t_r-Rt_l (11)

Step 13: and performing stereo correction on the binocular images to realize coplanarity and line alignment of the binocular images. Even if the optical axes of the two cameras are parallel, the image planes of the left camera and the right camera are re-projected to accurately fall on the same plane, and the lines of the left image and the right image are aligned at the same time. Based on the rotation matrix and translation vector between the left and right cameras, etc., adoptThe view overlapping maximization criterion and the Bouguet calibration algorithm realize the stereo correction to obtain the stereo correction internal matrix M of the left camera_{rect_l}And a row alignment matrix R_{rect_l}(ii) a Stereo-corrected internal matrix M for right camera_{red_r}And a row alignment matrix R_{rect_r}So as to obtain binocular stereo corrected image pair. The specific operation process of the stereo correction is as follows:

let image coordinate P of point P in binocular original image pair_{raw_l}And P_{raw_r}(ii) a Image coordinates P in a binocular distortion corrected image pair_{und_l}And P_{und_l}(ii) a Image coordinates P in a binocular stereo-corrected image pair_{rect_l}And P_{rect_r}Then, the stereo correction formula is:

P_{rect_l}＝M_{rect_l}R_l(M_1l)^-1P_{und_l} (12)

P_{rect_r}＝M_{rect_r}R_r(M_1r)^-1P_{und_r} (13)

wherein R is_lAnd R_rThe rotation matrixes of the external parameters of the left camera and the right camera are respectively.

P_{und_l}＝M_1l·UnDistort(M_1lP_{raw_l})

P_{und_r}＝M_1r·UnDistort(M_1rP_{raw_r})

In the formula,

UnDistort (·) is a distortion correction function for a pixel.

Step 2: feature extraction and thin part characterization. In the thin component binocular stereo correction image pair, SIFT features and neighborhood descriptions are used to represent the thin component images respectively.

FIG. 4 shows a flow chart of a method for feature extraction and thin part characterization in the present invention. Referring to fig. 4, the method specifically includes the following steps:

step 21: acquiring binocular images of the thin component, and respectively carrying out lens distortion correction and stereo correction to obtain a binocular distortion correction image pair and a binocular stereo correction image pair;

step 22: extracting SIFT features in the binocular stereo correction image pair by using an SIFT operator, wherein the SIFT features comprise SIFT feature points and neighborhood descriptions thereof;

step 23: and acquiring a priori feature set and characterizing the thin part.

In the reference pose measuring stage, aiming at the detected SIFT features in the left image of the binocular stereo correction image of the thin component, manually selecting a plurality of (at least 3) SIFT features near specific positions (such as positioning holes, positioning bolts and the like), carrying out feature matching on the SIFT features and SIFT features in the right image, manually rejecting mismatching pairs, obtaining a SIFT feature matching pair set, and further forming a prior SIFT feature library of the reference pose thin component.

In the pose estimation stage to be detected, SIFT features extracted from binocular stereo correction image pairs of thin components to be detected are respectively subjected to feature matching with a prior SIFT feature library, and a RANSAC algorithm is used for eliminating mismatching results to form SIFT feature matching pairs which are the same in number and correspond to the prior SIFT feature library. The matching purpose is that if the SIFT feature logarithm in the binocular stereo correction image pair of the thin component to be detected is less than the feature number of the prior SIFT feature library, the missing SIFT feature is obtained through transformation according to the homography matrix and the SIFT feature corresponding to the prior feature library.

Fig. 5 shows a schematic diagram of an acquisition mode of missing SIFT features in the present invention. As shown in fig. 5, a prior SIFT feature library (composed of 8 pairs of SIFT feature matching pairs) and 7 pairs of SIFT feature matching pairs extracted from a binocular stereo-corrected image pair of a thin component to be detected are provided, and the matching pairs of SIFT features with missing compensation are obtained according to a homography matrix H between two left images₁And point P₆Calculating the missing SIFT feature point P in the left image of the binocular stereo corrected image of the thin component to be measured₆And finally obtaining a SIFT feature matching pair set of the thin parts to be tested, wherein the SIFT feature matching pair set is the same as the SIFT feature library in number and corresponds to the SIFT feature library in a priori mode.

And step 3: binocular vision measurement and reference thin part three-dimensional point cloud acquisition.

FIG. 6 shows a schematic view of binocular vision measurement and thin part three-dimensional point cloud acquisition in the present invention. In the invention, the three-dimensional world coordinates of the feature point set at the specific position on the thin component are calculated by a binocular measurement method, so that the three-dimensional point cloud of the thin component is obtained.

Taking the three-dimensional coordinate calculation of a point P at a certain specific position on the three-dimensional space thin component as an example, the calculation process is described, wherein the imaging of the point P in the binocular stereo correction image pair corresponds to a certain SIFT feature matching pair. The coordinates of the point P under the coordinate system of the three-dimensional camera of the binocular left camera and the binocular right camera are respectively [ X ]_cl Y_cl Z_cl]^TAnd [ X ]_cr Y_cr Z_cr]^T(ii) a The image coordinates of the SIFT feature matching pair in the binocular stereo correction image pair are [ u ] respectively_{rect_l} v_{rect_l}]^TAnd [ u ]_{rect_r} v_{rect_r}]^TWith homogeneous coordinates P_{rect_l}＝[u_{rect_l} v_rect__l 1]^TAnd P_{rect_r}＝[u_{rect_r} v_{rect_r} 1]^T. As shown in fig. 6, the binocular vision measurement and the thin component three-dimensional point cloud acquisition specifically include the following steps:

step 31: and calculating the image coordinates of the SIFT feature points in the binocular distortion correction image pair. In the reference pose measuring stage, calculating image coordinates of binocular distortion correction image pairs acquired by SIFT feature points on a reference thin component on the reference thin component; and in the measurement stage of the pose to be measured, calculating the image coordinates of the binocular distortion correction image pair acquired by the SIFT feature points on the thin component to be measured on the component to be measured.

Let the image coordinates of point P on the binocular left and right distortion corrected image pair be [ u ] respectively_{und_l} v_{und_l}]^TAnd [ u ]_{und_r} v_{und_r}]^TTheir corresponding homogeneous coordinates are each P_{und_l}＝[u_{und_l} v_{und_l} 1]^TAnd P_{und_r}＝[u_{und_r} v_{und_r} 1]^TAccording to the formula (1):

$Z_{\mathrm{cl}}\left[\begin{array}{c}{u}_{\mathrm{und}\_l}\\ v_{\mathrm{und}\_l}\\ 1\end{array}\right]=M_{1l}{M}_{2l}\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=M_l\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=\left[\begin{array}{cccc}{m}_{l11}& m_{l12}& m_{l13}& m_{l14}\\ m_{l21}& m_{l22}& m_{l23}& m_{l24}\\ m_{l31}& m_{l32}& m_{l33}& m_{l34}\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]---\left(14\right)$

$Z_{\mathrm{cr}}\left[\begin{array}{c}{u}_{\mathrm{und}\_r}\\ v_{\mathrm{und}\_r}\\ 1\end{array}\right]=M_{1r}{M}_{2r}\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=M_r\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=\left[\begin{array}{cccc}{m}_{r11}& m_{r12}& m_{r13}& m_{r14}\\ m_{r21}& m_{r22}& m_{r23}& m_{r24}\\ m_{r31}& m_{r32}& m_{r33}& m_{r34}\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]---\left(15\right)$

in the formula, M_1l、M_2lAnd M_1r、M_2rInternal parameter matrix and external parameter matrix, M, of binocular left and right cameras, respectively_lAnd M_rRespectively, an overall projection matrix of the binocular left and right cameras, an

$M_l=\left[\begin{array}{cccc}{m}_{l11}& m_{l12}& m_{l13}& m_{l14}\\ m_{l21}& m_{l22}& m_{l23}& m_{l24}\\ m_{l31}& m_{l32}& m_{l33}& m_{l34}\end{array}\right]$

$M_r=\left[\begin{array}{cccc}{m}_{r11}& m_{r12}& m_{r13}& m_{r14}\\ m_{r21}& m_{r22}& m_{r23}& m_{r24}\\ m_{r31}& m_{r32}& m_{r33}& m_{r34}\end{array}\right]$

Knowing the image coordinates P of this point in the binocular stereo-corrected image pair_{rect_l}And P_{rect_r}Then the point is at the image coordinate P in the binocular distortion corrected image pair_{und_l}And P_{und_l}Respectively as follows:

P_{und_l}＝M_1l(M_{rect_l}R_l)^-1P_{rect_l}

P_{und_r}＝M_1r(M_{rect_r}R_r)^-1P_{rect_r}step 32: and calculating the three-dimensional coordinates of the SIFT feature points in a world coordinate system. In the reference pose measuring stage, three-dimensional coordinates under a world coordinate system are calculated according to the image coordinates of the SIFT feature points of the reference thin component; in the measurement stage of the pose to be measured, calculating a three-dimensional coordinate under a world coordinate system according to the image coordinate of the SIFT feature points of the thin component to be measured;

image coordinate P of the point on the binocular distortion corrected image_{und_l}And P_{und_r}Respectively substituted into pinhole camera model formulas (14) andequation (15), the overall parameter matrix M combining the left and right cameras_lAnd M_rElimination of Z_clAnd Z_crTo obtain the equation set:

$\left\{\begin{array}{c}(m_{l11}-m_{l31}{u}_{\mathrm{und}\_l})X_w+(m_{l12}-m_{l32}{u}_{\mathrm{und}\_l})Y_w+(m_{l13}-m_{l33}{u}_{\mathrm{und}\_l})Z_w=m_{134}{u}_{\mathrm{und}\_l}-m_{l14}\\ (m_{l21}-m_{l31}{v}_{\mathrm{und}\_l})X_w+(m_{l22}-m_{l32}{v}_{\mathrm{und}\_l})Y_w+(m_{l23}-m_{l33}{v}_{\mathrm{und}\_l})Z_w=m_{134}{v}_{\mathrm{und}\_l}-m_{l24}\\ (m_{r11}-m_{r31}{u}_{\mathrm{und}\_r})X_x+(m_{r12}-m_{r32}{u}_{\mathrm{und}\_r})Y_w+(m_{r13}-m_{r33}{u}_{\mathrm{und}\_r})Z_w=m_{r34}{u}_{\mathrm{und}\_r}-m_{r14}\\ (m_{r21}-m_{r31}{v}_{\mathrm{und}-r})X_w+(m_{r22}-m_{r32}{v}_{\mathrm{und}\_r})Y_w+(m_{r23}-m_{r33}{v}_{\mathrm{und}\_r})Z_w=m_{r34}{v}_{\mathrm{und}\_r}-m_{r24}\end{array}\right.$

writing the system of equations in matrix form:

$A=\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\end{array}\right]=B$

in the formula,

$A=\left[\begin{array}{ccc}{m}_{l11}-m_{l31}{u}_{\mathrm{und}\_l}& m_{l12}-m_{l32}{u}_{\mathrm{und}\_l}& m_{l13}-m_{l33}{u}_{\mathrm{und}\_l}\\ m_{l21}-m_{l31}{v}_{\mathrm{und}\_l}& m_{l22}-m_{l32}{v}_{\mathrm{und}\_l}& m_{l23}-m_{l33}{v}_{\mathrm{und}\_l}\\ m_{r11}-m_{r31}{u}_{\mathrm{und}\_r}& m_{r12}-m_{r32}{u}_{\mathrm{und}\_r}& m_{r13}-m_{r33}{u}_{\mathrm{und}\_r}\\ m_{r21}-m_{r31}{v}_{\mathrm{und}\_r}& m_{r22}-m_{r32}{v}_{\mathrm{und}\_r}& m_{r23}-m_{r33}{v}_{\mathrm{und}\_r}\end{array}\right]$

$B=\left[\begin{array}{c}{m}_{l34}{u}_{\mathrm{und}\_l}-m_{l14}\\ m_{l34}{v}_{\mathrm{und}\_l}-m_{l24}\\ m_{r34}{u}_{\mathrm{und}\_r}-m_{r14}\\ m_{r34}{v}_{\mathrm{und}\_r}-m_{r24}\end{array}\right]$

and obtaining the three-dimensional coordinates of the point in the world coordinate system by using a least square method.

[X_w Y_w Z_w]^T＝((A^TA)^-1A^TB) (16)

And 4, step 4: thin part three-dimensional position and attitude estimation.

And respectively calculating main planes of the three-dimensional point cloud of the reference pose thin part and the three-dimensional point cloud of the thin part to be measured according to a least square method, and obtaining the three-dimensional position and the pose of the thin part to be measured by calculating the spatial position relation between the two main planes.

Step 41: calculating a space main plane of the three-dimensional point cloud; namely, in the reference pose measurement stage, the three-dimensional plane where the three-dimensional point cloud of the SIFT feature points of the reference thin component is located is taken as the main plane pi of the reference space₀(ii) a In the measurement stage of the pose to be measured, taking the plane of the three-dimensional power supply of the SIFT feature points of the thin component to be measured as a main plane pi of a space to be measured;

calculating a space reference main plane pi of the three-dimensional point cloud of the reference pose thin part by adopting a least square method₀(ii) a And calculating a main plane pi to be measured in the space of the thin part to be measured according to the three-dimensional point cloud of the thin part to be measured. Taking the solution of the main plane pi to be measured in space as an example, let the equation of the space plane pi be: ax + by + cz +1 is 0, the plane pi normal vector n is (a, b, c), and the three-dimensional coordinate of the feature point at the specific position is P_i＝(x_i，y_i，z_i)^TThen point P_iDistance d to the fitting plane pi_i＝nP_i+1, according to the principle of least squares, requires S ═ Σ (d)_i)²Then the normal vector n is solved by solving the following system of equations.

<math> <mrow> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <mfrac> <mrow> <mo>&PartialD;</mo> <mi>S</mi> </mrow> <mi>a</mi> </mfrac> <mo>=</mo> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mfrac> <mrow> <mo>&PartialD;</mo> <mi>S</mi> </mrow> <mi>b</mi> </mfrac> <mo>=</mo> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mfrac> <mrow> <mo>&PartialD;</mo> <mi>S</mi> </mrow> <mi>c</mi> </mfrac> <mo>=</mo> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>17</mn> <mo>)</mo> </mrow> </mrow> </math>

Step 42: solving the main plane Pi of the space to be measured and the main plane Pi of the reference space₀The transformation matrix between the two is used for respectively projecting the characteristic points of the reference thin component and the thin component to be measured onto the main planes of the thin component to be measured, and the projection points on the main planes are used for calculating the rotation matrix and the translation vector between the two planes, so that the three-dimensional position and the posture information of the thin component to be measured relative to the reference thin component are accurately determined.

In the description of the steps of the method, for convenience, the reference pose measurement stage and the pose measurement stage to be measured are described in parallel, but in practical application, the camera calibration is firstly carried out, then the reference pose measurement, namely the acquisition of a prior feature library, the calculation of a reference plane and the like are carried out, and finally the pose measurement stage to be measured is carried out, namely the three-dimensional position and attitude information of the actual thin part to be measured are solved based on the reference plane.

The above-mentioned embodiments are intended to illustrate the objects, technical solutions and advantages of the present invention in further detail, and it should be understood that the above-mentioned embodiments are only exemplary embodiments of the present invention, and are not intended to limit the present invention, and any modifications, equivalents, improvements and the like made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (10)

1. A method for measuring the three-dimensional position and posture of a thin part to be measured comprises the following steps:

step 1: calibrating the internal parameters and the external parameters of the binocular camera and the spatial position relationship of the binocular camera, and establishing a mapping relationship between a three-dimensional world coordinate system and a two-dimensional image coordinate in a binocular vision measurement system;

step 2: extracting SIFT feature point pairs in a binocular image of the reference thin component by using an SIFT operator, generating an SIFT prior feature library, and using a three-dimensional plane where the SIFT feature point pairs are located as a reference plane where the reference thin component is located;

and step 3: respectively extracting a plurality of SIFT feature point pairs from the binocular stereo correction image pair of the thin component to be detected, and matching the extracted SIFT feature point pairs with SIFT feature point pairs in the SIFT prior feature library to obtain an SIFT feature point pair set which has the same number and corresponds to the SIFT feature point pairs in the SIFT feature prior library;

and 4, step 4: obtaining three-dimensional point cloud of the thin part to be measured according to the obtained image coordinates of the feature points in the SIFT feature point pair set, and further obtaining a space plane of the thin part to be measured;

and 5: and determining the three-dimensional position and attitude information of the thin component to be measured by solving the transformation matrix between the space plane and the reference plane.

2. The measurement method according to claim 1, wherein the step 1 specifically comprises:

step 11: respectively establishing a mapping relation between a single-camera two-dimensional image coordinate system and a three-dimensional world coordinate system;

step 12: calibrating a single camera respectively to obtain internal parameters and external parameters of the camera;

step 13: and carrying out three-dimensional calibration according to the obtained internal parameters and external parameters of the camera, and establishing a spatial position relationship between the binocular cameras.

3. The measurement method of claim 2, wherein the internal parameters of the camera include a focal ratio of the camera and a center point position of the camera, and the external parameters include a rotation matrix and a translation vector between a three-dimensional camera coordinate system and a three-dimensional world coordinate system.

4. The measurement method according to claim 2, wherein the spatial positional relationship between the binocular cameras represents a rotation matrix and a translation vector between the cameras.

5. The measurement method according to claim 1, wherein the specific step of obtaining the SIFT prior feature library in step 2 comprises:

step 21: acquiring a binocular original image pair of the reference thin component, and respectively correcting the binocular original image pair by using a lens distortion coefficient and a stereo correction parameter of a binocular camera to obtain a binocular stereo correction image pair of the reference thin component;

step 22: SIFT features are extracted from the binocular stereo correction image pair of the reference thin component to form an SIFT prior feature library of the reference thin component.

6. The measurement method according to claim 1, wherein step 3 comprises the following steps:

step 31, acquiring a binocular original image pair of the thin part to be detected, and respectively correcting the binocular original image pair by using a lens distortion coefficient and a stereo correction parameter of a binocular camera to obtain a binocular stereo correction image pair of the thin part to be detected;

step 32: and extracting SIFT feature point pairs from the binocular stereo correction image pairs of the thin component to be detected, and matching the extracted SIFT feature point pairs with SIFT feature point pairs in the SIFT prior feature library to obtain SIFT feature point pairs with the same number and corresponding to the SIFT feature point pairs in the SIFT prior feature library.

7. The measurement method according to any one of claims 5 to 6, wherein the stereo correction parameters include a stereo correction internal matrix and a row alignment matrix of the binocular cameras obtained based on a rotation matrix and a translation vector between the binocular cameras.

8. The measurement method according to claim 1, wherein step 4 specifically comprises:

step 41: according to the image coordinates of the SIFT feature point pairs on the thin component to be measured, the three-dimensional world coordinates of the SIFT feature point pairs are obtained by utilizing the mapping relation between the three-dimensional world coordinate system and the two-dimensional image coordinates in the established binocular vision measuring system;

step 42: and obtaining a three-dimensional plane where the SIFT feature point pairs are located according to the three-dimensional world coordinates of the SIFT feature point pairs, and taking the three-dimensional plane as a space plane of the thin component to be measured.

9. The measurement method according to claim 8, wherein the image coordinates of the SIFT feature point pairs are binocular stereo-corrected image coordinates on the thin part to be measured, and the three-dimensional world coordinates of the SIFT feature points are obtained from their image coordinates on a binocular distortion-corrected image pair, wherein the image coordinates on the binocular distortion-corrected image pair are obtained as follows:

P_{und_l}＝M_1l(M_{rect_l} R_l)^-1P_{rect_l}

P_{und_r}＝M_1r(M_{rect_r} R_r)^-1P_{rect_r}

wherein, P_{und_l}＝[u_{und_l} v_{und_l} 1]^TAnd P_{und_r}＝[u_{und_r} v_{und_r} 1]^TCorrecting the image coordinates, u, in the image pair for binocular distortion, respectively_{und_l}、v_{und_l}、u_{und_r}、v_{und_r}Are respectively the coordinates of the pixel points, P_{rect_l}And P_{rect_r}Correcting image coordinates, M, in an image pair for binocular stereo_1lAnd M_1rRespectively, the internal parameter matrix, M, of the binocular camera_{rect_l}And M_{rect_r}Are the stereo correction parameter matrixes of the binocular camera respectively.

10. The measurement method according to claim 9, wherein the three-dimensional coordinates of the SIFT feature points are obtained as follows:

[X_w Y_w Z_w]^T＝((A^TA)^-1A^TB)

$A=\left[\begin{array}{ccc}{m}_{l11}-m_{l31}{u}_{\mathrm{und}\_l}& m_{l12}-m_{l32}{u}_{\mathrm{und}\_l}& m_{l13}-m_{l33}{u}_{\mathrm{und}\_l}\\ m_{l21}-m_{l31}{v}_{\mathrm{und}\_l}& m_{l22}-m_{l32}{v}_{\mathrm{und}\_l}& m_{l23}-m_{l33}{v}_{\mathrm{und}\_l}\\ m_{r11}-m_{r31}{u}_{\mathrm{und}\_r}& m_{r12}-m_{r32}{u}_{\mathrm{und}\_r}& m_{r13}-m_{r33}{u}_{\mathrm{und}\_r}\\ m_{r21}-m_{r31}{v}_{\mathrm{und}\_r}& m_{r22}-m_{r32}{v}_{\mathrm{und}\_r}& m_{r23}-m_{r33}{v}_{\mathrm{und}\_r}\end{array}\right]$

$B=\left[\begin{array}{c}{m}_{l34}{u}_{\mathrm{und}\_l}-m_{l14}\\ m_{l34}{v}_{\mathrm{und}\_l}-m_{l24}\\ m_{r34}{u}_{\mathrm{und}\_r}-m_{r14}\\ m_{r34}{v}_{\mathrm{und}\_r}-m_{r24}\end{array}\right]$

wherein (X)_w，Y_w，Z_w) The three-dimensional world coordinates of the SIFT feature points are obtained; m is_lAnd m_rInternal parameter matrix M of binocular camera respectively_1lAnd M_1rOf (2) is used.