WO2023051019A1 - 视觉惯性里程计方法、装置、电子设备、存储介质及计算机程序 - Google Patents

视觉惯性里程计方法、装置、电子设备、存储介质及计算机程序 Download PDF

Info

Publication number
WO2023051019A1
WO2023051019A1 PCT/CN2022/109502 CN2022109502W WO2023051019A1 WO 2023051019 A1 WO2023051019 A1 WO 2023051019A1 CN 2022109502 W CN2022109502 W CN 2022109502W WO 2023051019 A1 WO2023051019 A1 WO 2023051019A1
Authority
WO
WIPO (PCT)
Prior art keywords
photometric
points
feature point
pose
optimization
Prior art date
Application number
PCT/CN2022/109502
Other languages
English (en)
French (fr)
Inventor
郑泽玲
高军强
Original Assignee
达闼科技（北京）有限公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 达闼科技（北京）有限公司 filed Critical 达闼科技（北京）有限公司
Publication of WO2023051019A1 publication Critical patent/WO2023051019A1/zh

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/77—Processing image or video features in feature spaces; using data integration or data reduction, e.g. principal component analysis [PCA] or independent component analysis [ICA] or self-organising maps [SOM]; Blind source separation
    • G06V10/80—Fusion, i.e. combining data from various sources at the sensor level, preprocessing level, feature extraction level or classification level
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10052—Images from lightfield camera
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Definitions

  • the present disclosure relates to the field of SLAM (simultaneous localization and mapping, Chinese name: real-time positioning and map construction, or concurrent map building and positioning), in particular to a visual-inertial odometer (VIO, Visual-Inertial Odometry) method, device, electronic equipment, storage media and computer programs.
  • SLAM simultaneous localization and mapping
  • VIO Visual-Inertial Odometry
  • the robot starts to move from an unknown position in an unknown environment, and performs its own positioning based on position estimation and maps during the movement process, and builds increments based on its own positioning. map to realize autonomous positioning and navigation of the robot.
  • Visual SLAM can be divided into two categories according to the principle: feature point method SLAM and photometric method SLAM.
  • the photometric method establishes an objective function based on the photometric error of points with large pixel gradient changes in the image, and optimizes the pose of the camera and the position of the map point.
  • the feature point method first needs to extract the feature points and descriptors in the image, and then determine the matching relationship between the feature points according to the descriptor, and establish the objective function according to the projection error between the matching points.
  • the photometric method does not have high requirements on the texture of the scene but is more sensitive to illumination changes, while the feature point method is not sensitive to illumination changes but has higher texture requirements. Therefore, how to construct a SLAM algorithm that meets the needs of different scenarios is an urgent problem in this field.
  • the purpose of the embodiments of the present disclosure is to provide a visual-inertial odometer method, device, electronic equipment, storage medium, storage medium and computer program.
  • an embodiment of the present disclosure provides a method for visual-inertial odometry, including:
  • Concatenation of constraint matrices is performed on the feature point matrix and the photometric constraint matrix to obtain a final constraint matrix.
  • said fusing pose information of at least one sensor to obtain a first initial pose includes:
  • IMU Inertial Measurement Unit
  • performing photometric optimization on the initial pose according to the pose transformation between two frames of images to obtain a second initial pose including:
  • r represents the photometric error
  • ⁇ h is the Huber weight
  • I 1 and I 2 represent two adjacent grayscale images
  • x 1 and x 2 are the pixel coordinates of the point X in the image in the space
  • x 2 is determined by x 1 projection
  • a, b are photometric affine transformation parameters.
  • x2 is obtained by projection of x1 , requiring the relative pose transformation ⁇ 21 between two frames and the inverse depth of x1 in I1 .
  • nonlinear least squares function is expressed as follows:
  • J represents Jacobian matrix
  • ⁇ represents Lie algebra
  • r represents photometric error
  • i and N are natural numbers.
  • map points including:
  • Map points are generated according to the second initial pose after feature point optimization and the corner points.
  • reprojection error function is expressed as follows:
  • ⁇ * is the reprojection error
  • u i is the known 2D homogeneous coordinates
  • s i is the scale
  • K is the internal parameters of the camera
  • ⁇ is the unknown camera pose
  • P i is the known 3D point Homogeneous coordinates
  • i and N are natural numbers.
  • the creation of the back-end tight coupling optimization according to the correspondence between the map points and the corner points includes:
  • the back-end tightly coupled optimization includes feature point constraints and photometric constraints, and the feature point constraints and the photometric constraints are tightly coupled;
  • map points are not constituted by the corner points, photometric constraints are included, and the update amount of the variable to be optimized is calculated by non-linear least squares.
  • feature point constraint matrix and photometric constraint matrix through the back-end tight coupling optimization includes:
  • the derivative between the variable to be changed in the feature point constraint and the photometric constraint is respectively obtained
  • a feature point constraint matrix and a photometric constraint matrix are respectively constructed according to the derivatives.
  • a final constraint matrix including:
  • the method also includes:
  • the back-end tightly coupled optimization includes feature point constraints and photometric constraints, and if the map points are not formed by the corner points, photometric constraints are included;
  • Incremental addition is directly performed on the variables to be optimized, and the rotation information in the second pose is multiplied.
  • variables to be optimized include internal camera parameters, pose, inverse depth of feature points, and photometric affine transformation parameters.
  • a visual-inertial odometer device including:
  • a fusion module configured to fuse pose information of at least one sensor to obtain a first initial pose
  • a photometric optimization module configured to perform photometric optimization on the initial pose according to the pose transformation between two frames of images, to obtain a second initial pose
  • a feature point optimization module configured to perform feature point optimization on the second initial pose by extracting and matching feature points in the image to generate map points
  • a tightly coupled optimization module configured to create a back-end tightly coupled optimization according to the correspondence between the map points and the corner points;
  • a matrix construction module configured to construct a feature point constraint matrix and a photometric constraint matrix through the back-end tight coupling optimization
  • the matrix splicing module is used to splice the constraint matrix on the feature point matrix and the photometric constraint matrix to obtain a final constraint matrix.
  • the fusion module is used to include:
  • the photometric optimization module is used to include:
  • r represents the photometric error
  • ⁇ h is the Huber weight
  • I 1 and I 2 represent two adjacent grayscale images
  • x 1 and x 2 are the pixel coordinates of the point X in the image in the space
  • x 2 is determined by x 1 projection
  • a, b are photometric affine transformation parameters.
  • x2 is obtained by projection of x1 , requiring the relative pose transformation ⁇ 21 between two frames and the inverse depth of x1 in I1 .
  • nonlinear least squares function is expressed as follows:
  • J represents Jacobian matrix
  • ⁇ represents Lie algebra
  • r represents photometric error
  • i and N are natural numbers.
  • the feature point optimization module is used to include:
  • Map points are generated according to the second initial pose after feature point optimization and the corner points.
  • reprojection error function is expressed as follows:
  • ⁇ * is the reprojection error
  • u i is the known 2D homogeneous coordinates
  • s i is the scale
  • K is the internal parameters of the camera
  • ⁇ is the unknown camera pose
  • P i is the known 3D point Homogeneous coordinates
  • i and N are natural numbers.
  • the tightly coupled optimization module is used to include:
  • the back-end tightly coupled optimization includes feature point constraints and photometric constraints, and the feature point constraints and the photometric constraints are tightly coupled;
  • map points are not constituted by the corner points, photometric constraints are included, and the update amount of the variable to be optimized is calculated by non-linear least squares.
  • matrix construction module is used to include:
  • the derivative between the variable to be changed in the feature point constraint and the photometric constraint is respectively obtained
  • a feature point constraint matrix and a photometric constraint matrix are respectively constructed according to the derivatives.
  • the matrix splicing module is used to include:
  • the device also includes:
  • An update module configured to update the variables to be optimized in the feature point constraint matrix and the photometric constraint matrix
  • the update module is used to include:
  • the back-end tightly coupled optimization includes feature point constraints and photometric constraints, and if the map points are not formed by the corner points, photometric constraints are included;
  • Incremental addition is directly performed on the variables to be optimized, and the rotation information in the second pose is multiplied.
  • variables to be optimized include internal camera parameters, pose, inverse depth of feature points, and photometric affine transformation parameters.
  • an electronic device including:
  • a processor configured to run the computer-readable instructions, so that the electronic device implements the method described in any one of the above first aspects.
  • another embodiment of the present disclosure provides a non-transitory computer-readable storage medium for storing computer-readable instructions.
  • the computer-readable instructions When executed by a computer, the computer implements the above-mentioned first The method described in any one of the aspects.
  • another embodiment of the present disclosure provides a computer program product, including instructions, which, when run on a computer, cause the computer to execute the method described above.
  • the embodiment of the present disclosure discloses a method, device, electronic device, computer-readable storage medium and computer program product of a visual-inertial odometer.
  • the method includes: fusing pose information of at least one sensor to obtain a first initial pose; Perform photometric optimization on the initial pose according to the pose transformation between two frames of images to obtain a second initial pose; perform feature point optimization on the second initial pose by extracting and matching feature points in the image to generate map points; create a back-end tight coupling optimization according to the correspondence between the map points and corner points; construct a feature point constraint matrix and a photometric constraint matrix through the back-end tight coupling optimization; for the feature point matrix and the photometric The constraint matrix is spliced to obtain the final constraint matrix.
  • the feature point method and photometric method in special SLAM are effectively combined, the front-end adopts the feature point method and photometric method to loosely couple, and the back-end optimization adopts the VIO method of feature point method and photometric method, which can meet different requirements.
  • the positioning and 3D reconstruction in the scene improve the robustness of the algorithm.
  • This disclosure proposes a VIO method in which the feature point method and the photometric method are loosely coupled at the front end, and the back-end optimization uses the feature point method and the photometric method to tightly couple the VIO method.
  • weak texture and the internal connection between these two methods solve the robustness of some current algorithms that are similar to running the two methods separately and splicing them together.
  • This method has good stability in some scenes where the illumination changes drastically but the texture is rich or the illumination changes little but the texture is weak, and can be applied in the fields of positioning and navigation.
  • FIG. 1 is a schematic flow chart of a visual-inertial odometer method provided by an embodiment of the present disclosure
  • FIG. 2 is a schematic diagram of a reprojection error of a feature point method provided by an embodiment of the present disclosure
  • FIG. 3 is a schematic diagram of back-end optimization constraint factors provided by an embodiment of the present disclosure.
  • FIG. 4 is a schematic diagram of a Hessian matrix in back-end optimization provided by an embodiment of the present disclosure
  • FIG. 5 is a schematic diagram of a feature point method constraint matrix provided by an embodiment of the present disclosure.
  • FIG. 6 is a schematic diagram of Hessian constraint matrix elimination provided by an embodiment of the present disclosure.
  • FIG. 7 is a wireframe flow chart of the visual-inertial mileage calculation method provided by an embodiment of the present disclosure.
  • Fig. 8 is a schematic diagram of a visual-inertial odometer device provided by another embodiment of the present disclosure.
  • FIG. 9 is a schematic structural diagram of an electronic device provided by another embodiment of the present disclosure.
  • the term “comprise” and its variations are open-ended, ie “including but not limited to”.
  • the term “based on” is “based at least in part on”.
  • the term “one embodiment” means “at least one embodiment”; the term “another embodiment” means “at least one further embodiment”; the term “some embodiments” means “at least some embodiments.” Relevant definitions of other terms will be given in the description below.
  • Visual SLAM is mainly divided into feature point method and photometric method (direct method).
  • the feature point method mainly relies on the feature points in the image to achieve data association, and has a strong dependence on the texture of the scene.
  • Photometry operates directly on pixels, but is more sensitive to lighting changes. Neither of them can run robustly in actual scenes, where there are usually situations where the lighting changes are drastic and the scene textures are not rich.
  • there are complementary situations between the two If the two methods can be well integrated together, the robustness of the visual SLAM algorithm will be improved to a certain extent.
  • the present disclosure proposes a VIO method combining the feature point method and the photometric method, so as to improve the robustness of the VIO algorithm.
  • This disclosure proposes a VIO method in which the feature point method and the photometric method are loosely coupled at the front end, and the back-end optimization uses the feature point method and the photometric method to tightly couple the VIO method.
  • weak texture and the internal connection between these two methods solve the robustness of some current algorithms that are similar to running the two methods separately and splicing them together.
  • This method has good stability in some scenes where the illumination changes drastically but the texture is rich or the illumination changes little but the texture is weak, and can be applied in the fields of positioning and navigation.
  • Fig. 1 is a schematic flow chart of the visual-inertial odometer method provided by the embodiment of the present disclosure.
  • the visual-inertial odometer method provided by the present embodiment can be executed by a visual-inertial odometer device, which can be implemented as software or implemented as software Combination with hardware, the device can be integrated in the electronic equipment or robot, and realized by the processor of the electronic equipment or robot control system. As shown in Figure 1, the method includes the following steps:
  • Step S101 Fusing pose information of at least one sensor to obtain a first initial pose.
  • the sensors in the present disclosure include a camera, an inertial sensor (IMU, Inertial Measurement Unit), and a wheel odometer.
  • the IMU sensor can predict a more accurate pose through pre-integration in a short period of time (the rotation information is more accurate), and the wheel odometer can also obtain the pose (the displacement information is more accurate), and the two pose information is fused to obtain a better initial pose.
  • the first initial pose is obtained by combining the IMU pre-integrated rotation and the wheel odometry translation.
  • the first initial pose is obtained by fusing the pose information of at least one sensor, including: using the rotation of the IMU sensor to obtain the rotation information in the pose information; using the translation of the wheel odometer to obtain the displacement information in the pose information; performing information fusion on the rotation information and the displacement information to obtain a first initial position.
  • Step S102 Perform photometric optimization on the initial pose according to the pose transformation between two frames of images to obtain a second initial pose.
  • step S102 the image is acquired by an image sensor (such as a camera).
  • the color image is first converted into a grayscale image, and the point with a large change in pixel gradient is extracted, and obtained in the next frame of image through optical flow tracking.
  • the position and construct the photometric error function and use the nonlinear least squares to optimize the initial pose. Since the pose transformation between the two frames has been predicted by the IMU and the wheel odometer, the position of the previous point in the subsequent image can be estimated, and then the photometric error function can be constructed.
  • the photometric optimization of the initial pose according to the pose transformation between two frames of images to obtain the second initial pose includes: acquiring an image through an image sensor; converting a color image into a grayscale image; extracting In the grayscale image, the pixel gradient change is greater than a certain threshold; the position in the next frame image is obtained by optical flow tracking and a photometric error function is constructed; a nonlinear least squares function is constructed according to the error function; The nonlinear least square function optimizes the first initial pose to obtain a second initial pose.
  • the photometric error function is expressed as follows:
  • r represents the photometric error
  • ⁇ h is the Huber weight
  • I 1 and I 2 represent two adjacent grayscale images
  • x 1 and x 2 are the pixel coordinates of the point X in the image in the space
  • x 2 is determined by x 1 projection
  • a, b are photometric affine transformation parameters.
  • x2 is obtained by projection of x1 , requiring the relative pose transformation ⁇ 21 between two frames and the inverse depth of x1 in I1 .
  • the nonlinear least squares function is expressed as follows:
  • J represents Jacobian matrix
  • ⁇ represents Lie algebra
  • r represents photometric error
  • i and N are natural numbers.
  • Step S103 Perform feature point optimization on the second initial pose by extracting and matching feature points in the image to generate map points.
  • a relatively accurate pose is obtained after the photometric optimization in steps S101 and S102, that is, the second initial position here, but the feature point method is used for optimization in order to make the tracking process more accurate.
  • feature extraction and matching are required. Screen out points that meet the threshold condition as corner points from the points with large gradient changes in the extracted pixels. Among them, the points with large changes can be characterized as pixel gradient changes greater than the first threshold, and further filter out feature points greater than the second threshold condition. As a corner point, where the second threshold is greater than the first threshold.
  • data association is realized. Then match by extracting the feature points in the image and the description of the feature points, and finally perform feature point reprojection optimization to obtain a more accurate pose, and use this pose to generate map points.
  • the feature point optimization of the second initial pose by extracting and matching feature points in the image to generate map points includes: converting the color image into a grayscale image; extracting pixels in the grayscale image A pixel point whose gradient change is greater than a certain threshold is used as a feature point of the image; a feature point whose pixel gradient change is greater than a second threshold is selected from the feature points of the image as a corner point; through the description of the corner point and the Perform association matching on the data of the second initial pose; construct a reprojection error function according to the data after association matching; perform feature point optimization on the second initial pose according to the reprojection error function; optimize the second initial pose according to the feature points The pose and the corner points generate map points.
  • the reprojection error function is constructed as shown in Figure 2, which shows the feature point method weight provided by an embodiment of the present disclosure. Schematic diagram of projection error.
  • the reprojection error function is constructed by the same method as the photometric error function.
  • the specific data amount expression is as follows:
  • ⁇ * is the reprojection error
  • u i is the known 2D homogeneous coordinates
  • s i is the scale
  • K is the internal parameters of the camera
  • ⁇ is the unknown camera pose
  • P i is the known 3D point Homogeneous coordinates
  • i and N are natural numbers.
  • Step S104 Create a back-end tight coupling optimization according to the correspondence between the map points and the corner points.
  • FIG. 3 shows a schematic diagram of back-end optimization constraint factors provided by an embodiment of the present disclosure.
  • L 1 and L 2 in the figure represent map points, hollow circles represent feature point reprojection constraint factors, and solid circles represent photometric constraint factors.
  • T h is the dominant frame (the inverse depth of the map point comes from this frame), and T t is the target frame.
  • L1 is a map point generated by a corner point, so it contains two kinds of constraints: photometric constraint factor and projection constraint factor.
  • L2 is generated by ordinary points and contains only one constraint factor: photometric constraint factor. Therefore, in the back-end optimization, if the map points are generated by corner points, two kinds of constraint factors are included, and the constraints of the feature point method and the photometric method are tightly coupled together.
  • the back-end optimization problem is constructed by whether the map points are generated by corner points. If the map points are generated by corner points, the back-end optimization includes both feature point constraints and photometric constraints. The Hessian matrix constructed by the two The corresponding parts are added to obtain the final constraint matrix. If the map points are not composed of corner points, only photometric constraints are included, and the update amount of the variable to be optimized is solved by nonlinear least squares.
  • the creation of the back-end tight coupling optimization according to the correspondence between the map points and the corner points includes: creating the back-end tight coupling optimization according to whether the map points are generated by the corner points; if the map Points are generated by the corner points, then the back-end tightly coupled optimization includes feature point constraints and photometric constraints, and the feature point constraints and the photometric constraints are tightly coupled; if the map point is not formed by the corner Point composition, it includes photometric constraints, and solves the update amount of the variable to be optimized by nonlinear least squares.
  • the variables to be optimized include camera internal parameters, pose, feature point inverse depth and photometric affine transformation parameters.
  • Step S105 Construct a feature point constraint matrix and a photometric constraint matrix through the back-end tight coupling optimization.
  • the variables to be optimized in the feature point method include: internal camera parameters, pose, and inverse depth of landmark points.
  • the variables to be optimized in the photometric method also include photometric affine transformation parameters a and b.
  • the construction of the feature point constraint matrix and the photometric constraint matrix through the back-end tight coupling optimization includes: respectively obtaining the relationship between the variable to be changed in the feature point constraint and the photometric constraint according to the photometric error function and the reprojection error function Derivatives of ; according to the derivatives, the feature point constraint matrix and the photometric constraint matrix are respectively constructed.
  • the content of the specific constraint matrix is shown in Figure 5
  • p represents the inverse depth of the map point
  • C represents the camera internal reference
  • x represents the camera pose.
  • the transposition of Hpx is equal to Hxp.
  • the Hxx part and bx part in the figure are only filled with x m as the host frame and x t as the target frame. If host and target are the other two frames, you need to put The corresponding numbered frames are accumulated, for example, if the host frame is x 1 and the target frame is x t , then this part of the constraint needs to be added to the part corresponding to Hxx and bx.
  • the photometric constraint will also get a matrix of a similar form, and the photometric constraint has two more photometric affine transformation parameters than the feature point method constraint.
  • Step S106 Concatenate the feature point matrix and the photometric constraint matrix to obtain a final constraint matrix.
  • the variable to be optimized for the difference between the photometric constraint and the projection constraint mentioned in the previous step is the photometric transformation parameter.
  • the back-end optimization problem is constructed by whether the map point is generated by the corner point. If If the map points are generated by corner points, then the back-end optimization includes both feature point constraints and photometric constraints, then construct the feature point constraint matrix and photometric constraint matrix at the same time, and add the corresponding parts of the two; if the map points are not composed of corner points, then Only photometric constraints are included, and the update amount of the variable to be optimized is solved by nonlinear least squares.
  • Figure 6 it is a schematic diagram of the Hessian constraint matrix elimination provided by an embodiment of the present disclosure. The update amount of the variable to be optimized is eliminated, and H and b are obtained after the constraint matrix is eliminated. The photometric constraint will obtain H guang and b guang , feature point constraints will get H point and b point .
  • the splicing of the constraint matrix on the feature point matrix and the photometric constraint matrix to obtain the final constraint matrix includes: combining the same variable to be optimized in the feature point constraint matrix with the photometric constraint matrix Partial addition is performed to obtain the update amount of the variable to be optimized; the update amount of the variable to be optimized is eliminated to obtain the final constraint matrix.
  • the visual-inertial odometry method in the embodiment of the present disclosure also includes: updating the variables to be optimized in the feature point constraint matrix and the photometric constraint matrix; the updated specific The process is: solve the update amount according to whether the map point is composed of corner points; if the map point is generated by the corner point, then the back-end tight coupling optimization includes feature point constraints and photometric constraints, if the If the map point is not composed of the corner points, it includes a photometric constraint; the variable to be optimized is directly added incrementally, and the rotation information in the second pose is multiplied.
  • FIG. 7 is a wireframe flowchart of a visual-inertial odometry calculation method provided by an embodiment of the present disclosure.
  • the sensors in the figure include a camera, an inertial sensor (IMU, Inertial Measurement Unit), and a wheel odometer.
  • IMU inertial sensor
  • the wheel odometer can also obtain the pose (the displacement information is more accurate), and the two pose information is fused Get a better initial pose.
  • the first initial pose is obtained by combining the IMU pre-integrated rotation and the wheel odometer translation.
  • the image sensor (such as a camera) is used to collect images, perform photometric optimization on the first initial pose obtained according to the pose transformation between two frames in the collected image, convert the acquired color image into a grayscale image, and extract pixels
  • the position in the next frame of the image is obtained through optical flow tracking and a photometric error function is constructed, and the initial pose is optimized by nonlinear least squares to obtain the optimized second initial pose.
  • the pose transformation between them can estimate the position of the previous point in the following image, and then construct the photometric error function.
  • the back-end tight coupling optimization is created according to the correspondence between the map points and the corner points, and the back-end optimization problem is constructed by whether the map points are generated by the corner points. If the map points are generated by the corner points, the back-end Optimization includes both feature point constraints and photometric constraints, then construct the feature point constraint matrix and photometric constraint matrix at the same time, and add the corresponding parts of the two; if the map points are not composed of corner points, only photometric constraints are included, through nonlinear least squares Multiply to get the update amount of the variable to be optimized. The error items in the matrix are added with respect to the same variables to be optimized to obtain the final constraint matrix. After obtaining the final constraint matrix, iterative Europeanization solves the increment of the variables to be optimized, so as to update the variables to be optimized.
  • Fig. 8 shows a schematic diagram of a visual-inertial odometer device provided by another embodiment of the present disclosure.
  • the device includes: a fusion module 801 , a photometric optimization module 802 , a feature point optimization module 803 , a tight coupling optimization module 804 , a matrix construction module 805 and a matrix splicing module 806 . in:
  • the fusion module 801 is configured to fuse pose information of at least one sensor to obtain a first initial pose.
  • This module is specifically used to: use the rotation of the IMU sensor to obtain the rotation information in the pose information; use the translation of the wheel odometer to obtain the displacement information in the pose information; perform information fusion on the rotation information and the displacement information to obtain first initial position.
  • the first initial pose is obtained by using the combination of IMU pre-integration rotation and wheel odometer translation.
  • the photometric optimization module 802 is configured to perform photometric optimization on the initial pose according to the pose transformation between two frames of images to obtain a second initial pose.
  • This module is specifically used for: acquiring an image through an image sensor; converting a color image into a grayscale image; extracting pixels in the grayscale image whose pixel gradient changes are greater than a certain threshold; obtaining the pixel in the next frame of image through optical flow tracking position and construct a photometric error function; construct a nonlinear least square function according to the error function; optimize the first initial pose by using the nonlinear least square function to obtain a second initial pose.
  • the photometric error function is expressed as follows:
  • r represents the photometric error
  • ⁇ h is the Huber weight
  • I 1 and I 2 represent two adjacent grayscale images
  • x 1 and x 2 are the pixel coordinates of the point X in the image in the space
  • x 2 is determined by x 1 projection
  • a, b are photometric affine transformation parameters.
  • x2 is obtained by projection of x1 , requiring the relative pose transformation ⁇ 21 between two frames and the inverse depth of x1 in I1 .
  • the nonlinear least squares function is expressed as follows:
  • J represents Jacobian matrix
  • ⁇ represents Lie algebra
  • r represents photometric error
  • i and N are natural numbers.
  • the feature point optimization module 803 is configured to perform feature point optimization on the second initial pose by extracting and matching feature points in the image to generate map points.
  • a more accurate pose is obtained, that is, the second initial position here.
  • feature extraction and matching are required, and the corner points that meet the threshold condition are selected from the points with large gradient changes in the extracted pixels.
  • the points with large changes can be characterized as pixel gradient changes greater than the first threshold, and feature points greater than the second threshold condition are further screened out as corner points, wherein the second threshold is greater than the first threshold.
  • data association is realized. Then match by extracting the feature points in the image and the description of the feature points, and finally perform feature point reprojection optimization to obtain a more accurate pose, and use this pose to generate map points. .
  • the acquiring module is specifically used for: converting a color image into a grayscale image; extracting a pixel point in the grayscale image whose pixel gradient change is greater than a certain threshold as a feature point of the image; from the feature point of the image Screen out the feature points whose pixel gradient changes are greater than the second threshold as corner points; perform association matching with the data of the second initial pose through the description of the corner points; construct a reprojection error function according to the data after association matching; The reprojection error function performs feature point optimization on the second initial pose; map points are generated according to the second initial pose after feature point optimization and the corner points.
  • the tight-coupling optimization module 804 is configured to create a back-end tight-coupling optimization according to the correspondence between the map points and corner points.
  • the back-end optimization module Create a back-end tightly coupled optimization module, and construct the back-end optimization problem by whether the map points are generated by corner points. If the map points are generated by corner points, the back-end optimization includes both feature point constraints and photometric constraints. The two constructed The corresponding parts in the Hessian matrix are added to obtain the final constraint matrix. If the map points are not composed of corner points, only photometric constraints are included, and the update amount of the variable to be optimized is solved by nonlinear least squares.
  • This module is specifically used for: creating the back-end tight coupling optimization according to the corresponding relationship between the map point and the corner point, including: creating the back-end tight coupling optimization according to whether the map point is generated by the corner point; if The map points are generated by the corner points, then the back-end tight coupling optimization includes feature point constraints and photometric constraints, and the feature point constraints and the photometric constraints are tightly coupled; if the map points are not composed of The composition of the corner points includes photometric constraints, and the update amount of the variable to be optimized is calculated by non-linear least squares.
  • the variables to be optimized include camera internal parameters, pose, feature point inverse depth and photometric affine transformation parameters.
  • the matrix construction module 805 is configured to construct a feature point constraint matrix and a photometric constraint matrix through the back-end tight coupling optimization.
  • This module is specifically used to: obtain the derivative between the variable to be changed in the feature point constraint and the photometric constraint according to the photometric error function and the reprojection error function respectively; construct the feature point constraint matrix and the photometric constraint matrix respectively according to the derivative.
  • the matrix splicing module 806 is configured to splice constraint matrices on the feature point matrix and the photometric constraint matrix to obtain a final constraint matrix.
  • This module is specifically used to construct the back-end optimization problem based on whether the map points are generated by corner points. If the map points are generated by corner points, the back-end optimization includes both feature point constraints and photometric constraints, and the feature point constraint matrix is constructed at the same time. and the photometric constraint matrix, add the corresponding parts of the two; if the map points are not composed of corner points, only photometric constraints are included, and the update amount of the variable to be optimized is calculated by nonlinear least squares. Add the part of the variable to be optimized in the feature point constraint matrix that is the same as that in the photometric constraint matrix to obtain the update amount of the variable to be optimized; perform elimination processing on the update amount of the variable to be optimized to obtain the final constraint matrix.
  • the visual-inertial odometry device also includes:
  • An update module configured to update the variables to be optimized in the feature point constraint matrix and the photometric constraint matrix.
  • the update module is specifically used to: solve the update amount by whether the map point is composed of corner points; if the map point is generated by the corner point, then the back-end tightly coupled optimization includes feature point constraints and photometric constraints , if the map point is not formed by the corner point, it includes a photometric constraint; directly perform incremental addition on the variable to be optimized, and multiply the rotation information in the second pose.
  • the device shown in FIG. 8 can execute the method of the embodiment shown in FIG. 1 .
  • the device shown in FIG. 8 can execute the method of the embodiment shown in FIG. 1 .
  • FIG. 9 it shows a schematic structural diagram of an electronic device 900 suitable for implementing another embodiment of the present disclosure.
  • the terminal equipment in the embodiment of the present disclosure may include but not limited to such as mobile phone, notebook computer, digital broadcast receiver, PDA (personal digital assistant), PAD (tablet computer), PMP (portable multimedia player), vehicle terminal (such as mobile terminals such as car navigation terminals) and fixed terminals such as digital TVs, desktop computers and the like.
  • the electronic device shown in FIG. 9 is only an example, and should not limit the functions and application scope of the embodiments of the present disclosure.
  • an electronic device 900 may include a processing device (such as a central processing unit, a graphics processing unit, etc.) 901, which may be randomly accessed according to a program stored in a read-only memory (ROM) 902 or loaded from a storage device 908.
  • a processing device such as a central processing unit, a graphics processing unit, etc.
  • RAM random access memory
  • Various appropriate actions and processes are executed by programs in the memory (RAM) 903 .
  • RAM 903 In the RAM 903, various programs and data necessary for the operation of the electronic device 900 are also stored.
  • the processing device 901, ROM 902, and RAM 903 are connected to each other through a communication line 904.
  • An input/output (I/O) interface 905 is also connected to the communication line 904 .
  • the following devices can be connected to the I/O interface 905: input devices 906 including, for example, a touch screen, touchpad, keyboard, mouse, camera, microphone, accelerometer, gyroscope, etc.; including, for example, a liquid crystal display (LCD), speaker, vibration an output device 907 such as a computer; a storage device 908 including, for example, a magnetic tape, a hard disk, etc.; and a communication device 909.
  • the communication means 909 may allow the electronic device 900 to perform wireless or wired communication with other devices to exchange data. While FIG. 9 shows electronic device 900 having various means, it is to be understood that implementing or having all of the means shown is not a requirement. More or fewer means may alternatively be implemented or provided.
  • embodiments of the present disclosure include a computer program product, which includes a computer program carried on a non-transitory computer readable medium, where the computer program includes program code for executing the method shown in the flowchart.
  • the computer program may be downloaded and installed from a network via communication means 909, or from storage means 908, or from ROM 902.
  • the processing device 901 the above-mentioned functions defined in the methods of the embodiments of the present disclosure are executed.
  • the above-mentioned computer-readable medium in the present disclosure may be a computer-readable signal medium or a computer-readable storage medium or any combination of the above two.
  • a computer readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of computer-readable storage media may include, but are not limited to, electrical connections with one or more wires, portable computer diskettes, hard disks, random access memory (RAM), read-only memory (ROM), erasable Programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination of the above.
  • a computer-readable storage medium may be any tangible medium that contains or stores a program that can be used by or in conjunction with an instruction execution system, apparatus, or device.
  • a computer-readable signal medium may include a data signal propagated in baseband or as part of a carrier wave carrying computer-readable program code therein. Such propagated data signals may take many forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the foregoing.
  • a computer-readable signal medium may also be any computer-readable medium other than a computer-readable storage medium, which can transmit, propagate, or transmit a program for use by or in conjunction with an instruction execution system, apparatus, or device .
  • Program code embodied on a computer readable medium may be transmitted by any appropriate medium, including but not limited to wires, optical cables, RF (radio frequency), etc., or any suitable combination of the above.
  • the client and the server can communicate using any currently known or future network protocols such as HTTP (HyperText Transfer Protocol, Hypertext Transfer Protocol), and can communicate with digital data in any form or medium
  • HTTP HyperText Transfer Protocol
  • the communication eg, communication network
  • Examples of communication networks include local area networks (“LANs”), wide area networks (“WANs”), internetworks (e.g., the Internet), and peer-to-peer networks (e.g., ad hoc peer-to-peer networks), as well as any currently known or future developed network of.
  • the above-mentioned computer-readable medium may be included in the above-mentioned electronic device, or may exist independently without being incorporated into the electronic device.
  • the above-mentioned computer-readable medium carries one or more programs, and when the above-mentioned one or more programs are executed by the electronic device, the electronic device: executes the interaction method in the above-mentioned embodiment.
  • Computer program code for carrying out operations of the present disclosure may be written in one or more programming languages, or combinations thereof, including but not limited to object-oriented programming languages—such as Java, Smalltalk, C++, and Includes conventional procedural programming languages—such as the "C" language or similar programming languages.
  • the program code may execute entirely on the user's computer, partly on the user's computer, as a stand-alone software package, partly on the user's computer and partly on a remote computer or entirely on the remote computer or server.
  • the remote computer can be connected to the user computer through any kind of network, including a local area network (LAN) or a wide area network (WAN), or it can be connected to an external computer (such as through an Internet service provider). Internet connection).
  • LAN local area network
  • WAN wide area network
  • Internet service provider such as AT&T, MCI, Sprint, EarthLink, MSN, GTE, etc.
  • each block in a flowchart or block diagram may represent a module, program segment, or portion of code that contains one or more logical functions for implementing specified executable instructions.
  • the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or they may sometimes be executed in the reverse order, depending upon the functionality involved.
  • each block of the block diagrams and/or flowchart illustrations, and combinations of blocks in the block diagrams and/or flowchart illustrations can be implemented by a dedicated hardware-based system that performs the specified functions or operations , or may be implemented by a combination of dedicated hardware and computer instructions.
  • the units involved in the embodiments described in the present disclosure may be implemented by software or by hardware. Wherein, the name of a unit does not constitute a limitation of the unit itself under certain circumstances.
  • FPGAs Field Programmable Gate Arrays
  • ASICs Application Specific Integrated Circuits
  • ASSPs Application Specific Standard Products
  • SOCs System on Chips
  • CPLD Complex Programmable Logical device
  • a machine-readable medium may be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, apparatus, or device.
  • a machine-readable medium may be a machine-readable signal medium or a machine-readable storage medium.
  • a machine-readable medium may include, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any suitable combination of the foregoing.
  • machine-readable storage media would include one or more wire-based electrical connections, portable computer discs, hard drives, random access memory (RAM), read only memory (ROM), erasable programmable read only memory (EPROM or flash memory), optical fiber, compact disk read only memory (CD-ROM), optical storage, magnetic storage, or any suitable combination of the foregoing.
  • RAM random access memory
  • ROM read only memory
  • EPROM or flash memory erasable programmable read only memory
  • CD-ROM compact disk read only memory
  • magnetic storage or any suitable combination of the foregoing.
  • an electronic device including: at least one processor; and a memory connected in communication with the at least one processor; wherein, the memory stores information that can be used by the Instructions executed by at least one processor, the instructions being executed by the at least one processor, so that the at least one processor can execute any one of the methods in the foregoing first aspect.
  • non-transitory computer-readable storage medium which is characterized in that the non-transitory computer-readable storage medium stores computer instructions, and the computer instructions are used to cause a computer to execute the aforementioned Any one of the methods of the first aspect.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Automation & Control Theory (AREA)
  • Multimedia (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computing Systems (AREA)
  • Databases & Information Systems (AREA)
  • Evolutionary Computation (AREA)
  • General Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Image Analysis (AREA)

Abstract

本公开提供了一种视觉惯性里程计的方法、装置、电子设备、存储介质及计算机程序，所述方法包括：融合至少一个传感器的位姿信息得到第一初始位姿；根据两帧图像之间的位姿变换对所述初始位姿进行光度优化，获得第二初始位姿；通过提取和匹配图像中的特征点对所述第二初始位姿进行特征点优化，生成地图点；根据所述地图点与角点的对应关系来创建后端紧耦合优化；通过所述后端紧耦合优化构建特征点约束矩阵和光度约束矩阵；对所述特征点矩阵和所述光度约束矩阵进行约束矩阵的拼接，得到最终约束矩阵。根据本方法，前端采用特征点法和光度法松耦合，后端优化采用特征点法作用和光度法紧耦合的VIO方法，能够满足不同场景下的定位和三维重建，提高算法的鲁棒性。

Description

视觉惯性里程计方法、装置、电子设备、存储介质及计算机程序
交叉引用
本申请要求2021年09月30日递交的、申请号为“202111162776.X”、发明名称为“视觉惯性里程计方法、装置及电子设备”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。
技术领域
本公开涉及SLAM(simultaneous localization and mapping，中文名称：即时定位与地图构建，或并发建图与定位)领域，尤其涉及一种视觉惯性里程计(VIO，Visual-Inertial Odometry)的方法、装置、电子设备、存储介质及计算机程序。
背景技术
随着定位导航的发展，尤其是SLAM技术的广泛应用，机器人在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上建造增量式地图，实现机器人的自主定位和导航。
不同场景对SLAM建图、VO(Visual Odometry，VO)定位及三维重建有不同的参数要求。目前视觉SLAM根据原理可分为两类：特征点法SLAM和光度法SLAM。光度法根据图像中像素梯度变化较大的点的光度误差建立目标函数，优化求解相机的位姿和地图点位置。特征点法首先需要提取图像中的特征点以及描述子，然后根据描述子确定特征点之间的匹配关系，并根据匹配点之间的投影误差建立目标函数。光度法对场景中的纹理要求不高但对光照变化比较敏感， 而特征点法对光照变化不敏感但纹理要求较高。因此如何构建满足不同场景需求的SLAM算法是本领域亟需解决的问题。
发明内容
为了解决上述技术问题，满足多种光线场景下的定位及三维重建，本公开实施方式的目的在于提供一种视觉惯性里程计方法、装置、电子设备、存储介质、存储介质及计算机程序。
根据本公开第一方面，本公开的实施例提供了一种视觉惯性里程计的方法，包括：
融合至少一个传感器的位姿信息得到第一初始位姿；
根据两帧图像之间的位姿变换对所述初始位姿进行光度优化，获得第二初始位姿；
通过提取和匹配图像中的特征点对所述第二初始位姿进行特征点优化，生成地图点；
根据所述地图点与角点的对应关系来创建后端紧耦合优化；
通过所述后端紧耦合优化构建特征点约束矩阵和光度约束矩阵；
对所述特征点矩阵和所述光度约束矩阵进行约束矩阵的拼接，得到最终约束矩阵。
进一步的，所述融合至少一个传感器的位姿信息得到第一初始位姿，包括：
利用惯性传感器(IMU，Inertial Measurement Unit)的旋转得到位姿信息中的旋转信息；
利用轮式里程计的平移得到位姿信息中的位移信息；
将所述旋转信息和所述位移信息进行信息融合得到第一初始位置。
进一步的，所述根据两帧图像之间的位姿变换对所述初始位姿进行光度优化，获得第二初始位姿，包括：
通过图像传感器获取图像；
将彩色图像转化为灰度图像；
提取所述灰度图像中像素梯度变化大于一定阈值的像素点；
通过光流跟踪得到在下一帧图像中的位置并构建光度误差函数；
根据所述误差函数构建非线性最小二乘函数；
利用所述非线性最小二乘函数优化所述第一初始位姿，获得第二初始位姿。
进一步的，所述光度误差函数表示如下：
r＝ω h(I 2[x 2]-(a 21I 1[x 1]+b 21))
其中，r表示光度误差，ω h为Huber权重，I 1，I 2表示相邻的两幅灰度图像，x 1，x 2分别是空间中点X在图像中的像素坐标，x 2是由x 1投影获得，a，b为光度仿射变换参数。
x 2是由x 1投影获得，需要两帧之间的相对位姿变换ξ 21和x 1在I 1中的逆深度。
进一步的，所述非线性最小二乘函数表示如下：
Figure PCTCN2022109502-appb-000001
其中，J表示雅可比矩阵，ξ为李代数，表示旋转和平移，r表示光度误差，i、N为自然数。
进一步的，所述通过提取和匹配图像中的特征点对所述第二初始位姿进行特征点优化，生成地图点，包括：
将彩色图像转化为灰度图像；
提取所述灰度图像中像素梯度变化大于一定阈值的像素点作为所述图像的特征点；
从所述图像的特征点中筛选出像素梯度变化大于第二阈值的特征点作为角点；
通过所述角点的描述与所述第二初始位姿的数据进行关联匹配；
根据关联匹配后的数据构建重投影误差函数；
根据重投影误差函数对所述第二初始位姿进行特征点优化；
根据特征点优化后的第二初始位姿和所述角点生成地图点。
进一步的，所述重投影误差函数表示如下：
Figure PCTCN2022109502-appb-000002
其中，ξ *为重投影误差，u i是已知的2D齐次坐标，s i表示尺度，K为相机的内部参数，ξ^是未知的相机位姿，P i为已知的3D点的齐次坐标，i、N为自然数。
进一步的，所述根据所述地图点与角点的对应关系来创建后端紧耦合优化，包括：
通过所述地图点是否由所述角点生成来创建后端紧耦合优化；
如果所述地图点是由所述角点生成，则所述后端紧耦合优化包含特征点约束和光度约束，将所述特征点约束和所述光度约束进行紧耦合；
如果所述地图点不是由所述角点构成，则包含光度约束，并通过非线性最小二乘求解出待优化变量的更新量。
进一步的，所述通过所述后端紧耦合优化构建特征点约束矩阵和光度约束矩阵，包括：
根据光度误差函数和重投影误差函数分别获取所述特征点约束和光度约束中待变化变量之间的导数；
根据所述导数分别构建特征点约束矩阵和光度约束矩阵。
进一步的，所述对所述特征点矩阵和所述光度约束矩阵进行约束矩阵的拼接，得到最终约束矩阵，包括：
将所述特征点约束矩阵中与所述光度约束矩阵中相同的待优化变量部分的相加，获取待优化变量的更新量；
对所述待优化变量的更新量进行消元处理，得到最终的约束矩阵。
进一步的，所述方法还包括：
对所述特征点约束矩阵中与所述光度约束矩阵中的待优化变量进行更新；包括：
通过所述地图点是否由角点构成来求解更新量；
如果所述地图点是由所述角点生成，则所述后端紧耦合优化包含特征点约束和光度约束，如果所述地图点不是由所述角点构成，则包含光度约束；
对所述待优化变量直接进行增量相加，对所述第二位姿中的旋转信息进行相乘。
进一步的，所述待优化变量包括相机内部参数、位姿、特征点逆深度和光度仿射变换参数。
第二方面，本公开另一实施例提供了一种视觉惯性里程计装置，包括：
融合模块，用于融合至少一个传感器的位姿信息得到第一初始位姿；
光度优化模块，用于根据两帧图像之间的位姿变换对所述初始位姿进行光度优化，获得第二初始位姿；
特征点优化模块，用于通过提取和匹配图像中的特征点对所述第二初始位姿进行特征点优化，生成地图点；
紧耦合优化模块，用于根据所述地图点与角点的对应关系来创建后端紧耦合优化；
矩阵构建模块，用于通过所述后端紧耦合优化构建特征点约束矩阵和光度约束矩阵；
矩阵拼接模块，用于对所述特征点矩阵和所述光度约束矩阵进行约束矩阵的拼接，得到最终约束矩阵。
进一步的，所述融合模块，用于包括：
利用惯性传感器的旋转得到位姿信息中的旋转信息；
利用轮式里程计的平移得到位姿信息中的位移信息；
将所述旋转信息和所述位移信息进行信息融合得到第一初始位置。
进一步的，所述光度优化模块，用于包括：
通过图像传感器获取图像；
将彩色图像转化为灰度图像；
提取所述灰度图像中像素梯度变化大于一定阈值的像素点；
通过光流跟踪得到在下一帧图像中的位置并构建光度误差函数；
根据所述误差函数构建非线性最小二乘函数；
利用所述非线性最小二乘函数优化所述第一初始位姿，获得第二初始位姿。
进一步的，所述光度误差函数表示如下：
r＝ω h(I 2[x 2]-(a 21I 1[x 1]+b 21))
其中，r表示光度误差，ω h为Huber权重，I 1，I 2表示相邻的两幅灰度图像，x 1，x 2分别是空间中点X在图像中的像素坐标，x 2是由x 1投影获得，a，b为光度仿射变换参数。
x 2是由x 1投影获得，需要两帧之间的相对位姿变换ξ 21和x 1在I 1中的逆深度。
进一步的，所述非线性最小二乘函数表示如下：
Figure PCTCN2022109502-appb-000003
其中，J表示雅可比矩阵，ξ为李代数，表示旋转和平移，r表示光度误差，i、N为自然数。
进一步的，所述特征点优化模块，用于包括：
将彩色图像转化为灰度图像；
提取所述灰度图像中像素梯度变化大于一定阈值的像素点作为所述图像的特征点；
从所述图像的特征点中筛选出像素梯度变化大于第二阈值的特征点作为角点；
通过所述角点的描述与所述第二初始位姿的数据进行关联匹配；
根据关联匹配后的数据构建重投影误差函数；
根据重投影误差函数对所述第二初始位姿进行特征点优化；
根据特征点优化后的第二初始位姿和所述角点生成地图点。
进一步的，所述重投影误差函数表示如下：
Figure PCTCN2022109502-appb-000004
其中，ξ *为重投影误差，u i是已知的2D齐次坐标，s i表示尺度，K为相机的内部参数，ξ^是未知的相机位姿，P i为已知的3D点的齐次坐标，i、N为自然数。
进一步的，所述紧耦合优化模块，用于包括：
通过所述地图点是否由所述角点生成来创建后端紧耦合优化；
如果所述地图点是由所述角点生成，则所述后端紧耦合优化包含特征点约束和光度约束，将所述特征点约束和所述光度约束进行紧耦合；
如果所述地图点不是由所述角点构成，则包含光度约束，并通过非线性最小二乘求解出待优化变量的更新量。
进一步的，所述矩阵构建模块，用于包括：
根据光度误差函数和重投影误差函数分别获取所述特征点约束和光度约束中待变化变量之间的导数；
根据所述导数分别构建特征点约束矩阵和光度约束矩阵。
进一步的，所述矩阵拼接模块，用于包括：
将所述特征点约束矩阵中与所述光度约束矩阵中相同的待优化变量部分相加，获取待优化变量的更新量；
对所述待优化变量的更新量进行消元处理，得到最终的约束矩阵。
进一步的，所述装置还包括：
更新模块，用于对所述特征点约束矩阵中与所述光度约束矩阵中的待优化变量进行更新；
所述更新模块，用于包括：
通过所述地图点是否由角点构成来求解更新量；
如果所述地图点是由所述角点生成，则所述后端紧耦合优化包含特征点约束和光度约束，如果所述地图点不是由所述角点构成，则包含光度约束；
对所述待优化变量直接进行增量相加，对所述第二位姿中的旋转信息进行相乘。
进一步的，所述待优化变量包括相机内部参数、位姿、特征点逆深度和光度仿射变换参数。
第三方面，本公开另一实施例提供一种电子设备，包括：
存储器，用于存储计算机可读指令；以及
处理器，用于运行所述计算机可读指令，使得所述电子设备实现上述第一方面中任意一项所述的方法。
第四方面，本公开另一实施例提供一种非暂态计算机可读存储介质，用于存储计算机可读指令，当所述计算机可读指令由计算机执行时，使得所述计算机实现上述第一方面中任意一项所述的方法。
第五方面，本公开另一实施例提供了一种计算机程序产品，包括指令，当其在计算机上运行时，使得计算机执行上述所述的方法。
本公开实施例公开了一种视觉惯性里程计的方法、装置、电子设备、计算机可读存储介质及计算机程序产品，所述方法包括：融合至少一个传感器的位姿信息得到第一初始位姿；根据两帧图像之间的位姿变换对所述初始位姿进行光度优化，获得第二初始位姿；通过提取和匹配图像中的特征点对所述第二初始位姿进行特征点优化，生成地图点；根据所述地图点与角点的对应关系来创建后端紧耦合优化；通过所述后端紧耦合优化构建特征点约束矩阵和光度约束矩阵；对所述特征点矩阵和所述光度约束矩阵进行约束矩阵的拼接，得到最终约束矩阵。根据本方法，将特SLAM中的特征点法和光度法进行有效结合，前端采用特征点法和光度法松耦合，后端优化采用特征点法作用和光度法紧耦合的VIO方法，能够满足不同场景下的定位和三维重建，提高算法的鲁棒性。
本公开提出了一种前端采用特征点法和光度法松耦合，后端优化采用特征 点法作用和光度法紧耦合的VIO方法，该方法考虑到了特征点法和光度法对不同环境(光照变化、弱纹理)的互补作用以及这两种方法之间的内在联系，解决了目前一些类似将两种方法分开运行拼接在一起的算法的鲁棒性。该方法在一些光照变化剧烈但纹理丰富或者光照变化不大但纹理较弱的场景下有较好的稳定性，可应用在定位、导航领域。
上述说明仅是本公开技术方案的概述，为了能更清楚了解本公开的技术手段，而可依照说明书的内容予以实施，并且为让本公开的上述和其他目的、特征和优点能够更明显易懂，以下特举较佳实施例，并配合附图，详细说明如下。
附图说明
图1为本公开一实施例提供的视觉惯性里程计方法流程示意图；
图2为本公开一实施例提供的特征点法重投影误差示意图；
图3为本公开一实施例提供的后端优化约束因子示意图；
图4为本公开一实施例提供的后端优化中的Hessian矩阵示意图；
图5为本公开一实施例提供的特征点法约束矩阵示意图；
图6为本公开一实施例提供的Hessian约束矩阵消元示意图；
图7为本公开一实施例提供的视觉惯性里程计算法的线框流程图；
图8为本公开另一实施例提供的视觉惯性里程计装置示意图；
图9为本公开另一实施例提供的电子设备的结构示意图。
具体实施方式
为了能够更清楚地描述本公开的技术内容，下面结合具体实施例来进行进一步的描述。
以下示例性实施例中所描述的实施方式并不代表与本申请相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本申请的一些方面相一致的装置和方法的例子。
下面将参照附图更详细地描述本公开的实施例。虽然附图中显示了本公开 的某些实施例，然而应当理解的是，本公开可以通过各种形式来实现，而且不应该被解释为限于这里阐述的实施例，相反提供这些实施例是为了更加透彻和完整地理解本公开。应当理解的是，本公开的附图及实施例仅用于示例性作用，并非用于限制本公开的保护范围。
应当理解，本公开的方法实施方式中记载的各个步骤可以按照不同的顺序执行，和/或并行执行。此外，方法实施方式可以包括附加的步骤和/或省略执行示出的步骤。本公开的范围在此方面不受限制。
本文使用的术语“包括”及其变形是开放性包括，即“包括但不限于”。术语“基于”是“至少部分地基于”。术语“一个实施例”表示“至少一个实施例”；术语“另一实施例”表示“至少一个另外的实施例”；术语“一些实施例”表示“至少一些实施例”。其他术语的相关定义将在下文描述中给出。
需要注意，本公开中提及的“第一”、“第二”等概念仅用于对不同的装置、模块或单元进行区分，并非用于限定这些装置、模块或单元所执行的功能的顺序或者相互依存关系。
需要注意，本公开中提及的“一个”、“多个”的修饰是示意性而非限制性的，本领域技术人员应当理解，除非在上下文另有明确指出，否则应该理解为“一个或多个”。
在本公开使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请。在本公开和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。下面参考附图详细描述公开的各实施方式。
在视觉SLAM的应用上，自动驾驶与智能机器人领域应用越来越多，定位与三维重建的精度和鲁棒性会越来越高。具体情况描述：视觉SLAM主要分为特征点法和光度法(直接法)，特征点法主要依赖于图像中的特征点实现数据关联，对场景的纹理依赖较强。光度法直接对像素操作，但对光照变化比较敏感。二者在实际场景中都无法鲁棒的运行，实际场景中通常会遇到光照变化比较剧烈、场景纹理不丰富的情况。但是二者之间有互补的情况，如果能够较好的将这两 种方法很好的融合在一起，对提高视觉SLAM算法的鲁棒性有一定的提升。
而在SLAM算法中，将特征点法和光度法结合的SLAM算法并不多，可以将特征点法和光度法分别运行在两个独立的线程中，在弱纹理场景下特征点法丢失时能够利用光度法对其重新初始化，而特征点法则主要用于消除累计误差，对于光照变化的情况并不能提供帮助，是一种松耦合的方式将二者组合在一起，并不能很好的利用这两种方法的互补性。因此，为了能够结合二者的优势实现互补效果，本公开提出了一种特征点法和光度法组合的VIO方法，以便能够提升VI O算法的鲁棒性。
本公开提出了一种前端采用特征点法和光度法松耦合，后端优化采用特征点法作用和光度法紧耦合的VIO方法，该方法考虑到了特征点法和光度法对不同环境(光照变化、弱纹理)的互补作用以及这两种方法之间的内在联系，解决了目前一些类似将两种方法分开运行拼接在一起的算法的鲁棒性。该方法在一些光照变化剧烈但纹理丰富或者光照变化不大但纹理较弱的场景下有较好的稳定性，可应用在定位、导航领域。
图1为本公开实施例提供的视觉惯性里程计方法的流程示意图，本实施例提供的视觉惯性里程计方法可以由一视觉惯性里程计装置来执行，该装置可以实现为软件，或者实现为软件和硬件的组合，该装置可以集成设置在电子设备或机器人内部，通过电子设备或机器人控制***的处理器来实现。如图1所示，该方法包括如下步骤：
步骤S101：融合至少一个传感器的位姿信息得到第一初始位姿。
在步骤S101中，本公开中的传感器有相机、惯性传感器(IMU，Inertial Measurement Unit)、轮式里程计。IMU传感器在较短的时间内能够通过预积分预测较为准确的位姿(旋转信息更加准确)，轮式里程计同样能够得到位姿(位移信息更加准确)，将二者位姿信息融合得到一个较好的初始位姿。为了能够为后续的光度优化提供一个较好的初值，利用IMU预积分旋转和轮式里程计平移组合得到第一初始位姿。
具体的，融合至少一个传感器的位姿信息得到第一初始位姿，包括：利用 IMU传感器的旋转得到位姿信息中的旋转信息；利用轮式里程计的平移得到位姿信息中的位移信息；将所述旋转信息和所述位移信息进行信息融合得到第一初始位置。
步骤S102：根据两帧图像之间的位姿变换对所述初始位姿进行光度优化，获得第二初始位姿。
在步骤S102中，通过图像传感器(例如摄像头)获取图像，当获取图像信息后首先将彩色图像转为灰度图像，并提取像素梯度变化较大的点，通过光流跟踪得到在下一帧图像中的位置并构建光度误差函数，利用非线性最小二乘优化初始位姿。由于前面已经通过IMU和轮式里程计预测得到了两帧之间的位姿变换，可以估计出前面的点在后面图像中的位置，然后构建光度误差函数。
具体的，所述根据两帧图像之间的位姿变换对所述初始位姿进行光度优化，获得第二初始位姿，包括：通过图像传感器获取图像；将彩色图像转化为灰度图像；提取所述灰度图像中像素梯度变化大于一定阈值的像素点；通过光流跟踪得到在下一帧图像中的位置并构建光度误差函数；根据所述误差函数构建非线性最小二乘函数；利用所述非线性最小二乘函数优化所述第一初始位姿，获得第二初始位姿。
光度误差函数表示如下：
r＝ω h(I 2[x 2]-(a 21I 1[x 1]+b 21))        (式1)
其中，r表示光度误差，ω h为Huber权重，I 1，I 2表示相邻的两幅灰度图像，x 1，x 2分别是空间中点X在图像中的像素坐标，x 2是由x 1投影获得，a，b为光度仿射变换参数。
x 2是由x 1投影获得，需要两帧之间的相对位姿变换ξ 21和x 1在I 1中的逆深度。
非线性最小二乘函数表示如下：
Figure PCTCN2022109502-appb-000005
其中，J表示雅可比矩阵，ξ为李代数，表示旋转和平移，r表示光度误差，i、N为自然数。
步骤S103：通过提取和匹配图像中的特征点对所述第二初始位姿进行特征点优化，生成地图点。
在步骤S103中，其中，通过步骤S101和S102的光度优化后得到了较为准确的位姿，即此处的所述第二初始位置，但是为了跟踪过程更加准确再用特征点法进行优化。首先需要进行特征提取和匹配。从提取像素梯度变化较大的点中筛选出满足阈值条件的作为角点，其中，变化较大的点可以表征为像素梯度变化大于第一阈值，而进一步筛选出大于第二阈值条件的特征点作为角点，其中第二阈值大于第一阈值。此外为了能够进行匹配提取角点的描述实现数据关联。再通过提取图像中的特征点以及特征点的描述进行匹配，最后进行特征点重投影优化得到更加精确的位姿，利用此位姿生成地图点。
具体的，所述通过提取和匹配图像中的特征点对所述第二初始位姿进行特征点优化，生成地图点，包括：将彩色图像转化为灰度图像；提取所述灰度图像中像素梯度变化大于一定阈值的像素点作为所述图像的特征点；从所述图像的特征点中筛选出像素梯度变化大于第二阈值的特征点作为角点；通过所述角点的描述与所述第二初始位姿的数据进行关联匹配；根据关联匹配后的数据构建重投影误差函数；根据重投影误差函数对所述第二初始位姿进行特征点优化；根据特征点优化后的第二初始位姿和所述角点生成地图点。
尽管经过光度优化后的第二初始位姿较为准确但仍存在误差，特征匹配完成后构建重投影误差函数如图2所示，该图2示出了本公开一实施例提供的特征点法重投影误差示意图。
根据该图所示，通过和光度误差函数一样的方法构建，构建重投影误差函数，具体的数据额表达式如下：
Figure PCTCN2022109502-appb-000006
其中，ξ *为重投影误差，u i是已知的2D齐次坐标，s i表示尺度，K为相机的内部参数，ξ^是未知的相机位姿，P i为已知的3D点的齐次坐标，i、N为自然数。经过所述再次光度误差函数优化后，前端跟踪会更加准确。
步骤S104：根据所述地图点与角点的对应关系来创建后端紧耦合优化。
在步骤S104中，本公开实施例中，首先说明一下地图点的属性，具体参见图3，图3示出了本公开一实施例提供的后端优化约束因子示意图。如图3所示，图中L 1，L 2表示地图点，空心圆圈表示特征点重投影约束因子、实心圆圈表示光度约束因子。T h为主导帧(地图点的逆深度来自此帧)，T t为目标帧。从图中可以看出L 1是由角点生成的地图点因此包含两种约束：光度约束因子和投影约束因子。L 2是由普通点生成仅包含一种约束因子：光度约束因子。因此在后端优化时如果地图点是由角点生成则包含了两种约束因子，将特征点法和光度法的约束紧耦合在一起。
本实施例中，通过地图点是否由角点生成来构建后端优化问题，如果地图点是由角点生成则后端优化即包含特征点约束也包含光度约束，将二者构建的Hessian矩阵中对应的部分相加得到最终的约束矩阵。如果地图点不是由角点构成则仅包含光度约束，通过非线性最小二乘求解出待优化变量的更新量。
具体的，所述根据所述地图点与角点的对应关系来创建后端紧耦合优化，包括：通过所述地图点是否由所述角点生成来创建后端紧耦合优化；如果所述地图点是由所述角点生成，则所述后端紧耦合优化包含特征点约束和光度约束，将所述特征点约束和所述光度约束进行紧耦合；如果所述地图点不是由所述角点构成，则包含光度约束，并通过非线性最小二乘求解出待优化变量的更新量。其中，待优化变量包括相机内部参数、位姿、特征点逆深度和光度仿射变换参数。
步骤S105：通过所述后端紧耦合优化构建特征点约束矩阵和光度约束矩阵。
在步骤S105中，本公开实施例中，其中特征点法的待优化变量有：相机内部参数,位姿，路标点的逆深度。光度法的待优化变量除了特征点法中的还包含光度仿射变换参数a、b。
具体的，所述通过所述后端紧耦合优化构建特征点约束矩阵和光度约束矩阵，包括：根据光度误差函数和重投影误差函数分别获取所述特征点约束和光度约束中待变化变量之间的导数；根据所述导数分别构建特征点约束矩阵和光度约束矩阵。
前面步骤中已经说明了光度误差函数以及特征点法投影误差函数数学表达式，因此可以推导误差函数关于待优化变量之间的导数构建如图4所示的Hessian约束矩阵。如果地图点是由角点生成则后端优化即包含特征点约束也包含光度约束，将二者构建的Hessian矩阵中对应的部分相加得到最终的约束矩阵。
以特征点法为例具体的约束矩阵的内容如图5所示，p表示地图点的逆深度、C表示相机内参、x表示相机位姿。其中Hpx的转置等于Hxp，图中Hxx部分和bx部分仅以x m为host帧，x t为target帧为例填充，若host和target为其它两帧则需要在Hxx和bx两部分中把对应的编号帧累加，比如：host帧为x 1，target帧为x t则需要把这部分的约束增加到Hxx和bx对应的部分。
同样的方法，光度约束也会得到类似形式的矩阵，光度约束比特征点法约束多了两个光度仿射变换参数。
步骤S106：对所述特征点矩阵和所述光度约束矩阵进行约束矩阵的拼接，得到最终约束矩阵。
在步骤S106中，前面步骤中提到光度约束和投影约束之间差的待优化变量为光度变换参数，后端优化紧耦合时，通过地图点是否由角点生成来构建后端优化问题，如果地图点是由角点生成则后端优化即包含特征点约束也包含光度约束，则同时构建特征点约束矩阵和光度约束矩阵，将二者对应部分相加；如 果地图点不是由角点构成则仅包含光度约束，通过非线性最小二乘求解出待优化变量的更新量。如图6所示，为本公开一实施例提供的Hessian约束矩阵消元示意图，对待优化变量的更新量进行消元处理，为约束矩阵消元后得到的H和b，光度约束会得到H guang和b guang，特征点约束会得到H point和b point。
H guangx guang＝-b guang      (式4)
H pointx point＝-b point      (式5)
H finalx final＝-b final，H final＝H guang+H point,b final＝b guang+b point
                                      (式6)
注意上面的‘+’表示残差关于相同待优化变量导数部分相加。x point，x guang表示待优化变量的更新量。
具体的，所述对所述特征点矩阵和所述光度约束矩阵进行约束矩阵的拼接，得到最终约束矩阵，包括：将所述特征点约束矩阵中与所述光度约束矩阵中相同的待优化变量部分相加，获取待优化变量的更新量；对所述待优化变量的更新量进行消元处理，得到最终的约束矩阵。
此外，除了以上步骤S101至S106之外，本公开实施例中的视觉惯性里程计方法还包括：对所述特征点约束矩阵中与所述光度约束矩阵中的待优化变量进行更新；更新的具体过程为：通过所述地图点是否由角点构成来求解更新量；如果所述地图点是由所述角点生成，则所述后端紧耦合优化包含特征点约束和光度约束，如果所述地图点不是由所述角点构成，则包含光度约束；对所述待优化变量直接进行增量相加，对所述第二位姿中的旋转信息进行相乘。
图7为本公开一实施例提供的视觉惯性里程计算法的线框流程图。如图所示，结合图1的流程，首先，通过电子设备中的传感器，该图中传感器有相机、惯性传感器(IMU，Inertial Measurement Unit)、轮式里程计。其中，IMU传感器在较短的时间内能够通过预积分预测较为准确的位姿(旋转信息更加准确)，轮式里程计同样能够得到位姿(位移信息更加准确)，将二者位姿信息融 合得到一个较好的初始位姿。从而为了能够为后续的光度优化提供一个较好的初值，利用IMU预积分旋转和轮式里程计平移组合得到第一初始位姿。图像传感器(例如相机)用于采集图像，根据采集的图像中两帧之间的位姿变换对得到的第一初始位姿进行光度优化，将获取的彩色图像转为灰度图像，并提取像素梯度变化较大的点，通过光流跟踪得到在下一帧图像中的位置并构建光度误差函数，利用非线性最小二乘优化初始位姿，得到经过优化的第二初始位姿，根据两帧之间的位姿变换，可以估计出前面的点在后面图像中的位置，然后构建光度误差函数。然后对所述第二初始位姿进行特征提取和匹配，从提取像素梯度变化较大的点中筛选出满足阈值条件的作为角点，其中，变化较大的点可以表征为像素梯度变化大于第一阈值，而进一步筛选出大于第二阈值条件的特征点作为角点，其中第二阈值大于第一阈值。再通过提取图像中的特征点以及特征点的描述进行匹配，进行特征点重投影优化得到更加精确的位姿，利用此位姿生成地图点。
生成地图点后，根据所述地图点与角点的对应关系来创建后端紧耦合优化，通过地图点是否由角点生成来构建后端优化问题，如果地图点是由角点生成则后端优化即包含特征点约束也包含光度约束，则同时构建特征点约束矩阵和光度约束矩阵，将二者对应部分相加；如果地图点不是由角点构成则仅包含光度约束，通过非线性最小二乘求解出待优化变量的更新量。矩阵中误差项关于相同待优化变量部分相加，得到最终的约束矩阵。得到最终的约束矩阵后，迭代欧化求解待优化变量的增量，从而将待优化变量进行更新。
图8示出了本公开另一实施例提供的视觉惯性里程计装置示意图。该装置包括：融合模块801、光度优化模块802、特征点优化模块803、紧耦合优化模块804、矩阵构建模块805和矩阵拼接模块806。其中：
所述融合模块801，用于融合至少一个传感器的位姿信息得到第一初始位姿。该模块具体用于：利用IMU传感器的旋转得到位姿信息中的旋转信息；利用轮式里程计的平移得到位姿信息中的位移信息；将所述旋转信息和所述位移信息进行信息融合得到第一初始位置。为了能够为后续的光度优化提供一个较 好的初值，利用IMU预积分旋转和轮式里程计平移组合得到第一初始位姿。
所述光度优化模块802，用于根据两帧图像之间的位姿变换对所述初始位姿进行光度优化，获得第二初始位姿。
该模块，具体用于：通过图像传感器获取图像；将彩色图像转化为灰度图像；提取所述灰度图像中像素梯度变化大于一定阈值的像素点；通过光流跟踪得到在下一帧图像中的位置并构建光度误差函数；根据所述误差函数构建非线性最小二乘函数；利用所述非线性最小二乘函数优化所述第一初始位姿，获得第二初始位姿。
光度误差函数表示如下：
r＝ω h(I 2[x 2]-(a 21I 1[x 1]+b 21))        (式1)
其中，r表示光度误差，ω h为Huber权重，I 1，I 2表示相邻的两幅灰度图像，x 1，x 2分别是空间中点X在图像中的像素坐标，x 2是由x 1投影获得，a，b为光度仿射变换参数。
x 2是由x 1投影获得，需要两帧之间的相对位姿变换ξ 21和x 1在I 1中的逆深度。
非线性最小二乘函数表示如下：
Figure PCTCN2022109502-appb-000007
其中，J表示雅可比矩阵，ξ为李代数，表示旋转和平移，r表示光度误差，i、N为自然数。
所述特征点优化模块803，用于通过提取和匹配图像中的特征点对所述第二初始位姿进行特征点优化，生成地图点。
光度优化后得到了较为准确的位姿，即此处的所述第二初始位置，首先需要进行特征提取和匹配，从提取像素梯度变化较大的点中筛选出满足阈值条件的作为角点，其中，变化较大的点可以表征为像素梯度变化大于第一阈值，而 进一步筛选出大于第二阈值条件的特征点作为角点，其中第二阈值大于第一阈值。此外为了能够进行匹配提取角点的描述实现数据关联。再通过提取图像中的特征点以及特征点的描述进行匹配，最后进行特征点重投影优化得到更加精确的位姿，利用此位姿生成地图点。。
所述获取模块，具体用于：将彩色图像转化为灰度图像；提取所述灰度图像中像素梯度变化大于一定阈值的像素点作为所述图像的特征点；从所述图像的特征点中筛选出像素梯度变化大于第二阈值的特征点作为角点；通过所述角点的描述与所述第二初始位姿的数据进行关联匹配；根据关联匹配后的数据构建重投影误差函数；根据重投影误差函数对所述第二初始位姿进行特征点优化；根据特征点优化后的第二初始位姿和所述角点生成地图点。
所述紧耦合优化模块804，用于根据所述地图点与角点的对应关系来创建后端紧耦合优化。
创建后端紧耦合优化模块，通过地图点是否由角点生成来构建后端优化问题，如果地图点是由角点生成则后端优化即包含特征点约束也包含光度约束，将二者构建的Hessian矩阵中对应的部分相加得到最终的约束矩阵。如果地图点不是由角点构成则仅包含光度约束，通过非线性最小二乘求解出待优化变量的更新量。
该模块具体用于：所述根据所述地图点与角点的对应关系来创建后端紧耦合优化，包括：通过所述地图点是否由所述角点生成来创建后端紧耦合优化；如果所述地图点是由所述角点生成，则所述后端紧耦合优化包含特征点约束和光度约束，将所述特征点约束和所述光度约束进行紧耦合；如果所述地图点不是由所述角点构成，则包含光度约束，并通过非线性最小二乘求解出待优化变量的更新量。其中，待优化变量包括相机内部参数、位姿、特征点逆深度和光度仿射变换参数。
所述矩阵构建模块805，用于通过所述后端紧耦合优化构建特征点约束矩阵和光度约束矩阵。
该模块具体用于：根据光度误差函数和重投影误差函数分别获取所述特征 点约束和光度约束中待变化变量之间的导数；根据所述导数分别构建特征点约束矩阵和光度约束矩阵。
前面已经说明了光度误差函数以及特征点法投影误差函数数学表达式，因此可以推导误差函数关于待优化变量之间的导数构建如图4所示的Hessian约束矩阵。如果地图点是由角点生成则后端优化即包含特征点约束也包含光度约束，将二者构建的Hessian矩阵中对应的部分相加得到最终的约束矩阵。
所述矩阵拼接模块806，用于对所述特征点矩阵和所述光度约束矩阵进行约束矩阵的拼接，得到最终约束矩阵。
该模块具体用于：通过地图点是否由角点生成来构建后端优化问题，如果地图点是由角点生成则后端优化即包含特征点约束也包含光度约束，则同时构建特征点约束矩阵和光度约束矩阵，将二者对应部分相加；如果地图点不是由角点构成则仅包含光度约束，通过非线性最小二乘求解出待优化变量的更新量。将所述特征点约束矩阵中与所述光度约束矩阵中相同的待优化变量部分相加，获取待优化变量的更新量；对所述待优化变量的更新量进行消元处理，得到最终的约束矩阵。
另外，该视觉惯性里程计装置还包括：
更新模块，用于对所述特征点约束矩阵中与所述光度约束矩阵中的待优化变量进行更新。
该更新模块具体用于：通过所述地图点是否由角点构成来求解更新量；如果所述地图点是由所述角点生成，则所述后端紧耦合优化包含特征点约束和光度约束，如果所述地图点不是由所述角点构成，则包含光度约束；对所述待优化变量直接进行增量相加，对所述第二位姿中的旋转信息进行相乘。
图8所示装置可以执行图1所示实施例的方法，本实施例未详细描述的部分，可参考对图1所示实施例的相关说明。该技术方案的执行过程和技术效果参见图1所示实施例中的描述，在此不再赘述。
下面参考图9，其示出了适于用来实现本公开另一实施例的电子设备900的结构示意图。本公开实施例中的终端设备可以包括但不限于诸如移动电话、笔 记本电脑、数字广播接收器、PDA(个人数字助理)、PAD(平板电脑)、PMP(便携式多媒体播放器)、车载终端(例如车载导航终端)等等的移动终端以及诸如数字TV、台式计算机等等的固定终端。图9示出的电子设备仅仅是一个示例，不应对本公开实施例的功能和使用范围带来任何限制。
如图9所示，电子设备900可以包括处理装置(例如中央处理器、图形处理器等)901，其可以根据存储在只读存储器(ROM)902中的程序或者从存储装置908加载到随机访问存储器(RAM)903中的程序而执行各种适当的动作和处理。在RAM 903中，还存储有电子设备900操作所需的各种程序和数据。处理装置901、ROM 902以及RAM 903通过通信线路904彼此相连。输入/输出(I/O)接口905也连接至通信线路904。
通常，以下装置可以连接至I/O接口905：包括例如触摸屏、触摸板、键盘、鼠标、摄像头、麦克风、加速度计、陀螺仪等的输入装置906；包括例如液晶显示器(LCD)、扬声器、振动器等的输出装置907；包括例如磁带、硬盘等的存储装置908；以及通信装置909。通信装置909可以允许电子设备900与其他设备进行无线或有线通信以交换数据。虽然图9示出了具有各种装置的电子设备900，但是应理解的是，并不要求实施或具备所有示出的装置。可以替代地实施或具备更多或更少的装置。
特别地，根据本公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本公开的实施例包括一种计算机程序产品，其包括承载在非暂态计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信装置909从网络上被下载和安装，或者从存储装置908被安装，或者从ROM 902被安装。在该计算机程序被处理装置901执行时，执行本公开实施例的方法中限定的上述功能。
需要说明的是，本公开上述的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的***、装 置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本公开中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行***、装置或者器件使用或者与其结合使用。而在本公开中，计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读信号介质可以发送、传播或者传输用于由指令执行***、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：电线、光缆、RF(射频)等等，或者上述的任意合适的组合。
在一些实施方式中，客户端、服务器可以利用诸如HTTP(HyperText Transfer Protocol，超文本传输协议)之类的任何当前已知或未来研发的网络协议进行通信，并且可以与任意形式或介质的数字数据通信(例如，通信网络)互连。通信网络的示例包括局域网(“LAN”)，广域网(“WAN”)，网际网(例如，互联网)以及端对端网络(例如，ad hoc端对端网络)，以及任何当前已知或未来研发的网络。
上述计算机可读介质可以是上述电子设备中所包含的；也可以是单独存在，而未装配入该电子设备中。
上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被该电子设备执行时，使得该电子设备：执行上述实施例中的交互方法。
可以以一种或多种程序设计语言或其组合来编写用于执行本公开的操作的计算机程序代码，上述程序设计语言包括但不限于面向对象的程序设计语言—诸如Java、Smalltalk、C++，还包括常规的过程式程序设计语言—诸如“C” 语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)—连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。
附图中的流程图和框图，图示了按照本公开各种实施例的***、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，该模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的***来实现，或者可以用专用硬件与计算机指令的组合来实现。
描述于本公开实施例中所涉及到的单元可以通过软件的方式实现，也可以通过硬件的方式来实现。其中，单元的名称在某种情况下并不构成对该单元本身的限定。
本文中以上描述的功能可以至少部分地由一个或多个硬件逻辑部件来执行。例如，非限制性地，可以使用的示范类型的硬件逻辑部件包括：现场可编程门阵列(FPGA)、专用集成电路(ASIC)、专用标准产品(ASSP)、片上***(SOC)、复杂可编程逻辑设备(CPLD)等等。
在本公开的上下文中，机器可读介质可以是有形的介质，其可以包含或存储以供指令执行***、装置或设备使用或与指令执行***、装置或设备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或 半导体***、装置或设备，或者上述内容的任何合适组合。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
根据本公开的一个或多个实施例，提供了一种电子设备，包括：至少一个处理器；以及，与所述至少一个处理器通信连接的存储器；其中，所述存储器存储有能被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行前述第一方面中的任一所述方法。
根据本公开的一个或多个实施例，提供了一种非暂态计算机可读存储介质，其特征在于，该非暂态计算机可读存储介质存储计算机指令，该计算机指令用于使计算机执行前述第一方面中的任一所述方法。
以上描述仅为本公开的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本公开中所涉及的公开范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离上述公开构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本公开中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。

