Visual inertial odometry optimization methods, devices, vehicles, and readable storage media

By dynamically optimizing the covariance of the visual inertial odometry (VIO) and utilizing the pixel coordinates and depth information of feature points, the problem of low optimization accuracy of VIO was solved, achieving higher positioning accuracy.

CN117419743BActive Publication Date: 2026-11-10GUANGDONG HUITIAN AEROSPACE TECH CO LTD
View PDF 2 Cites 0 Cited by

Patent Information

Application Number
CN202311342713.1
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2023-10-16
Publication Date
2026-11-10
Estimated Expiration
2043-10-16

AI Technical Summary

Technical Problem

In existing VIO optimization, the visual residual covariance is set to a fixed value, resulting in low optimization accuracy and positioning accuracy, and failing to adapt to the differences in feature point error distribution in the scene.

Method used

By dynamically optimizing the covariance of visual inertial odometry based on the pixel coordinates and depth information of feature points, VIO optimization is performed using the pixel deviation between different image frames.

Benefits of technology

It improves the optimization and positioning accuracy of VIO, adapts to the feature point error distribution in different scenarios, and enhances positioning accuracy.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN117419743B_ABST
    Figure CN117419743B_ABST
Patent Text Reader

Abstract

This application discloses a visual inertial odometry (VIO) optimization method, apparatus, vehicle, and readable storage medium. The method includes: determining first estimated coordinates of each feature point in the camera coordinate system of a second image frame based on the first pixel coordinates of each feature point in a first image frame; determining a first pixel deviation of each feature point based on the first estimated coordinates and the second pixel coordinates of each feature point in the second image frame; and optimizing the VIO based on the first pixel deviation of the target feature point if the image frame is the last keyframe corresponding to the target feature point. This application optimizes the VIO based on the pixel deviation between different image frames at the same time, thereby improving the optimization accuracy of the VIO and the positioning accuracy of the optimized VIO by optimizing the VIO according to the dynamic covariance corresponding to the actual scene.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] This application relates to the field of image processing technology, and in particular to a visual inertial odometry optimization method, apparatus, vehicle, and readable storage medium. Background Technology

[0002] Currently, most optimizations of VIO (Visual Inertial Odometry) set the covariance corresponding to the visual residual to a fixed value, meaning that each feature point has a fixed bias.

[0003] Because cameras are very sensitive to lighting, for example, some areas in a scene may be very bright, while others may be very dark, with some areas having very weak textures and others having very clear textures. This results in different error distributions for feature points in the scene. If a fixed value is used as the covariance corresponding to the visual residual during the VIO optimization process, the VIO optimization will not match the actual scene, causing the VIO optimization accuracy to fall short of expectations. This leads to low VIO optimization accuracy and consequently, low VIO positioning accuracy. Summary of the Invention

[0004] The main objective of this application is to provide a visual inertial odometry (VIO) optimization method, device, vehicle, and readable storage medium, aiming to solve the technical problem of low optimization accuracy in current VIO systems.

[0005] To achieve the above objectives, this application provides a visual inertial odometry optimization method, the method comprising:

[0006] Based on the first pixel coordinates of each feature point in the first image frame, the first estimated coordinates of each feature point in the camera coordinate system corresponding to the second image frame are determined. The image frame includes the first image frame of the left eye camera and the second image frame of the right eye camera in the binocular camera at the current time, or the image frame includes the first image frame of the right eye camera and the second image frame of the left eye camera at the current time, and each feature point belongs to the target feature point.

[0007] Based on the first estimated coordinates and the second pixel coordinates corresponding to each feature point in the second image frame, the first pixel deviation of each feature point is determined.

[0008] If the image frame is the last keyframe corresponding to the target feature point, then the visual inertial odometry (VIO) is optimized based on the first pixel deviation corresponding to the target feature point.

[0009] Further, the step of determining the first estimated coordinates of each feature point in the camera coordinate system corresponding to the second image frame based on the first pixel coordinates of each feature point in the first image frame includes:

[0010] Based on the first pixel coordinates, determine the depth corresponding to each feature point;

[0011] Based on the normalized coordinates corresponding to the first pixel coordinates and the depth, the first estimated coordinates of each feature point in the camera coordinate system corresponding to the second image frame are determined.

[0012] Furthermore, the step of determining the depth corresponding to each feature point based on the first pixel coordinates includes:

[0013] The disparity corresponding to each feature point is determined based on the first pixel coordinates;

[0014] The depth corresponding to each feature point is determined based on the parallax, the focal length of the binocular camera, and the baseline of the binocular camera.

[0015] Further, the step of determining the first estimated coordinates of each feature point in the camera coordinate system corresponding to the second image frame based on the normalized coordinates corresponding to the first pixel coordinates and the depth includes:

[0016] Based on the normalized coordinates corresponding to the first pixel coordinates and the depth, the first coordinates of each feature point in the camera coordinate system corresponding to the first image frame are determined.

[0017] The first estimated coordinates are determined based on the constraint information corresponding to the baseline of the binocular camera and the first coordinates.

[0018] Further, the step of determining the first pixel deviation of each feature point based on the first estimated coordinates and the second pixel coordinates corresponding to each feature point in the second image frame includes:

[0019] Based on the second pixel coordinates corresponding to each feature point in the second image frame, determine the second coordinates of each feature point in the camera coordinate system corresponding to the second image frame;

[0020] The first pixel deviation is determined based on the second coordinate and the first estimated coordinate.

[0021] Further, the step of determining the first pixel deviation based on the second coordinate and the first estimated coordinate includes:

[0022] Based on the second coordinates and the first estimated coordinates, determine the first L2 norm corresponding to each feature point;

[0023] Based on the camera focal length of the binocular camera and the first L2 norm, the first pixel deviation of each feature point is determined.

[0024] Furthermore, after the step of determining the first pixel deviation of each feature point based on the first estimated coordinates and the second pixel coordinates corresponding to each feature point in the second image frame, the method further includes:

[0025] If the image frame is not the last key frame corresponding to the target feature point, then based on the previous image frame corresponding to the first image frame, the second estimated coordinates corresponding to each feature point are determined, and based on the second estimated coordinates and the first pixel coordinates, the second pixel deviation of each feature point is determined.

[0026] If the image frame is the last keyframe corresponding to the target feature point, then the step of optimizing the visual inertial odometry (VIO) based on the first pixel deviation corresponding to the target feature point includes:

[0027] If the image frame is the last keyframe corresponding to the target feature point, then the VIO is optimized based on the first pixel deviation and the second pixel deviation corresponding to the target feature point.

[0028] Furthermore, the step of determining the second estimated coordinates corresponding to each feature point based on the previous image frame corresponding to the first image frame includes:

[0029] Based on the velocity and acceleration corresponding to the previous image frame, obtain the velocity of each feature point in the previous image frame;

[0030] Based on the feature point velocity and the third pixel coordinates corresponding to each feature point in the previous image frame, the second estimated coordinates corresponding to each feature point are determined.

[0031] Further, the step of determining the second estimated coordinates corresponding to each feature point based on the feature point velocity and the third pixel coordinates corresponding to each feature point in the previous image frame includes:

[0032] Obtain the normalized coordinates corresponding to the third pixel coordinates;

[0033] Based on the normalized coordinates corresponding to the third pixel coordinates and the velocity of the feature points, the second estimated coordinates corresponding to each feature point are determined.

[0034] Further, the step of determining the second pixel deviation of each feature point based on the second estimated coordinates and the first pixel coordinates includes:

[0035] Based on the normalized coordinates corresponding to the first pixel coordinates and the second estimated coordinates, the second L2 norm corresponding to each feature point is determined.

[0036] Based on the camera focal length of the binocular camera and the second norm, the second pixel deviation of each feature point is determined.

[0037] Furthermore, to achieve the above objectives, this application also provides a vehicle, the vehicle comprising:

[0038] The first determining module is used to determine the first estimated coordinates of each feature point in the camera coordinate system corresponding to the second image frame based on the first pixel coordinates of each feature point in the first image frame. The image frame includes the first image frame of the left eye camera and the second image frame of the right eye camera in the binocular camera at the current time, or the image frame includes the first image frame of the right eye camera and the second image frame of the left eye camera at the current time, and each feature point belongs to the target feature point.

[0039] The second determining module is used to determine the first pixel deviation of each feature point based on the first estimated coordinates and the second pixel coordinates corresponding to each feature point in the second image frame.

[0040] An optimization module is used to optimize the visual inertial odometry (VIO) based on the first pixel deviation corresponding to the target feature point if the image frame is the last key frame corresponding to the target feature point.

[0041] In addition, to achieve the above objectives, this application also provides a visual inertial odometry optimization device, which includes: a memory, a processor, and a visual inertial odometry optimization program stored in the memory and executable on the processor. When the visual inertial odometry optimization program is executed by the processor, it implements the steps of the aforementioned visual inertial odometry optimization method.

[0042] In addition, to achieve the above objectives, this application also provides a computer-readable storage medium storing a visual inertial odometry optimization program, which, when executed by a processor, implements the steps of the aforementioned visual inertial odometry optimization method.

[0043] This application determines the first estimated coordinates of each feature point in the camera coordinate system of the second image frame based on the first pixel coordinates of each feature point in the first image frame; then, based on the first estimated coordinates and the second pixel coordinates of each feature point in the second image frame, it determines the first pixel deviation of each feature point; then, if the image frame is the last keyframe corresponding to the target feature point, it optimizes the visual inertial odometry (VIO) based on the first pixel deviation of the target feature point. By optimizing the VIO according to the pixel deviation between different image frames at the same time, it achieves optimization of the VIO based on the dynamic covariance corresponding to the actual scene, thereby improving the optimization accuracy of the VIO and the positioning accuracy of the optimized VIO. Attached Figure Description

[0044] Figure 1This is a schematic diagram of the structure of the visual inertial odometry optimization device in the hardware operating environment involved in the embodiments of this application;

[0045] Figure 2 This is a flowchart illustrating the first embodiment of the visual inertial odometry optimization method of this application;

[0046] Figure 3 This is a schematic diagram of the functional modules of one embodiment of the vehicle of this application.

[0047] The realization of the purpose, functional features and advantages of this application will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0048] It should be understood that the specific embodiments described herein are merely illustrative of this application and are not intended to limit this application.

[0049] like Figure 1 As shown, Figure 1 This is a schematic diagram of the structure of the visual inertial odometry optimization device in the hardware operating environment involved in the embodiments of this application.

[0050] In this application embodiment, the terminal can be a vehicle. For example... Figure 1 As shown, the visual inertial odometry optimization device may include: a processor 1001, such as a CPU; a network interface 1004; a user interface 1003; a memory 1005; and a communication bus 1002. The communication bus 1002 is used to enable communication between these components. The user interface 1003 may include a display screen or an input unit such as a keyboard; optionally, the user interface 1003 may also include a standard wired interface or a wireless interface. The network interface 1004 may optionally include a standard wired interface or a wireless interface (such as a Wi-Fi interface). The memory 1005 may be high-speed RAM or non-volatile memory, such as a disk drive. Optionally, the memory 1005 may also be a storage device independent of the aforementioned processor 1001.

[0051] Optionally, the visual inertial odometry optimization device may also include a camera, RF (Radio Frequency) circuitry, sensors, audio circuitry, a WiFi module, and so on. These sensors may include, for example, light sensors, motion sensors, and other sensors. Of course, the visual inertial odometry optimization device may also be equipped with other sensors such as gyroscopes, barometers, hygrometers, thermometers, and infrared sensors, which will not be elaborated upon here.

[0052] Those skilled in the art will understand that Figure 1The terminal structure shown does not constitute a limitation on the visual inertial odometry optimization device, and may include more or fewer components than shown, or combine certain components, or have different component arrangements.

[0053] like Figure 1 As shown, the memory 1005, which serves as a computer storage medium, may include an operating system, a network communication module, a user interface module, and a visual inertial odometry optimization program.

[0054] exist Figure 1 In the visual inertial odometry optimization device shown, the network interface 1004 is mainly used to connect to the backend server and communicate data with the backend server; the user interface 1003 is mainly used to connect to the client (user end) and communicate data with the client; and the processor 1001 can be used to call the visual inertial odometry optimization program stored in the memory 1005.

[0055] In this embodiment, the visual inertial odometry optimization device includes: a memory 1005, a processor 1001, and a visual inertial odometry optimization program stored in the memory 1005 and executable on the processor 1001. When the processor 1001 calls the visual inertial odometry optimization program stored in the memory 1005, it executes the steps of the visual inertial odometry optimization method in the following embodiments.

[0056] This application also provides a method for optimizing visual inertial odometry, referring to... Figure 2 , Figure 2 This is a flowchart illustrating the first embodiment of the visual inertial odometry optimization method of this application.

[0057] The optimization method for visual inertial odometry includes:

[0058] Step S101: Based on the first pixel coordinates of each feature point in the first image frame, determine the first estimated coordinates of each feature point in the camera coordinate system corresponding to the second image frame. The image frame includes the first image frame of the left eye camera and the second image frame of the right eye camera in the binocular camera at the current time, or the image frame includes the first image frame of the right eye camera and the second image frame of the left eye camera at the current time. Each feature point belongs to the target feature point.

[0059] The visual inertial odometry (VIO) uses a binocular camera. The backend optimization of the VIOMA receives initial values ​​of state variables from the frontend module and iteratively updates these variables, which primarily include position, velocity, and rotation. To minimize time consumption during optimization, a fixed-size sliding window is maintained, which moves over time. This ensures minimal time consumption without affecting the accuracy of the output. Within this sliding window, there are prior constraints, IMU constraints, binocular vision constraints, and monocular vision constraints. The optimization process essentially involves minimizing these constraints.

[0060] Since the target feature points are determined during the front-end feature point detection and tracking process, and the pixel coordinates and feature points in each image frame can be obtained, the current image frame can be any image frame during the VIO optimization process, such as the image frame at the start of optimization, the image frame at a certain moment in the middle of the optimization process, or the last keyframe. The last keyframe refers to the last image frame in the current optimization process that contains one or more feature points from the target feature points. The image frames obtained by the binocular cameras after the last keyframe do not include the feature points from the target feature points. The image frame includes the first image frame of the left camera and the second image frame of the right camera in the binocular cameras at the current moment, or the image frame includes the first image frame of the right camera and the second image frame of the left camera at the current moment. That is, when the first image frame is the image frame of the left camera, the second image frame is the image frame of the right camera, and when the first image frame is the image frame of the right camera, the second image frame is the image frame of the left camera. Each feature point belongs to the target feature point, that is, each feature point in the first image frame is the target feature point or a part of the target feature points.

[0061] After obtaining the first image frame, the first pixel coordinates of each feature point in the first image frame are obtained, and the first estimated coordinates of each feature point in the camera coordinate system corresponding to the second image frame are calculated based on the first pixel coordinates. Specifically, the depth of each feature point can be determined first by the first pixel coordinates, and the first estimated coordinates of each feature point in the camera coordinate system corresponding to the second image frame can be determined based on the depth.

[0062] Step S102: Based on the first estimated coordinates and the second pixel coordinates corresponding to each feature point in the second image frame, determine the first pixel deviation of each feature point;

[0063] After obtaining the first estimated coordinates, the second pixel coordinates corresponding to each feature point in the second image frame are obtained, and the first pixel deviation of each feature point is calculated based on the first estimated coordinates and the second pixel coordinates. Specifically, the second coordinates of each feature point in the camera coordinate system corresponding to the second image frame are first calculated based on the second pixel coordinates, and the first pixel deviation of each feature point is determined based on the first estimated coordinates and the second coordinates.

[0064] Step S103: If the image frame is the last key frame corresponding to the target feature point, then optimize the visual inertial odometry (VIO) based on the first pixel deviation corresponding to the target feature point.

[0065] After obtaining the first pixel deviation, the VIO can be optimized directly based on the first pixel deviation. Specifically, first determine whether the image frame is the last keyframe corresponding to the target feature point. If the image frame is the last keyframe corresponding to the target feature point, then obtain the first pixel deviation corresponding to the target feature point. The first pixel deviation corresponding to the target feature point includes the first pixel deviation of each feature point currently calculated and the first pixel deviation of each target feature point obtained before the current time. Then, optimize the visual inertial odometry VIO based on the first pixel deviation corresponding to the target feature point, that is, use the first pixel deviation corresponding to the target feature point as the covariance of the visual constraints of different frames at different times to optimize the VIO.

[0066] It should be noted that this application can be applied to the optimization of various VIO systems, such as GNSS-VIO systems, ranging radar-VIO systems, etc.

[0067] Based on the first pixel coordinates of each feature point in the first image frame, the first estimated coordinates of each feature point in the camera coordinate system corresponding to the second image frame are determined. Then, based on the first estimated coordinates and the second pixel coordinates of each feature point in the second image frame, the first pixel deviation of each feature point is determined. If the image frame is the last key frame corresponding to the target feature point, the visual inertial odometry (VIO) is optimized based on the first pixel deviation of the target feature point. By optimizing the VIO according to the pixel deviation between different image frames at the same time, the optimization accuracy of the VIO and the positioning accuracy of the optimized VIO are improved by optimizing the VIO according to the dynamic covariance corresponding to the actual scene.

[0068] Based on the first embodiment, a second embodiment of the visual inertial odometry optimization method of this application is proposed, wherein step S101 includes:

[0069] Step S201: Determine the depth corresponding to each feature point based on the first pixel coordinates;

[0070] Step S202: Based on the normalized coordinates corresponding to the first pixel coordinates and the depth, determine the first estimated coordinates of each feature point in the camera coordinate system corresponding to the second image frame.

[0071] After obtaining the first image frame, the depth of each feature point is determined by the first pixel coordinates. Specifically, the depth of each feature point is calculated based on the disparity corresponding to the first pixel coordinates. Further, in one possible implementation, this step S201 includes:

[0072] Step S2011: Determine the disparity corresponding to each feature point based on the first pixel coordinates;

[0073] Step S2012: Determine the depth corresponding to each feature point based on the parallax, the camera focal length corresponding to the binocular camera, and the binocular camera baseline.

[0074] After obtaining the first image frame, the disparity corresponding to each feature point is determined based on the first pixel coordinates. Specifically, the disparity corresponding to each feature point can be calculated using the first pixel coordinates according to relevant algorithms.

[0075] After obtaining the disparity corresponding to the feature points, the camera focal length and baseline of the stereo camera are obtained. Based on the disparity, camera focal length, and stereo camera baseline, the depth corresponding to each feature point is determined. This improves the accuracy of the first pixel deviation by accurately obtaining the depth of each feature point through disparity, camera focal length, and stereo camera baseline. The formula for depth is:

[0076] z = fb / d;

[0077] Where z is depth, f is camera focal length, b is the binocular camera baseline, and d is parallax.

[0078] After obtaining the depth of each feature point, the coordinates of the first pixel are first normalized to obtain the normalized coordinates corresponding to the first pixel coordinates. Then, based on the normalized coordinates corresponding to the first pixel coordinates and the depth, the first estimated coordinates of each feature point in the camera coordinate system corresponding to the second image frame are determined. Further, in one possible implementation, this step S202 includes:

[0079] Step S2021: Based on the normalized coordinates corresponding to the first pixel coordinates and the depth, determine the first coordinates of each feature point in the camera coordinate system corresponding to the first image frame;

[0080] Step S2022: Determine the first estimated coordinates based on the constraint information corresponding to the baseline of the binocular camera and the first coordinates.

[0081] After obtaining the normalized coordinates corresponding to the first pixel coordinates, the first coordinates of each feature point in the camera coordinate system corresponding to the first image frame are determined based on the normalized coordinates corresponding to the first pixel coordinates and the depth. Specifically, relevant algorithms can be used to calculate the first coordinates of each feature point in the camera coordinate system corresponding to the first image frame using the normalized coordinates corresponding to the first pixel coordinates and the depth. When the first image frame is an image frame of the left eye camera, the camera coordinate system corresponding to the first image frame is the left eye camera coordinate system; when the first image frame is an image frame of the right eye camera, the camera coordinate system corresponding to the first image frame is the right eye camera coordinate system.

[0082] After obtaining the first coordinates corresponding to each feature point, the first estimated coordinates are determined based on the constraint information corresponding to the binocular camera baseline and the first coordinates. Specifically, the first estimated coordinates are calculated using relevant algorithms and the constraint information corresponding to the binocular camera baseline through the first coordinates, thereby accurately obtaining the first estimated coordinates and improving the accuracy of the first pixel deviation.

[0083] By determining the depth corresponding to each feature point based on the first pixel coordinates, and then determining the first estimated coordinates of each feature point in the camera coordinate system corresponding to the second image frame based on the normalized coordinates corresponding to the first pixel coordinates and the depth, the first estimated coordinates can be accurately obtained to improve the accuracy of the first pixel deviation, and further improve the optimization accuracy of VIO and the positioning accuracy of the optimized VIO.

[0084] Based on the first embodiment, a third embodiment of the visual inertial odometry optimization method of this application is proposed, wherein step S102 includes:

[0085] Step S301: Based on the second pixel coordinates corresponding to each feature point in the second image frame, determine the second coordinates of each feature point in the camera coordinate system corresponding to the second image frame;

[0086] Step S302: Determine the first pixel deviation based on the second coordinate and the first estimated coordinate.

[0087] After obtaining the first estimated coordinates, the second pixel coordinates corresponding to each feature point in the second image frame are obtained, and the second coordinates of each feature point in the camera coordinate system corresponding to the second image frame are determined based on the second pixel coordinates corresponding to each feature point. The method of obtaining the first coordinates is similar to that of obtaining the first coordinates, and will not be described again here.

[0088] After obtaining the second coordinates, the first pixel deviation is determined based on the second coordinates and the first estimated coordinates. Further, in one possible implementation, step S302 includes:

[0089] Step S3021: Based on the second coordinates and the first estimated coordinates, determine the first L2 norm corresponding to each feature point;

[0090] Step S3022: Based on the camera focal length of the binocular camera and the first L2 norm, determine the first pixel deviation of each feature point.

[0091] After obtaining the second coordinates, the first L2 norm corresponding to each feature point is calculated based on the second coordinates and the first estimated coordinates. Specifically, relevant algorithms can be used to calculate the first L2 norm corresponding to each feature point using the second coordinates and the first estimated coordinates.

[0092] After obtaining the first L2 norm, the first pixel deviation of each feature point is determined based on the camera focal length of the stereo camera and the first L2 norm. Specifically, for each feature point, its first pixel deviation = camera focal length * corresponding first L2 norm, thus accurately obtaining the first pixel deviation.

[0093] By determining the second coordinates of each feature point in the second image frame in the camera coordinate system corresponding to the second image frame based on the second pixel coordinates of each feature point, the first pixel deviation is determined based on the second coordinates and the first estimated coordinates. The first pixel deviation can be accurately obtained through the second pixel coordinates and the first estimated coordinates, thereby improving the accuracy of the first pixel deviation and further improving the optimization accuracy of VIO and the positioning accuracy of the optimized VIO.

[0094] Based on the above embodiments, a fourth embodiment of the visual inertial odometry optimization method of this application is proposed, wherein, after step S102, the visual inertial odometry optimization method further includes:

[0095] Step S401: If the image frame is not the last key frame corresponding to the target feature point, then based on the previous image frame corresponding to the first image frame, determine the second estimated coordinates corresponding to each feature point, and based on the second estimated coordinates and the first pixel coordinates, determine the second pixel deviation of each feature point.

[0096] Step S103 includes:

[0097] Step S402: If the image frame is the last key frame corresponding to the target feature point, then optimize the VIO based on the first pixel deviation and the second pixel deviation corresponding to the target feature point.

[0098] If the image frame is not the last keyframe corresponding to the target feature point, then obtain the previous image frame corresponding to the first image frame, obtain the third pixel coordinates of each feature point in the previous image frame, and obtain the second estimated coordinates of each feature point based on the velocity and acceleration of the IMU (inertial measurement unit) at the time corresponding to the previous image frame and the third pixel coordinates.

[0099] After obtaining the second estimated coordinates, based on the second estimated coordinates and the first pixel coordinates, the second pixel deviation of each feature point is determined. Further, in one possible implementation, step S401 includes:

[0100] Step S4011: Based on the normalized coordinates corresponding to the first pixel coordinates and the second estimated coordinates, determine the second L2 norm corresponding to each feature point;

[0101] Step S4012: Based on the camera focal length of the binocular camera and the second L2 norm, determine the second pixel deviation of each feature point.

[0102] After obtaining the second estimated coordinates, the normalized coordinates corresponding to the first pixel coordinates are obtained. For the first pixel coordinates (x, y, z), each is divided by z to obtain (x / z, y / z, 1). (x / z, y / z) are the normalized coordinates corresponding to the first pixel coordinates. Based on the normalized coordinates corresponding to the first pixel coordinates and the second estimated coordinates, the second L2 norm corresponding to each feature point is calculated. Specifically, relevant algorithms can be used to calculate the second L2 norm corresponding to each feature point using the normalized coordinates corresponding to the first pixel coordinates and the second estimated coordinates.

[0103] After obtaining the second L2 norm, the second pixel deviation of each feature point is determined based on the camera focal length of the stereo camera and the second L2 norm. Specifically, for each feature point, its second pixel deviation = camera focal length * corresponding second L2 norm, thus accurately obtaining the second pixel deviation.

[0104] After obtaining the second pixel deviation, the process returns to step S101 until the image frame is the last keyframe corresponding to the target feature point. If the image frame is the last keyframe corresponding to the target feature point, the VIO is optimized based on the first pixel deviation and the second pixel deviation corresponding to the target feature point. The first pixel deviation corresponding to the target feature point includes the first pixel deviation and the second pixel deviation of each feature point currently calculated, as well as the first pixel deviation and the second pixel deviation of each target feature point obtained before the current time. Then, the visual inertial odometry (VIO) is optimized based on the first pixel deviation and the second pixel deviation corresponding to the target feature point. Specifically, the first pixel deviation corresponding to the target feature point is used as the covariance of the visual constraints of different frames at different times to optimize the VIO, and the covariance of the visual constraints formed by the frames before and after different times corresponding to the second pixel deviation of the target feature point is used to optimize the VIO.

[0105] If the image frame is not the last keyframe corresponding to the target feature point, then based on the previous image frame corresponding to the first image frame, the second estimated coordinates corresponding to each feature point are determined, and based on the second estimated coordinates and the first pixel coordinates, the second pixel deviation of each feature point is determined; if the image frame is the last keyframe corresponding to the target feature point, then the VIO is optimized based on the first pixel deviation and the second pixel deviation corresponding to the target feature point. By optimizing the VIO according to the pixel deviation between different image frames at the same time and the pixel deviation between frames before and after at different times, the optimization accuracy of the VIO and the positioning accuracy of the optimized VIO are improved by optimizing the VIO according to the dynamic covariance corresponding to the actual scene.

[0106] Based on the fourth embodiment, a fifth embodiment of the visual inertial odometry optimization method of this application is proposed, wherein step S401 includes:

[0107] Step S501: Based on the velocity and acceleration corresponding to the previous image frame, obtain the velocity of each feature point in the previous image frame.

[0108] Step S502: Based on the feature point velocity and the third pixel coordinates corresponding to each feature point in the previous image frame, determine the second estimated coordinates corresponding to each feature point.

[0109] If the image frame is not the last keyframe corresponding to the target feature point, then the previous image frame corresponding to the first image frame is obtained, the third pixel coordinates of each feature point in the previous image frame are obtained, and the velocity and acceleration of the IMU (inertial measurement unit) at the corresponding time of the previous image frame are obtained. The velocity of the camera is calculated based on the velocity and acceleration corresponding to the previous image frame according to the relevant algorithm. The velocity of the feature point corresponding to each feature point in the previous image frame is calculated based on the velocity of the camera using the relevant algorithm. The feature point velocity and the camera velocity are in opposite directions, that is, the feature point velocity is - the camera velocity.

[0110] After obtaining the feature point velocities corresponding to each feature point in the previous image frame, based on the feature point velocities and the third pixel coordinates corresponding to each feature point in the previous image frame, the second estimated coordinates corresponding to each feature point are determined. Further, in one possible implementation, this step S502 includes:

[0111] Step S5021: Obtain the normalized coordinates corresponding to the coordinates of the third pixel;

[0112] Step S5022: Based on the normalized coordinates corresponding to the third pixel coordinates and the velocity of the feature points, determine the second estimated coordinates corresponding to each feature point.

[0113] After obtaining the feature point velocity, the normalized coordinates corresponding to the third pixel coordinates are obtained. For the third pixel coordinate of (x, y, z), divide by z to obtain (x / z, y / z, 1), and (x / z, y / z) is the normalized coordinate corresponding to the third pixel coordinate.

[0114] After obtaining the normalized coordinates corresponding to the third pixel coordinates, since the feature point velocity is a 3D velocity, it is first converted into a 2D velocity in the normalized plane. Based on the converted feature point velocity and the normalized coordinates corresponding to the third pixel coordinates, the second estimated coordinates corresponding to each feature point are determined. Specifically, if the binocular camera is moving at a constant speed, the second estimated coordinates corresponding to each feature point can be calculated using the converted feature point velocity and the normalized coordinates corresponding to the third pixel coordinates according to the constant speed motion model. In other embodiments, if the binocular camera is moving at a constant speed or a variable speed, the second estimated coordinates corresponding to each feature point can be calculated using the converted feature point velocity and the normalized coordinates corresponding to the third pixel coordinates according to the constant speed motion model or the variable speed motion model.

[0115] By obtaining the velocity and acceleration corresponding to the previous image frame, the velocity of each feature point in the previous image frame is obtained. Then, based on the velocity of the feature points and the third pixel coordinates of each feature point in the previous image frame, the second estimated coordinates of each feature point are determined. The second estimated coordinates can be accurately obtained through the velocity of the feature points to improve the accuracy of the second pixel deviation. This facilitates the optimization of VIO based on the pixel deviation between different image frames at the same time and the pixel deviation between frames before and after at different times. This enables the optimization of VIO based on the dynamic covariance corresponding to the actual scene, thereby improving the optimization accuracy of VIO and the positioning accuracy of the optimized VIO.

[0116] In addition, this application also proposes a vehicle, referring to Figure 3 The vehicle includes:

[0117] The first determining module 10 is used to determine the first estimated coordinates of each feature point in the camera coordinate system corresponding to the second image frame based on the first pixel coordinates of each feature point in the first image frame. The image frame includes the first image frame of the left eye camera and the second image frame of the right eye camera in the binocular camera at the current time, or the image frame includes the first image frame of the right eye camera and the second image frame of the left eye camera at the current time, and each feature point belongs to the target feature point.

[0118] The second determining module 20 is used to determine the first pixel deviation of each feature point based on the first estimated coordinates and the second pixel coordinates corresponding to each feature point in the second image frame.

[0119] The optimization module 30 is used to optimize the visual inertial odometry (VIO) based on the first pixel deviation corresponding to the target feature point if the image frame is the last key frame corresponding to the target feature point.

[0120] The methods executed by the above-mentioned program units can be referred to in the various embodiments of the visual inertial odometry optimization method of this application, and will not be repeated here.

[0121] Furthermore, this application also proposes a computer-readable storage medium storing a visual inertial odometry optimization program, which, when executed by a processor, implements the steps of the visual inertial odometry optimization method as described above.

[0122] Furthermore, embodiments of this application also propose a computer-readable storage medium storing a visual inertial odometry optimization program, which, when executed by a processor, implements the steps of the aforementioned in-vehicle brightness detection method.

[0123] It should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or system that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or system. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or system that includes that element.

[0124] The sequence numbers of the embodiments in this application are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0125] Through the above description of the embodiments, those skilled in the art can clearly understand that the methods of the above embodiments can be implemented by means of software plus necessary general-purpose hardware platforms. Of course, they can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk) as described above, and includes several instructions to cause a terminal device (which may be a mobile phone, computer, server, air conditioner, or network device, etc.) to execute the methods described in the various embodiments of this application.

[0126] The above are merely preferred embodiments of this application and do not limit the patent scope of this application. Any equivalent structural or procedural transformations made using the content of this application's specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of this application.

Claims

1. A method for optimizing visual inertial odometry, characterized in that, The method includes: Based on the first pixel coordinates of each feature point in the first image frame, the first estimated coordinates of each feature point in the camera coordinate system corresponding to the second image frame are determined. The image frame includes the first image frame of the left eye camera and the second image frame of the right eye camera in the binocular camera at the current time, or the image frame includes the first image frame of the right eye camera and the second image frame of the left eye camera at the current time, and each feature point belongs to the target feature point. Based on the first estimated coordinates and the second pixel coordinates corresponding to each feature point in the second image frame, the first pixel deviation of each feature point is determined. If the image frame is not the last key frame corresponding to the target feature point, then based on the previous image frame corresponding to the first image frame, the second estimated coordinates corresponding to each feature point are determined, and based on the second estimated coordinates and the first pixel coordinates, the second pixel deviation of each feature point is determined. If the image frame is the last keyframe corresponding to the target feature point, then the visual inertial odometry (VIO) is optimized based on the first pixel deviation and the second pixel deviation corresponding to the target feature point.

2. The visual inertial odometry optimization method as described in claim 1, characterized in that, The step of determining the first estimated coordinates of each feature point in the camera coordinate system of the second image frame based on the first pixel coordinates of each feature point in the first image frame includes: Based on the first pixel coordinates, determine the depth corresponding to each feature point; Based on the normalized coordinates corresponding to the first pixel coordinates and the depth, the first estimated coordinates of each feature point in the camera coordinate system corresponding to the second image frame are determined.

3. The visual inertial odometry optimization method as described in claim 2, characterized in that, The step of determining the depth corresponding to each feature point based on the first pixel coordinates includes: The disparity corresponding to each feature point is determined based on the first pixel coordinates; The depth corresponding to each feature point is determined based on the parallax, the focal length of the binocular camera, and the baseline of the binocular camera.

4. The visual inertial odometry optimization method as described in claim 2, characterized in that, The step of determining the first estimated coordinates of each feature point in the camera coordinate system corresponding to the second image frame based on the normalized coordinates corresponding to the first pixel coordinates and the depth includes: Based on the normalized coordinates corresponding to the first pixel coordinates and the depth, the first coordinates of each feature point in the camera coordinate system corresponding to the first image frame are determined. The first estimated coordinates are determined based on the constraint information corresponding to the baseline of the binocular camera and the first coordinates.

5. The visual inertial odometry optimization method as described in claim 1, characterized in that, The step of determining the first pixel deviation of each feature point based on the first estimated coordinates and the second pixel coordinates corresponding to each feature point in the second image frame includes: Based on the second pixel coordinates corresponding to each feature point in the second image frame, determine the second coordinates of each feature point in the camera coordinate system corresponding to the second image frame; The first pixel deviation is determined based on the second coordinate and the first estimated coordinate.

6. The visual inertial odometry optimization method as described in claim 5, characterized in that, The step of determining the first pixel deviation based on the second coordinate and the first estimated coordinate includes: Based on the second coordinates and the first estimated coordinates, determine the first L2 norm corresponding to each feature point; Based on the camera focal length of the binocular camera and the first L2 norm, the first pixel deviation of each feature point is determined.

7. The visual inertial odometry optimization method as described in any one of claims 1 to 6, characterized in that, The step of determining the second estimated coordinates of each feature point based on the previous image frame corresponding to the first image frame includes: Based on the velocity and acceleration corresponding to the previous image frame, obtain the velocity of each feature point in the previous image frame; Based on the feature point velocity and the third pixel coordinates corresponding to each feature point in the previous image frame, the second estimated coordinates corresponding to each feature point are determined.

8. The visual inertial odometry optimization method as described in claim 7, characterized in that, The step of determining the second estimated coordinates of each feature point based on the feature point velocity and the third pixel coordinates of each feature point in the previous image frame includes: Obtain the normalized coordinates corresponding to the third pixel coordinates; Based on the normalized coordinates corresponding to the third pixel coordinates and the velocity of the feature points, the second estimated coordinates corresponding to each feature point are determined.

9. The visual inertial odometry optimization method as described in any one of claims 1 to 6, characterized in that, The step of determining the second pixel deviation of each feature point based on the second estimated coordinates and the first pixel coordinates includes: Based on the normalized coordinates corresponding to the first pixel coordinates and the second estimated coordinates, the second L2 norm corresponding to each feature point is determined. Based on the camera focal length of the binocular camera and the second norm, the second pixel deviation of each feature point is determined.

10. A vehicle, characterized in that, The vehicles include: The first determining module is used to determine the first estimated coordinates of each feature point in the camera coordinate system corresponding to the second image frame based on the first pixel coordinates of each feature point in the first image frame. The image frame includes the first image frame of the left eye camera and the second image frame of the right eye camera in the binocular camera at the current time, or the image frame includes the first image frame of the right eye camera and the second image frame of the left eye camera at the current time, and each feature point belongs to the target feature point. The second determining module is used to determine the first pixel deviation of each feature point based on the first estimated coordinates and the second pixel coordinates corresponding to each feature point in the second image frame. The optimization module is used to determine the second estimated coordinates of each feature point based on the previous image frame corresponding to the first image frame if the image frame is not the last key frame corresponding to the target feature point, and to determine the second pixel deviation of each feature point based on the second estimated coordinates and the first pixel coordinates; if the image frame is the last key frame corresponding to the target feature point, the visual inertial odometry (VIO) is optimized based on the first pixel deviation and the second pixel deviation corresponding to the target feature point.

11. A visual inertial odometry optimization device, characterized in that, The visual inertial odometry optimization device includes: a memory, a processor, and a visual inertial odometry optimization program stored in the memory and executable on the processor, wherein the visual inertial odometry optimization program, when executed by the processor, implements the steps of the visual inertial odometry optimization method as described in any one of claims 1 to 9.

12. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a visual inertial odometry optimization program, which, when executed by a processor, implements the steps of the visual inertial odometry optimization method as described in any one of claims 1 to 9.

Citation Information

Patent Citations

  • Intelligent automobile high-precision positioning and space-time situation safety method based on multi-sensor fusion

    CN112484725A

  • Visual odometry and implementation method therefor

    WO2019084804A1