A method and storage medium for estimating vehicle absolute posture based on visual line features

Abstract

The present invention discloses a method and storage medium for estimating the absolute posture of a vehicle based on visual line features. The method comprises the following steps: S1, processing an image of the outside of a vehicle acquired by a vehicle camera and mapping the image onto a Gaussian sphere; S2, setting the total number of iterations of a RANSAC algorithm; S3, selecting any three points (P1, P2, P3) on the Gaussian sphere to construct a set of Manhattans; S4, calculating the number of inner points of the set of Manhattans according to an error formula of points and vectors; S5, repeating steps S3 and S4 until the total number of iterations set in step S2 is reached, selecting a set of Manhattans with the largest number of inner points, and calculating the camera posture R according to all the inner points and the formula cw ; S6, calculate vehicle posture R vw , to estimate the absolute posture of the vehicle. The present invention can accurately estimate the position so that the position and posture of the vehicle in the indoor environment positioning process are more accurate.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle positioning, and in particular to a method and a storage medium for estimating the absolute posture of a vehicle based on visual line features. Background Art

[0002] The most basic function of an autonomous vehicle is positioning, and the vehicle's positioning system is mainly composed of sensors such as wheel odometers, Global Navigation Satellite System (GNSS), Inertial Measurement Unit (IMU), cameras, and lidar. In open areas (such as highways), GPS can provide effective global position and global attitude estimation, that is, the positioning result of the vehicle in the world coordinate system. However, in areas where GNSS signals are blocked, especially indoors or in parking lots, effective global position and global attitude estimation cannot be performed. At this time, simultaneous localization and mapping (SLAM) technology is usually required.

[0003] SLAM technology, in essence, calculates changes in its own position and posture through visual or laser photography of changes in the environment, and then calculates long-distance position and posture changes through continuous accumulation, obtains the posture change of the current moment relative to the initial moment, and then obtains global positioning information. However, since each estimation has errors, SLAM technology has cumulative errors, and the position and posture will diverge with distance. According to the theory of vehicle science, posture accuracy is more important than position accuracy, because posture accuracy can directly affect position, but not vice versa. Therefore, how to accurately estimate the position to make the position and posture of the vehicle more accurate during the positioning process in the indoor environment is a problem that technical personnel need to solve urgently. Summary of the invention

[0004] In view of the above-mentioned deficiencies of the prior art, the technical problem to be solved by the present invention is: how to provide a method and storage medium for estimating the absolute posture of a vehicle based on visual line features, and solve the problem of how to accurately estimate the position so that the posture of the vehicle in the indoor environment positioning process is more accurate.

[0005] In order to solve the above technical problems, the present invention adopts the following technical solutions:

[0006] A method for estimating the absolute posture of a vehicle based on visual line features comprises the following steps:

[0007] S1, processing the image of the vehicle exterior acquired by the vehicle camera and mapping it onto a Gaussian sphere;

[0008] S2, set the total number of iterations of the RANSAC algorithm;

[0009] S3, select any three points (P1, P2, P3) on the Gaussian sphere to construct a set of Manhattan;

[0010] S4, calculating the number of inner points of the Manhattan group according to the error formula of points and vectors;

[0011] S5. Repeat steps S3 and S4 until the total number of iterations set in step S2 is reached, select the set of Manhattan with the largest number of inliers, and calculate the camera pose R based on all inliers and the formula cw ;

[0012] S6. Calculate vehicle posture R vw , to estimate the absolute posture of the vehicle.

[0013] Furthermore, in step S1, after the image of the vehicle exterior acquired by the vehicle camera is processed, the step of mapping it onto the Gaussian sphere includes:

[0014] S11, grayscale processing the color image and extracting the image edge;

[0015] S12. Extract straight lines from the edge of the image and map all straight lines onto the Gaussian sphere to complete the mapping of straight lines in the image to points on the Gaussian sphere.

[0016] Furthermore, in step S3, the steps of constructing a set of Manhattan include:

[0017] S31. Generate a plane with point P1, point P2 and the optical center, and its normal vector v 1 The world axis 1 for Manhattan;

[0018] S32, point P3, v 1 and the optical center generate a plane whose normal vector v 2 Axis 2 for Manhattan;

[0019] S33, with v 1 、v 2 and the optical center generate a plane whose normal vector v 3 Axis 3 for Manhattan;

[0020] By v 1 、v 2 and v 3 Construct a set of Manhattan.

[0021] Furthermore, in step S4, the error formula of the point and vector error is:

[0022]

[0023] Select a threshold φ, when D(v i ,P k )<φ, then point P k is the interior point of the Manhattan group, T is the point P k lateral value to determine the number of interior points.

[0024] Further, in step S5, the camera posture R is calculated based on all the internal points and the formula cw , where the formula is:

[0025]

[0026] Where N is the maximum number of interior points, a1 =

[100] T , a2=

[010] T , a3=

[001] T , thus solving the camera posture R cw .

[0027] Furthermore, in step S6, the vehicle posture R is calculated vw for:

[0028] R vw =R vc ×R cw

[0029] In the formula, R cw is the camera pose, R vc is the vehicle external parameter, thereby estimating the absolute posture of the vehicle.

[0030] The present invention also provides a computer-readable storage medium on which a readable and writable program is stored, and when the program is executed, the steps of the above-mentioned method for estimating the absolute posture of a vehicle based on visual line features can be implemented.

[0031] Compared with the prior art, the present invention has the following beneficial effects:

[0032] 1. The present invention only extracts straight lines from the image and maps them to a Gaussian sphere. It then randomly selects three points from the Gaussian sphere to construct a set of Manhattans, calculates the number of inner points of the set of Manhattans, selects a set of Manhattans with the largest number of inner points, and calculates the camera pose R based on all the inner points and the formula cw , and finally according to the camera posture R cw and vehicle external parameter R vc Calculate vehicle posture R vw, in order to estimate the absolute posture of the vehicle and locate the current vehicle position, so as to accurately estimate the position to make the vehicle's position more accurate during the positioning process in the indoor environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] In order to make the purpose, technical solution and advantages of the invention more clear, the present invention will be further described in detail below with reference to the accompanying drawings, in which:

[0034] Figure 1 is a flow chart of the present invention;

[0035] Figure 2a is a schematic diagram of mapping points on the Gaussian sphere in the present invention;

[0036] Figure 2b A schematic diagram of extracting a straight line in an image in the present invention;

[0037] Figure 3a is the normal vector v in the present invention 1 This is a schematic diagram of Manhattan's World Axis 1;

[0038] Figure 3b is the normal vector v in the present invention 2 This is a schematic diagram of Manhattan's World Axis 2;

[0039] Figure 3c is the normal vector v in the present invention 3 This is a schematic diagram of Manhattan's World Axis 3;

[0040] Figure 3d For the present invention, 1 、v 2 and v 3 Construct a set of diagrams of Manhattan. DETAILED DESCRIPTION

[0041] Human-built environments are usually defined as Manhattan World, which usually has three perpendicular axes. For example, in a parking lot environment, the two sides and the wall of the parking space usually form three perpendicular axes along the direction of gravity, so the parking lot environment is usually Manhattan World. Similarly, office environments are usually Manhattan World. In Manhattan World, a set of perpendicular axes can be solved by extracting line features in the image, and then the posture transformation between the current camera and the environment can be calculated. Then, through the external parameter relationship on the vehicle, the relationship between the vehicle and the environment can be obtained, which is the global posture estimation.

[0042] The present invention estimates the Manhattan World by extracting line features from the image. The line features in the image can obtain multiple sets of parallel lines through projection geometry. The intersection of these lines is called the vanishing point. The vanishing point can provide certain attitude information, just like the stars can guide the course when navigating at sea. The Manhattan World essentially has three stable vanishing points. Therefore, the present invention provides a method for estimating the absolute attitude of the vehicle based on visual line features, and thereby calculates the camera attitude R when taking the picture. cw , combined with the vehicle external parameter R vc Calculate the vehicle posture R vw .

[0043] The present invention will be further described in detail below in conjunction with the accompanying drawings.

[0044] like Figure 1 As shown, this embodiment provides a method for estimating the absolute posture of a vehicle based on visual line features, comprising the following steps:

[0045] S1, processing the image of the vehicle exterior acquired by the vehicle camera and mapping it onto a Gaussian sphere;

[0046] S2, set the total number of iterations of the RANSAC algorithm;