Claims (27)

  1. 一种视觉惯性里程计方法，其特征在于，包括：
    融合至少一个传感器的位姿信息得到第一初始位姿；
    根据两帧图像之间的位姿变换对所述初始位姿进行光度优化，获得第二初始位姿；
    通过提取和匹配图像中的特征点对所述第二初始位姿进行特征点优化，生成地图点；
    根据所述地图点与角点的对应关系来创建后端紧耦合优化；
    通过所述后端紧耦合优化构建特征点约束矩阵和光度约束矩阵；
    对所述特征点矩阵和所述光度约束矩阵进行约束矩阵的拼接，得到最终约束矩阵。
  2. 根据权利要求1所述的方法，其特征在于，所述融合至少一个传感器的位姿信息得到第一初始位姿，包括：
    利用惯性传感器的旋转得到位姿信息中的旋转信息；
    利用轮式里程计的平移得到位姿信息中的位移信息；
    将所述旋转信息和所述位移信息进行信息融合得到第一初始位置。
  3. 根据权利要求1所述的方法，其特征在于，所述根据两帧图像之间的位姿变换对所述初始位姿进行光度优化，获得第二初始位姿，包括：
    通过图像传感器获取图像；
    将彩色图像转化为灰度图像；
    提取所述灰度图像中像素梯度变化大于一定阈值的像素点；
    通过光流跟踪得到在下一帧图像中的位置并构建光度误差函数；
    根据所述误差函数构建非线性最小二乘函数；
    利用所述非线性最小二乘函数优化所述第一初始位姿，获得第二初始位姿。
  4. 根据权利要求3所述的方法，其特征在于，所述光度误差函数表示如下：
    r＝ω h(I 2[x 2]-(a 21I 1[x 1]+b 21))
    其中，r表示光度误差，ω h为Huber权重，I 1，I 2表示相邻的两幅灰度图像，x 1，x 2分别是空间中点X在图像中的像素坐标，x 2是由x 1投影获得，a，b为光度仿射变换参数；
    x 2是由x 1投影获得，需要两帧之间的相对位姿变换ξ 21和x 1在I 1中的逆深度。
  5. 根据权利要求3所述的方法，其特征在于，所述非线性最小二乘函数表示如下：
    Figure PCTCN2022109502-appb-100001
    其中，J表示雅可比矩阵，ξ为李代数，表示旋转和平移，r表示光度误差，i、N为自然数。
  6. 根据权利要求1所述的方法，其特征在于，所述通过提取和匹配图像中的特征点对所述第二初始位姿进行特征点优化，生成地图点，包括：
    将彩色图像转化为灰度图像；
    提取所述灰度图像中像素梯度变化大于一定阈值的像素点作为所述图像的特征点；
    从所述图像的特征点中筛选出像素梯度变化大于第二阈值的特征点作为角点；
    通过所述角点的描述与所述第二初始位姿的数据进行关联匹配；
    根据关联匹配后的数据构建重投影误差函数；
    根据重投影误差函数对所述第二初始位姿进行特征点优化；
    根据特征点优化后的第二初始位姿和所述角点生成地图点。
  7. 根据权利要求6所述的方法，其特征在于，所述重投影误差函数表示如下：
    Figure PCTCN2022109502-appb-100002
    其中，ξ *为重投影误差，u i是已知的2D齐次坐标，s i表示尺度，K为相机的内部参数，ξ^是未知的相机位姿，P i为已知的3D点的齐次坐标，i、N为自然数。
  8. 根据权利要求1所述的方法，其特征在于，所述根据所述地图点与角点的对应关系来创建后端紧耦合优化，包括：
    通过所述地图点是否由所述角点生成来创建后端紧耦合优化；
    如果所述地图点是由所述角点生成，则所述后端紧耦合优化包含特征点约束和光度约束，将所述特征点约束和所述光度约束进行紧耦合；
    如果所述地图点不是由所述角点构成，则包含光度约束，并通过非线性最小二乘求解出待优化变量的更新量。
  9. 根据权利要求1所述的方法，其特征在于，所述通过所述后端紧耦合优化构建特征点约束矩阵和光度约束矩阵，包括：
    根据光度误差函数和重投影误差函数分别获取所述特征点约束和光度约束中待变化变量之间的导数；
    根据所述导数分别构建特征点约束矩阵和光度约束矩阵。
  10. 根据权利要求1所述的方法，其特征在于，所述对所述特征点矩阵和所述光度约束矩阵进行约束矩阵的拼接，得到最终约束矩阵，包括：
    将所述特征点约束矩阵中与所述光度约束矩阵中相同的待优化变量部分相加，获取待优化变量的更新量；
    对所述待优化变量的更新量进行消元处理，得到最终的约束矩阵。
  11. 根据权利要求1所述的方法，所述方法还包括：
    对所述特征点约束矩阵中与所述光度约束矩阵中的待优化变量进行更新；包括：
    通过所述地图点是否由角点构成来求解更新量；
    如果所述地图点是由所述角点生成，则所述后端紧耦合优化包含特征点约束和光度约束，如果所述地图点不是由所述角点构成，则包含光度约束；
    对所述待优化变量直接进行增量相加，对所述第二位姿中的旋转信息进行相乘。
  12. 根据权利要求8至11其中之一所述的方法，其特征在于，所述待优化变量包括相机内部参数、位姿、特征点逆深度和光度仿射变换参数。
  13. 一种视觉惯性里程计装置，其特征在于，包括：
    融合模块，用于融合至少一个传感器的位姿信息得到第一初始位姿；
    光度优化模块，用于根据两帧图像之间的位姿变换对所述初始位姿进行光度优化，获得第二初始位姿；
    特征点优化模块，用于通过提取和匹配图像中的特征点对所述第二初始位姿进行特征点优化，生成地图点；
    紧耦合优化模块，用于根据所述地图点与角点的对应关系来创建后端紧耦合优化；
    矩阵构建模块，用于通过所述后端紧耦合优化构建特征点约束矩阵和光度约束矩阵；
    矩阵拼接模块，用于对所述特征点矩阵和所述光度约束矩阵进行约束矩阵的拼接，得到最终约束矩阵。
  14. 根据权利要求13所述的装置，其特征在于，所述融合模块，用于包括：
    利用惯性传感器的旋转得到位姿信息中的旋转信息；
    利用轮式里程计的平移得到位姿信息中的位移信息；
    将所述旋转信息和所述位移信息进行信息融合得到第一初始位置。
  15. 根据权利要求13所述的装置，其特征在于，所述光度优化模块，用于包括：
    通过图像传感器获取图像；
    将彩色图像转化为灰度图像；
    提取所述灰度图像中像素梯度变化大于一定阈值的像素点；
    通过光流跟踪得到在下一帧图像中的位置并构建光度误差函数；
    根据所述误差函数构建非线性最小二乘函数；
    利用所述非线性最小二乘函数优化所述第一初始位姿，获得第二初始位姿。
  16. 根据权利要求15所述的装置，其特征在于，所述光度误差函数表示如下：
    r＝ω h(I 2[x 2]-(a 21I 1[x 1]+b 21))
    其中，r表示光度误差，ω h为Huber权重，I 1，I 2表示相邻的两幅灰度图像，
    x 1，x 2分别是空间中点X在图像中的像素坐标，x 2是由x 1投影获得，a，b为光度仿射变换参数；
    x 2是由x 1投影获得，需要两帧之间的相对位姿变换ξ 21和x 1在I 1中的逆深度。
  17. 根据权利要求15所述的装置，其特征在于，所述非线性最小二乘函数表示如下：
    Figure PCTCN2022109502-appb-100003
    其中，J表示雅可比矩阵，ξ为李代数，表示旋转和平移，r表示光度误差，i、N为自然数。
  18. 根据权利要求13所述的装置，其特征在于，所述特征点优化模块，用于包括：
    将彩色图像转化为灰度图像；
    提取所述灰度图像中像素梯度变化大于一定阈值的像素点作为所述图像的特征点；
    从所述图像的特征点中筛选出像素梯度变化大于第二阈值的特征点作为角点；
    通过所述角点的描述与所述第二初始位姿的数据进行关联匹配；
    根据关联匹配后的数据构建重投影误差函数；
    根据重投影误差函数对所述第二初始位姿进行特征点优化；
    根据特征点优化后的第二初始位姿和所述角点生成地图点。
  19. 根据权利要求18所述的装置，其特征在于，所述重投影误差函数表示如下：
    Figure PCTCN2022109502-appb-100004
    其中，ξ *为重投影误差，u i是已知的2D齐次坐标，s i表示尺度，K为相机的内部参数，ξ^是未知的相机位姿，P i为已知的3D点的齐次坐标，i、N为自然数。
  20. 根据权利要求13所述的装置，其特征在于，所述紧耦合优化模块，用于包括：
    通过所述地图点是否由所述角点生成来创建后端紧耦合优化；
    如果所述地图点是由所述角点生成，则所述后端紧耦合优化包含特征点约束和光度约束，将所述特征点约束和所述光度约束进行紧耦合；
    如果所述地图点不是由所述角点构成，则包含光度约束，并通过非线性最小二乘求解出待优化变量的更新量。
  21. 根据权利要求13所述的装置，其特征在于，所述矩阵构建模块，用于包括：
    根据光度误差函数和重投影误差函数分别获取所述特征点约束和光度约束中待变化变量之间的导数；
    根据所述导数分别构建特征点约束矩阵和光度约束矩阵。
  22. 根据权利要求13所述的装置，其特征在于，所述矩阵拼接模块，用于包括：
    将所述特征点约束矩阵中与所述光度约束矩阵中相同的待优化变量部分相加，获取待优化变量的更新量；
    对所述待优化变量的更新量进行消元处理，得到最终的约束矩阵。
  23. 根据权利要求13所述的装置，所述装置还包括：
    更新模块，用于对所述特征点约束矩阵中与所述光度约束矩阵中的待优化变量进行更新；
    所述更新模块，用于包括：
    通过所述地图点是否由角点构成来求解更新量；
    如果所述地图点是由所述角点生成，则所述后端紧耦合优化包含特征点约束和光度约束，如果所述地图点不是由所述角点构成，则包含光度约束；
    对所述待优化变量直接进行增量相加，对所述第二位姿中的旋转信息进行相乘。
  24. 根据权利要求20至23其中之一所述的装置，其特征在于，所述待优化变量包括相机内部参数、位姿、特征点逆深度和光度仿射变换参数。
  25. 一种电子设备，包括：
    存储器，用于存储计算机可读指令；以及
    处理器，用于运行所述计算机可读指令，使得所述电子设备实现根据权利要求1-12中任意一项所述的方法。
  26. 一种计算机存储介质，所述存储介质中存储有至少一可执行指令，所述可执行指令使处理器执行根据权利要求1-12任一项所述视觉惯性里程计方法的步骤。
  27. 一种计算机程序，包括指令，当其在计算机上运行时，使得计算机执行根据权利要求1-12任一项所述的视觉惯性里程计方法。
