阅读说明：本技术 一种基于点特征的单目视觉位姿测量方法 (Monocular vision pose measuring method based on point characteristics ) 是由 王中宇 李亚茹 程银宝 于 2019-10-29 设计创作，主要内容包括：发明涉及一种基于点特征的单目视觉位姿测量方法,属于单目视觉位姿测量领域,其特征在于：(1)根据共面4个特征点之间的位置关系,分别对平行和相交两种情况进行分析；(2)根据特征点的空间坐标、图像坐标和空间位置关系,推导出世界坐标系中3个坐标轴上的向量变换到摄像机坐标系中的单位向量,进而求解出物体相对于摄像机的初始位姿；(3)用LM算法对初始位姿进行优化,得到最终位姿。上述方法仅需给出任意4个特征点的空间相对位置,其中任意3个特征点不共线,不需要给定其他限制条件,就可求解出物体相对于摄像机的位姿。解决单目视觉位姿求解过程中存在的解不唯一、选解难和解的精度不高等问题。(The invention relates to a monocular vision pose measurement method based on point characteristics, which belongs to the field of monocular vision pose measurement and is characterized in that: (1) according to the position relation among the coplanar 4 characteristic points, analyzing the two conditions of parallel and intersection respectively; (2) deducing vectors on 3 coordinate axes in a world coordinate system to be converted into unit vectors in a camera coordinate system according to the spatial coordinates, the image coordinates and the spatial position relation of the feature points, and further solving the initial pose of the object relative to the camera; (3) and optimizing the initial pose by using an LM algorithm to obtain a final pose. The method can solve the pose of the object relative to the camera only by giving the spatial relative positions of any 4 characteristic points, wherein any 3 characteristic points are not collinear, and other limiting conditions are not required to be given. The method solves the problems that the solution is not unique, the solution is difficult to select, the precision is not high and the like in the monocular vision pose solving process.)

1. A monocular vision pose measurement method based on point features is characterized by comprising the following steps:

the method comprises the following steps: acquiring an image shot by a camera, and calibrating the camera by using a Zhangyingyou calibration method to acquire a parameter matrix in the camera;

step two: extracting any 4 non-coplanar feature points P in the image_i(i ═ 1,2,3,4), and the coordinates { q } of the feature point in the imaging plane coordinate system are calculated_i(x_i,y_iF), i is 1,2,3,4, and the distance between any two feature points is calculated according to the distance formula between two points: d₁₂＝|P₁P₂|、d₂₃＝|P₂P₃|、d₁₃＝|P₁P₃|、d₁₄＝|P₁P₄|；

Step three: let P₁Is the origin of the world coordinate system, O_cFor the centroid point, the relative positional relationship between given arbitrary 4 feature points that are not collinear is analyzed: when two straight lines where 4 feature points are located are parallel, a feature point correlation distance factor { k } is defined_iI ═ 1,2,3,4 }; when two straight lines where the 4 feature points are located are intersected, respectively calculating coordinates of the intersection point of the two straight lines in a world coordinate system and coordinates in an imaging coordinate system;

step four: respectively solve X in world coordinate system_w、Y_w、Z_wTransformation of on-axis vectors to unit vectors (r) in the camera coordinate system₁₁,r₂₁,r₃₁)；

Step five: and solving the initial position of the object relative to the camera according to the coordinates of the origin of the world coordinate system in the camera, and optimizing the initial pose by an LM algorithm to obtain the initial posture of the object relative to the camera.

2. The method for monocular vision pose measurement based on point features as claimed in claim 1, wherein given 4 coplanar noncollinear feature points P are analyzed_i(i is 1,2,3, 4): p₁、P₂Straight line P of two points₁P₂And P₃、P₄Straight line P of two points₃P₄Parallel and intersecting.

3. The method for measuring monocular vision pose based on point features as claimed in claim 1, wherein P is the median of the four steps₁、P₂Straight line P of two points₁P₂And P₃、P₄Straight line P of two points₃P₄When in parallel:

x in world coordinate system_wTransformation of on-axis vectors to unit vectors (r) in the camera coordinate system₁₁,r₂₁,r₃₁) Is composed of

Z in the world coordinate system_wTransformation of on-axis vectors to unit vectors (r) in the camera coordinate system₁₃,r₂₃,r₃₃) Is composed of

Y in the world coordinate system_wTransformation of on-axis vectors to unit vectors (r) in the camera coordinate system₁₂,r₂₂,r₃₂) Is (r)₁₃,r₂₃,r₃₃)×(r₁₁,r₂₁,r₃₁)。

4. The method for measuring monocular vision pose based on point features as claimed in claim 1, wherein P is the median of the four steps₁、P₂Straight line P of two points₁P₂And P₃、P₄Straight line P of two points₃P₄When intersecting:

x in world coordinate system_wTransformation of on-axis vectors to unit vectors (r) in the camera coordinate system₁₁,r₂₁,r₃₁) Is composed of

Z in the world coordinate system_wTransformation of on-axis vectors to unit vectors (r) in the camera coordinate system₁₃,r₂₃,r₃₃) Is composed of

Y in the world coordinate system_wTransformation of on-axis vectors to unit vectors (r) in the camera coordinate system₁₂,r₂₂,r₃₂) Is (r)₁₃,r₂₃,r₃₃)×(r₁₁,r₂₁,r₃₁)。

5. The method for measuring monocular vision pose based on point features as claimed in claim 1, wherein P is the number P in the fifth step₁、P₂Straight line P of two points₁P₂And P₃、P₄Straight line P of two points₃P₄Parallel time, according to the origin P of the world coordinate system₁And solving a rotation matrix R and a translational vector t from the world coordinate system to the camera coordinate system:

6. the method for measuring monocular vision pose based on point features as claimed in claim 1, wherein P is the number P in the fifth step₁、P₂Straight line P of two points₁P₂And P₃、P₄Straight line P of two points₃P₄At the time of intersection, according to world coordinatesOrigin P of the system₁And solving a rotation matrix R and a translational vector t from the world coordinate system to the camera coordinate system, wherein the Euler angle of the world coordinate system relative to the initial posture of the camera is as follows:

Technical Field

The invention belongs to the field of monocular vision pose measurement, relates to a monocular vision pose measurement method based on point characteristics, and solves the problems of non-unique solution, low resolution difficulty in solution selection, and the like in the monocular vision pose solving process.

Technical Field

The vision measurement technology is a technology which projects an object in a three-dimensional space into a two-dimensional image through a camera based on computer vision and then restores the two-dimensional image into three-dimensional information by means of a mathematical model, and can realize measurement of the geometric dimension of the object, the position, the posture and the like of the object in the space. The position and attitude are simply called pose and are usually represented by 6 degrees of freedom, i.e. 3 rotations and 3 translations between two coordinate systems. Pose measurement is currently widely used in the fields of aerospace, target tracking, augmented reality, automatic navigation, visual servo control, industrial manufacturing, and the like. When the pose is solved, firstly, characteristics such as point characteristics, straight line characteristics, quadratic curve characteristics and the like which are commonly used need to be selected. Algorithms based on linear and curvilinear features are generally evolved from point feature algorithms, so that the study of point features is of great significance.

The pose estimation problem based on the Point features is also called a PnP (coherent-n-Point) problem. The scholars at home and abroad carry out a great deal of research on the problem, mainly focus on the number n of points and the algorithm thereof, and the initial conclusion is that when n is less than 3, the PnP problem has infinite solutions; when n is 3, the PnP problem has 4 solutions at most, and if a triangle formed by 3 points is an isosceles triangle and the camera is in a specific area, the PnP problem has a unique solution; when n is 4, if the 4 points are coplanar, the PnP problem has a unique solution, and if the 4 points are not coplanar, the PnP problem has 4 solutions at most; when n is 5, the PnP problem has a maximum of 2 solutions; when n >5, the PnP problem has a unique solution, but the form of the solution is not closed, so the algorithm for solving has limitations. In addition, according to coplanar 4 feature points in quadrilateral distribution, methods for solving pose by using corresponding geometric relations are also applied, but the method has the defects of difficult feature point extraction, non-unique solution, poor robustness, non-convergence of iteration results and the like, and in an actual measurement scene, the feature points are often unknown, and at the moment, the feature point extraction precision has great influence on the precision of pose solution.

Aiming at the problems, the invention provides a monocular vision pose measuring method based on point characteristics, which solves the pose of an object relative to a camera by utilizing the known characteristic of the relative positions of the given 4 non-collinear characteristic points according to the spatial relative positions of the arbitrary non-collinear 4 characteristic points without the conditions of giving an included angle formed by a connecting line of the characteristic points and an optical center, giving the distance from the optical center to the characteristic points, limiting the distribution of the 4 characteristic points in a rectangular or special shape and the like.

Disclosure of Invention

The invention aims to: the method for measuring the monocular vision pose based on the point features is provided, and aims at the problems that solutions are not unique, the solutions are difficult to select, the accuracy is not high and the like in the monocular vision pose solving process.

The invention belongs to the field of monocular vision pose measurement, relates to a monocular vision pose measurement method based on point characteristics, and solves the problems of non-unique solution, low resolution difficulty in solution selection, and the like in the monocular vision pose solving process.

The technical solution of the invention is as follows: a monocular vision pose measurement method based on point characteristics is realized by the following steps:

the method comprises the following steps: acquiring an image shot by a camera, and calibrating the camera by using a Zhangyingyou calibration method to acquire a parameter matrix in the camera;

step two: extracting any 4 non-coplanar feature points P in the image_i(i ═ 1,2,3,4), and the coordinates { q } of the feature point in the imaging plane coordinate system are calculated_i(x_i,y_iF), i is 1,2,3,4, and the distance between any two feature points is calculated according to the distance formula between two points: d₁₂＝|P₁P₂|、d₂₃＝|P₂P₃|、d₁₃＝|P₁P₃|、d₁₄＝|P₁P₄|；

Step three: let P₁Is the origin of the world coordinate system, O_cFor the centroid point, the relative positional relationship between given arbitrary 4 feature points that are not collinear is analyzed: when two straight lines where 4 feature points are located are parallel, a feature point correlation distance factor { k } is defined_iI ═ 1,2,3,4 }; when two straight lines where the 4 feature points are located are intersected, respectively calculating coordinates of the intersection point of the two straight lines in a world coordinate system and coordinates in an imaging coordinate system;

step four: respectively solve X in world coordinate system_w、Y_w、Z_wTransformation of on-axis vectors to unit vectors (r) in the camera coordinate system₁₁,r₂₁,r₃₁)；

Step five: and solving the initial position of the object relative to the camera according to the coordinates of the origin of the world coordinate system in the camera, and optimizing the initial pose by an LM algorithm to obtain the initial posture of the object relative to the camera.

Analyzing the given relative position relation of 4 characteristic points in the third step and the fourth step to solve the X in the world coordinate system_w、Y_w、Z_wTransformation of on-axis vectors to unit vectors (r) in the camera coordinate system₁₁,r₂₁,r₃₁) The specific implementation process is as follows:

according to 4 coplanar characteristic points P with 3 arbitrary points not collinear_i(i is 1,2,3,4) for P₁、P₂Straight line P of two points₁P₂And P₃、P₄Straight line P of two points₃P₄The two conditions of parallel and intersection are respectively analyzed:

(1) when P is present₁P₂And P₃P₄When parallel to each other

Let P₁Is the origin of the world coordinate system, O_cIs a light center point, point O_c、q₃、q₄The determined plane is pi₁Point of, O_c、q₁、q₂The determined plane is pi₂Point of, O_cTo the target feature point P_i(i is 1,2,3,4) and the distance to the image point q_iThe distance ratios of (i ═ 1,2,3,4) are k_i＝|O_cP_i|/|O_cq_i|(i＝1,2,3,4)。

According to the vector

Calculating a parameter matrix in the camera to obtain a plane pi₁Normal vector of

Due to the straight line P₁P₂And a straight line P₃P₄In parallel, the equation can be derived:

from | P₁P₂|＝d₁₂The equation can be found:

simultaneous equations (1) and (2) are solved:

in the same way, according to | P₃P₄|＝d₃₄Obtaining by solution:

therefore, X in the world coordinate system_wTransformation of on-axis vectors to unit vectors (r) in the camera coordinate system₁₁,r₂₁,r₃₁) Is composed of

Z_wTransformation of on-axis vectors to unit vectors (r) in the camera coordinate system₁₃,r₂₃,r₃₃) Is composed ofY_wTransformation of on-axis vectors to unit vectors (r) in the camera coordinate system₁₂,r₂₂,r₃₂) Is (r)₁₃,r₂₃,r₃₃)×(r₁₁,r₂₁,r₃₁)。

So as to obtain a rotation matrix R from the world coordinate system to the camera coordinate system as follows:

from P₁The translation vector from the world coordinate system to the camera coordinate system is known as the origin of the world coordinate system

(2) When P is present₁P₂And P₃P₄At the time of intersection

Let P₃P₄And P₁P₂Intersect at a point P₀According to given 4 feature points P_i(i ═ 1,2,3,4) coordinates in the world coordinate system to give P₀Coordinates in the world coordinate system, point P₀Set as the origin of the world coordinate system, P₀The projected point in the image coordinate system is q₀(x₀,y₀F), respectively let | P₀P₁|＝d₀₁、|P₀P₂|＝d₀₂、|P₀P₃|＝d₀₃、|P₀P₄|＝d₀₄、|P₀P₅|＝d₀₅. According to the projection point q_i(x_i,y_iF) (i ═ 0,1,2,3,4) to give ∠ P₁O_cP₀、∠P₂O_cP₁、∠P₂O_cP₀、∠P₄O_cP₀、∠P₄O_cP₃And ∠ P₃O_cP₀。

At Δ O_cP₀P₂And Δ O_cP₀P₁In, from the sine theorem we can get:

solution point P₀Coordinates in the camera coordinate system

Comprises the following steps:

in a similar manner, at Δ O_cP₀P₂And Δ O_cP₁P₂In, the point P is solved by the sine theorem₂Coordinates in the camera coordinate systemComprises the following steps:

at Δ O_cP₀P₃And Δ O_cP₃P₄In, the point P is solved by the sine theorem₃Coordinates in the camera coordinate system

Comprises the following steps:

then there are:

the rotation matrix R and translation vector t from the world coordinate system to the camera coordinate system are solved according to equations (8) and (9). Then, when cos β ≠ 0, knowing the rotation matrix R, solving to obtain a corresponding euler angle as:

in summary, starting from 4 feature points with different distributions, a unique pose solution can be obtained by distance and angle constraints and corresponding geometric relationships.

Drawings

FIG. 1 is a flow chart of a monocular vision pose measurement method based on point features according to the present invention;

FIG. 2 is a diagram of a point feature-based monocular vision pose measurement method of the present invention₁P₂And P₃P₄A pose measurement schematic diagram in parallel;

FIG. 3 is a diagram of a point-feature-based monocular vision pose measurement method of the present invention₁P₂And P₃P₄A pose measurement schematic diagram at the time of intersection;

Detailed Description

The invention is further illustrated by the following figures and examples.

The invention provides a monocular vision pose measurement method based on point features, and provides a monocular vision pose measurement algorithm based on point features aiming at the problems that solutions are not unique, the solutions are difficult to select, the accuracy is not high and the like in the pose solving process.