[0047] S3, select any three points (P1, P2, P3) on the Gaussian sphere to construct a set of Manhattan;

[0048] S4, calculating the number of inner points of the Manhattan group according to the error formula of points and vectors;

[0049] S5. Repeat steps S3 and S4 until the total number of iterations set in step S2 is reached, select the set of Manhattan with the largest number of inliers, and calculate the camera pose R based on all inliers and the formula cw ;

[0050] S6. According to the camera posture R cw and vehicle external parameter R vc Calculate vehicle posture R vw ,

[0051] R vw =R vc ×R cw

[0052] Thus the absolute posture of the vehicle is estimated.

[0053] In the present invention, after processing the image of the vehicle exterior acquired by the vehicle camera, the line features in the image are extracted. Multiple sets of parallel lines can be obtained through projection geometry. Each straight line is projected onto the Gaussian sphere. The straight line in the image and the optical center (the center of the Gaussian sphere) form a plane. The normal vector of this plane intersects with the Gaussian sphere at one point, thus completing the mapping from the straight line in the image to the point on the Gaussian sphere. A set of Manhattans is constructed by randomly selecting 3 points from the Gaussian sphere, calculating the number of inner points of the set of Manhattans, selecting a set of Manhattans with the largest number of inner points, and calculating the camera posture R according to all the inner points and the formula cw , and finally according to the camera posture R cw and vehicle external parameter R vc Calculate vehicle posture R vw , in order to estimate the absolute posture of the vehicle and locate the current vehicle position, so as to accurately estimate the position and make the vehicle's position more accurate during the positioning process in the indoor environment.

[0054] like Figure 2a and Figure 2b As shown, in a specific implementation, in step S1, after the image of the vehicle exterior acquired by the vehicle camera is processed, the step of mapping it onto the Gaussian sphere includes:

[0055] S11, grayscale processing the color image and extracting the image edge;

[0056] S12. Extract straight lines from the edge of the image, and map all straight lines in the image to the Gaussian sphere, calculate the plane composed of the straight lines and the optical center of the image plane, find the direction of the plane normal vector, calculate the intersection of the plane normal vector and the Gaussian sphere, and complete the mapping of the straight lines in the image to the points on the Gaussian sphere. The Gaussian sphere is a unit sphere centered on the camera, and the Gaussian sphere can express all directions in a limited space. Therefore, the straight lines extracted from the image can be projected onto the Gaussian sphere.

[0057] In specific implementation, in step S3, the steps of constructing a set of Manhattan include:

[0058] S31, generate a plane with point P1, point P2 and the optical center (the center c of the Gaussian sphere), and the normal vector v of the plane 1 World axis 1 for Manhattan (see Figure 3a );

[0059] S32, point P3, v 1 and the optical center generate a plane whose normal vector v 2 For Manhattan's World Axis 2 (see Figure 3b );

[0060] S33, with v 1 、v2 and the optical center generate a plane whose normal vector v 3 For Manhattan's World Axis 3 (see Figure 3c );

[0061] By v 1 、v 2 and v 3 Construct a set of Manhattan (see Figure 3d ).

[0062] In specific implementation, in step S4, the error formula of the point and vector error is:

[0063]

[0064] Select a threshold φ, when D(v i ,P k )<φ, then point P k is the interior point of the Manhattan group, T is the point P k lateral value to determine the number of interior points.

[0065] In specific implementation, in step S5, the camera posture R is calculated based on all the internal points and the formula cw , where the formula is:

[0066]

[0067] Where N is the maximum number of interior points, a i is the axial unit vector (i.e., X-axis, Y-axis, and Z-axis), where a1 =

[100] T , a2=

[010] T , a3=

[001] T , thus solving the camera posture R cw .

[0068] The present invention also provides a computer-readable storage medium on which a readable and writable program is stored, and when the program is executed, the steps of the above-mentioned method for estimating the absolute posture of a vehicle based on visual line features can be implemented.

[0069] The workflow of the present invention is:

[0070] 1. Obtain the vehicle's external image through the vehicle camera, perform grayscale processing on the original color image, and extract the image edge.

[0071] 2. Extract straight lines from the edge of the image, map all straight lines in the image to the Gaussian sphere, calculate the plane composed of the straight lines and the optical center of the image plane, find the direction of the plane normal vector, calculate the intersection of the plane normal vector and the Gaussian sphere, and complete the mapping of the straight lines in the image to the points on the Gaussian sphere.

[0072] 3. Set the total number of iterations N of the RANSAC (Random Sample Consensus) algorithm.

[0073] 4. Select any three points (P1, P2, P3) on the Gaussian sphere to construct a set of Manhattans, and calculate the number of interior points of the set of Manhattans using formula (1).

[0074] 5. Repeat step 4 to solve the number of inliers of different groups of Manhattan in the total number of iterations N. Select the group of Manhattan with the largest number of inliers and calculate the camera pose R according to formula (2). cw .

[0075] 6. Finally, according to the camera posture R cw and vehicle external parameter R vc Calculate vehicle posture R vw , in order to estimate the absolute posture of the vehicle and locate the current vehicle position, so as to accurately estimate the position and make the vehicle's position more accurate during the positioning process in the indoor environment.

[0076] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit the present invention. Although the present invention has been described with reference to the preferred embodiments of the present invention, it should be understood by those skilled in the art that various changes may be made to the present invention in form and detail. Any obvious changes derived from the technical solutions of the present invention are still within the scope of protection of the present invention.

Claims

1. A method for estimating the absolute posture of a vehicle based on visual line features, characterized in that: The following steps are involved: S1, processing the image of the vehicle exterior acquired by the vehicle camera and mapping it onto a Gaussian sphere; S2, set the total number of iterations of the RANSAC algorithm; S3, select any three points P1, P2, P3 on the Gaussian sphere to construct a set of Manhattan; S4, calculating the number of inner points of the Manhattan group according to the error formula of points and vectors; S5. Repeat steps S3 and S4 until the total number of iterations set in step S2 is reached, select the set of Manhattan with the largest number of inliers, and calculate the camera pose R based on all inliers and the formula cw ; S6. Calculate vehicle posture R vw , to estimate the absolute posture of the vehicle; In step S1, after the image of the vehicle exterior acquired by the vehicle camera is processed, the step of mapping it onto a Gaussian sphere includes: S11, grayscale processing the color image and extracting the image edge; S12. Extract straight lines from the edge of the image, and map all straight lines in the image to the Gaussian sphere, calculate the plane composed of the straight lines and the optical center of the image plane, find the direction of the plane normal vector, calculate the intersection of the plane normal vector and the Gaussian sphere, and complete the mapping of the straight lines in the image to the points on the Gaussian sphere.

2. The method for estimating the absolute posture of a vehicle based on visual line features according to claim 1, characterized in that: In step S3, the steps of constructing a set of Manhattan include: S31. Generate a plane with point P1, point P2 and the optical center, and its normal vector v 1 The world axis 1 for Manhattan; S32, point P3, v 1 and the optical center generate a plane whose normal vector v 2 Axis 2 for Manhattan; S33, with v 1 、v 2 and the optical center generate a plane whose normal vector v 3 Axis 3 for Manhattan; By v 1 、v 2 and v 3 Construct a set of Manhattan.

3. The method for estimating the absolute posture of a vehicle based on visual line features according to claim 1, characterized in that: In step S4, the error formula of the point and vector is: Select a threshold φ, when D(vi,Pk)<φ, then point P k is the interior point of the Manhattan group, T is the point P k lateral value to determine the number of interior points.

4. The method for estimating the absolute posture of a vehicle based on visual line features according to claim 1, characterized in that: In step S5, the camera pose R is calculated based on all the internal points and the formula cw , where the formula is: Where N is the maximum number of interior points, a i is the axial unit vector, where a1 = [100] T , a2=[010] T , a3=[001] T , thus solving the camera posture R cw .

5. The method for estimating the absolute posture of a vehicle based on visual line features according to claim 1, characterized in that: In step S6, the vehicle posture R is calculated vw for: R vw =R vc ×R cw In the formula, R cw is the camera pose, R vc is the vehicle external parameter, thereby estimating the absolute posture of the vehicle.

6. A computer-readable storage medium having a readable and writable program stored thereon, characterized in that: When the program is executed, the steps of the method for estimating the absolute posture of a vehicle based on visual line features according to any one of claims 1 to 5 can be implemented.

Images