PCT/CN2022/109502 2021-09-30 2022-08-01 视觉惯性里程计方法、装置、电子设备、存储介质及计算机程序 WO2023051019A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202111162776.XA CN115371699B (zh) 2021-09-30 2021-09-30 视觉惯性里程计方法、装置及电子设备
CN202111162776.X 2021-09-30

Publications (1)

Publication Number Publication Date
WO2023051019A1 true WO2023051019A1 (zh) 2023-04-06

Family

ID=84060740

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2022/109502 WO2023051019A1 (zh) 2021-09-30 2022-08-01 视觉惯性里程计方法、装置、电子设备、存储介质及计算机程序

Country Status (2)

Country Link
CN (1) CN115371699B (zh)
WO (1) WO2023051019A1 (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116188536A (zh) * 2023-04-23 2023-05-30 深圳时识科技有限公司 视觉惯性里程计方法与装置、电子设备
CN116389887A (zh) * 2023-04-07 2023-07-04 北京拙河科技有限公司 一种基于动态优化的光场相机配置方法及装置
CN116883938A (zh) * 2023-07-05 2023-10-13 禾多科技（北京）有限公司 行人速度信息生成方法、装置、设备和计算机可读介质
CN117474180A (zh) * 2023-12-28 2024-01-30 深圳市中远通电源技术开发有限公司 一种基于配电柜调节的区域供电优化***、方法及介质

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109166149A (zh) * 2018-08-13 2019-01-08 武汉大学 一种融合双目相机与imu的定位与三维线框结构重建方法与***
US20190251696A1 (en) * 2018-02-12 2019-08-15 Samsung Electronics Co., Ltd. Device and method with pose estimator
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
CN110375738A (zh) * 2019-06-21 2019-10-25 西安电子科技大学 一种融合惯性测量单元的单目同步定位与建图位姿解算方法
CN111983639A (zh) * 2020-08-25 2020-11-24 浙江光珀智能科技有限公司 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法
CN112347840A (zh) * 2020-08-25 2021-02-09 天津大学 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN112435325A (zh) * 2020-09-29 2021-03-02 北京航空航天大学 基于vi-slam和深度估计网络的无人机场景稠密重建方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109387204B (zh) * 2018-09-26 2020-08-28 东北大学 面向室内动态环境的移动机器人同步定位与构图方法
CN109544636B (zh) * 2018-10-10 2022-03-15 广州大学 一种融合特征点法和直接法的快速单目视觉里程计导航定位方法
CN109523589B (zh) * 2018-11-13 2021-06-08 浙江工业大学 一种更鲁棒的视觉里程计的设计方法
CN112419497A (zh) * 2020-11-13 2021-02-26 天津大学 基于单目视觉的特征法与直接法相融合的slam方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190251696A1 (en) * 2018-02-12 2019-08-15 Samsung Electronics Co., Ltd. Device and method with pose estimator
CN109166149A (zh) * 2018-08-13 2019-01-08 武汉大学 一种融合双目相机与imu的定位与三维线框结构重建方法与***
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
CN110375738A (zh) * 2019-06-21 2019-10-25 西安电子科技大学 一种融合惯性测量单元的单目同步定位与建图位姿解算方法
CN111983639A (zh) * 2020-08-25 2020-11-24 浙江光珀智能科技有限公司 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法
CN112347840A (zh) * 2020-08-25 2021-02-09 天津大学 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN112435325A (zh) * 2020-09-29 2021-03-02 北京航空航天大学 基于vi-slam和深度估计网络的无人机场景稠密重建方法

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116389887A (zh) * 2023-04-07 2023-07-04 北京拙河科技有限公司 一种基于动态优化的光场相机配置方法及装置
CN116188536A (zh) * 2023-04-23 2023-05-30 深圳时识科技有限公司 视觉惯性里程计方法与装置、电子设备
CN116188536B (zh) * 2023-04-23 2023-07-18 深圳时识科技有限公司 视觉惯性里程计方法与装置、电子设备
CN116883938A (zh) * 2023-07-05 2023-10-13 禾多科技（北京）有限公司 行人速度信息生成方法、装置、设备和计算机可读介质
CN117474180A (zh) * 2023-12-28 2024-01-30 深圳市中远通电源技术开发有限公司 一种基于配电柜调节的区域供电优化***、方法及介质

Also Published As

Publication number Publication date
CN115371699B (zh) 2024-03-15
CN115371699A (zh) 2022-11-22

Similar Documents

Publication Publication Date Title
WO2023051019A1 (zh) 视觉惯性里程计方法、装置、电子设备、存储介质及计算机程序
CN115265523B (zh) 机器人同时定位与建图方法、装置及可读介质
CN110073313B (zh) 使用母设备和至少一个伴随设备与环境交互
US11313684B2 (en) Collaborative navigation and mapping
WO2023165093A1 (zh) 视觉惯性里程计模型的训练方法、位姿估计方法、装置、电子设备、计算机可读存储介质及程序产品
CN112150490B (zh) 图像检测方法、装置、电子设备和计算机可读介质
CN111292420A (zh) 用于构建地图的方法和装置
WO2023029893A1 (zh) 纹理映射方法、装置、设备及存储介质
JP7369847B2 (ja) 自動運転車両に対するデータ処理方法及び装置、電子機器、記憶媒体、コンピュータプログラム、ならびに自動運転車両
CN110660098A (zh) 基于单目视觉的定位方法和装置
CN115534925B (zh) 车辆控制方法、装置、设备和计算机可读介质
CN115585818A (zh) 一种地图构建方法、装置、电子设备及存储介质
CN117232499A (zh) 多传感器融合的点云地图构建方法、装置、设备及介质
Yu et al. Tightly-coupled fusion of VINS and motion constraint for autonomous vehicle
Cao et al. Fast incremental structure from motion based on parallel bundle adjustment
Hua et al. M2C-GVIO: motion manifold constraint aided GNSS-visual-inertial odometry for ground vehicles
CN112150491B (zh) 图像检测方法、装置、电子设备和计算机可读介质
CN115900697B (zh) 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆
CN115330851A (zh) 单目深度估计方法、装置、电子设备、存储介质及车辆
CN112880675B (zh) 用于视觉定位的位姿平滑方法、装置、终端和移动机器人
CN114119973A (zh) 一种基于图像语义分割网络的空间距离预测方法及***
CN114494379A (zh) 基于fpfh的凸包辅助三维点云配准方法、设备及介质
Lee et al. Confidence analysis of feature points for visual‐inertial odometry of urban vehicles
CN113870271A (zh) 3d点云的压缩方法、装置、设备及存储介质
Zhao et al. Research on Visual-Inertial SLAM Technology with GNSS Assistance

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 22874415

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE