Visual odometer initialization method based on point cloud multilayer sampling

Abstract

A visual odometer initialization method based on point cloud multilayer sampling is suitable for an unmanned system of unmanned aerial vehicles (UAV) and unmanned ground vehicles (UGV) cooperative tasks, and comprises the following main steps: firstly, performing feature point matching in multiple frames of images by using a KLT algorithm, and constructing a sliding window; secondly, through 2D-2D feature association and a PnP algorithm, constructing a scale-free SFM point cloud based on the feature point data; then, IMU data and the SFM point cloud are fused, and the pose and the scale are calculated through pre-integration; thirdly, performing vision-inertia alignment by applying a multi-level sampling method to obtain a coarse registration pose; and finally, through a BA optimization structure, carrying out fine adjustment to obtain an accurate pose. The method can effectively improve the pose estimation precision and efficiency in an unknown environment, and has an important application value.

Description

Technical Field

[0001] The present invention relates to automated navigation and positioning technology, and in particular to a visual odometer initialization method based on point cloud multi-layer sampling. Background Art

[0002] In modern unmanned aerial vehicles (UAVs) and unmanned ground vehicles (UGVs) collaborative missions, accurate relative positioning has always been a critical and challenging task. With the development of automation technology, unmanned systems are increasingly used in agriculture, military, rescue, logistics and other fields. These systems need to know their position and posture in real time and accurately when performing tasks to ensure the smooth completion of the mission and the safe operation of the system.

[0003] Visual-Inertial Odometry (VIO) systems have become the preferred positioning technology for many unmanned systems due to their low cost and lightweight advantages. VIO systems calculate the three-dimensional position and posture of devices in real time by fusing data from visual sensors and inertial measurement units (IMUs). Visual sensors provide rich environmental information, while IMUs provide high-frequency motion data. The combination of the two can overcome the limitations of a single sensor and improve the accuracy and robustness of positioning.

[0004] However, VIO systems face some challenges in practical applications. First, VIO systems require accurate initial pose information during the initialization process, which is the basis for ensuring subsequent positioning accuracy. Traditional VIO systems usually rely on prior maps or manually provided initial values ​​for initialization, which has great limitations in practical applications. The acquisition and maintenance of prior maps requires a lot of manpower and material resources, while manually providing initial values ​​requires operators to have high professional skills, which is difficult to achieve in complex environments and dynamic scenes.

[0005] Secondly, the application of VIO systems in unknown environments also faces many uncertainties. The complexity and dynamic changes of the environment will affect the performance of the visual sensor. For example, changes in lighting, occlusions, and dynamic objects will lead to the loss or mismatch of visual feature points, which will affect the positioning accuracy of the system. At the same time, the noise and drift in the IMU data will also accumulate, resulting in long-term positioning drift of the system.

[0006] In addition, existing VIO systems also have certain limitations when dealing with scale estimation issues. The image data provided by the visual sensor is scale-free and cannot directly reflect the actual physical size. Although the IMU data can provide acceleration information of the movement, it cannot accurately estimate the actual movement scale of the system when the initial scale is unknown. This results in the VIO system requiring additional steps to determine the actual scale information during the initialization process, which increases the complexity and computational burden of the system.

[0007] A traditional VIO system relies on a priori maps or manually provided initial values ​​for initialization. These methods usually require a lot of manpower and material resources to obtain and maintain the prior map, or require the operator to have high professional skills and manually provide the initial pose. These methods are difficult to implement in complex environments and dynamic scenes, and limit the applicability and robustness of the system. The need for prior maps or manually providing initial values ​​increases the complexity and difficulty of operation of the system. It is difficult to implement in unknown environments and dynamic scenes, which limits the applicability and robustness of the system. The professional skills required of the operator are high, which increases the threshold for using the system.

[0008] Another related prior art is the perspective n-point (PnP) algorithm, which is used to estimate the pose of a 3D object from 2D image points. The PnP algorithm is widely used in computer vision, especially in the fields of augmented reality (AR) and robot positioning. The basic idea is to solve the pose of the camera through the correspondence between the known 3D points and their 2D projection points in the image. In the specific implementation process, the PnP algorithm requires some known 3D point coordinates and their 2D projection points in the image, and uses these correspondences to estimate the pose of the camera by minimizing the projection error. Common PnP algorithms include EPnP (Efficient PnP), UPnP (Unified PnP) and RPnP (Robust PnP), which differ in computational efficiency and robustness. In the visual inertial odometry (VIO) system, the PnP algorithm is often used in the initialization stage to estimate the initial pose of the camera through the correspondence between the feature points extracted from the image and the known 3D points. This provides an accurate initial value for the subsequent fusion of visual and inertial data. The PnP algorithm can only estimate the pose of the camera, but cannot directly solve the scale estimation problem. In the VIO system, although the IMU can provide acceleration information of the motion, it cannot accurately estimate the actual motion scale of the system when the initial scale is unknown. Additional steps are required to determine the actual scale information, which increases the complexity and computational burden of the system.

[0009] It should be noted that the information disclosed in the above background technology section is only used for understanding the background of the present application, and therefore may include information that does not constitute prior art known to ordinary technicians in the field. Summary of the invention

[0010] The main purpose of the present invention is to solve the problems existing in the above-mentioned background technology and provide a visual odometer initialization method based on point cloud multi-layer sampling.

[0011] To achieve the above object, the present invention adopts the following technical solutions:

[0012] In a first aspect of the present invention, a visual odometer initialization method based on point cloud multi-layer sampling is applicable to an unmanned system for collaborative tasks of an unmanned aerial vehicle (UAV) and an unmanned ground vehicle (UGV), comprising the following steps:

[0013] Step 1: Use the KLT sparse optical flow algorithm to match feature points between multiple consecutive frames of images and build a sliding window to obtain stable and continuous feature point data;

[0014] Step 2: Using the feature point data obtained in step 1, a scale-free SFM point cloud is constructed from the image sequence through 2D-2D feature association, PnP algorithm and triangulation method to provide a geometric basis for pose estimation;

[0015] Step 3: Combine the motion information of the IMU and the SFM point cloud obtained in step 2, calculate the displacement and rotation information between adjacent frames through the pre-integration method, and then estimate the actual scale of the SFM point cloud, the speed of the system, and the direction of gravity;

[0016] Step 4: Based on step 3, a multi-level sampling method is used for visual-inertial alignment, including first-level sampling and second-level sampling, to achieve rapid convergence of the initial camera pose and generate a coarse registration pose estimate;

[0017] Step 5: Using the coarse registration result obtained in step 4, construct a point-assisted BA optimization structure, and perform fine optimization by minimizing the visual reprojection error, IMU pre-integration error, and point-surface distance error to obtain accurate relative pose estimation.

[0018] In a second aspect of the present invention, a computer program product includes a computer program, characterized in that when the computer program is executed by a processor, the visual odometer initialization method based on point cloud multi-layer sampling is implemented.

[0019] The present invention has the following beneficial effects:

[0020] The present invention proposes a method combining reconstruction from motion (SFM) and multi-layer sampling, providing an efficient and accurate initialization scheme, solving the problem of the lack of accurate initial values ​​in the initialization process of the existing VIO system, and improving the applicability and robustness of the system in unknown environments. The method proposed in the present invention combines reconstruction from motion (Structure from Motion, SFM) and multi-layer sampling, and gradually refines the parameter space through multiple sampling and iterative optimization, thereby realizing an efficient and accurate initialization scheme. The SFM technology reconstructs the three-dimensional structure by analyzing the moving objects in the image sequence, while the multi-layer sampling method quickly converges to the optimal initial pose through multiple sampling and iteration. The combination of the two can not only improve the accuracy and efficiency of initialization, but also achieve robust positioning in unknown environments.

[0021] The initialization scheme combining multi-layer sampling and SFM proposed in the present invention significantly improves the initialization accuracy and efficiency of the VIO system in an unknown environment, and has the following significant advantages:

[0022] 1. High-precision initial pose estimation: Through the multi-layer sampling method, it quickly converges to the optimal initial camera pose, ensuring the high accuracy and robustness of the initial pose. Combining SFM point cloud and IMU data, it solves the scale estimation error and trajectory drift problems existing in traditional methods.

[0023] 2. Efficient initialization process: The multi-layer sampling method significantly reduces the sampling space, improves sampling efficiency, and shortens the initialization time. The collaborative optimization framework combines visual reprojection error, IMU pre-integration error, and point-surface distance error to quickly obtain accurate relative pose estimation by minimizing these error functions.

[0024] 3. Strong robustness: It does not rely on prior maps or manually provided initial values, which enhances the applicability of the system in unknown environments. A new collaborative optimization framework is proposed to effectively solve the drift problem existing in existing methods.

[0025] Through the implementation of the present invention, the initial pose estimation accuracy and efficiency of the VIO system in an unknown environment can be significantly improved, and the present invention has broad application prospects and practical value.

[0026] Other beneficial effects of the embodiments of the present invention will be further described below. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 It is a schematic structural diagram of an embodiment of the present invention. Figure 2 The effect of multi-level sampling is demonstrated for estimating the relative pose of the camera and LiDAR during the initialization phase. DETAILED DESCRIPTION

[0028] The following is a detailed description of the embodiments of the present invention. It should be emphasized that the following description is only exemplary and is not intended to limit the scope and application of the present invention.

[0029] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the features. In the description of the embodiments of the present invention, the meaning of "plurality" is two or more, unless otherwise clearly and specifically defined.

[0030] Definitions of Abbreviations and Key Terms:

[0031] An Inertial Measurement Unit (IMU) is an electronic device used to measure specific forces, angular velocity, and usually magnetic fields of an object.

[0032] Visual-Inertial Odometry (VIO),

[0033] VIO is a technology that combines visual sensors and inertial measurement units (IMUs) to estimate the pose of a device in real time.

[0034] Structure from Motion (SFM)

[0035] SFM is a computer vision technique that reconstructs 3D structures by analyzing moving objects in image sequences.

[0036] Bundle Adjustment (BA)

[0037] BA is an optimization technique that improves reconstruction accuracy by minimizing the error function and adjusting camera parameters and 3D point coordinates.

[0038] See also Figure 1 The embodiment of the present invention provides a visual odometer initialization method based on point cloud multi-layer sampling, which is applicable to an unmanned system of an unmanned aerial vehicle (UAV) and an unmanned ground vehicle (UGV) collaborative task, and includes the following steps:

[0039] Step 1: Visual front-end processing: Use the KLT sparse optical flow algorithm to match feature points between multiple consecutive frames of images and build a sliding window to obtain stable and continuous feature point data.

[0040] In a preferred embodiment, step one specifically includes the following sub-steps: feature point extraction: in each frame image, a number of corner points are extracted as initial feature points using a corner point detection algorithm; feature point tracking: between adjacent frame images, the KLT sparse optical flow algorithm is applied to track the initial feature points to determine their position changes in consecutive frames; feature point matching: using the position change information obtained by optical flow tracking, the corresponding feature point pairs are identified and matched in consecutive frames; sliding window construction: the matched feature point pairs are stored in a sliding window to maintain the continuity and stability of the feature point data, providing a reliable data basis for subsequent processing steps. Through the above processing, accurate extraction and stable tracking of feature points are ensured, providing high-quality input data for constructing scale-free SFM point clouds.

[0041] Step 2: Feature point tracking and SFM point cloud construction: Using the stable feature point data obtained in step 1, a scale-free SFM point cloud is constructed from the image sequence through 2D-2D feature association, PnP algorithm and triangulation method, providing a geometric basis for pose estimation.

[0042] In a preferred embodiment, step 2 specifically includes the following sub-steps: image selection: select a group of continuous images such as the latest 10 frames in the sliding window as the basis for SFM point cloud construction; feature point association: perform 2D-2D feature association on the selected images to find the correspondence between the same feature points in different images; pose estimation: use the PnP algorithm to estimate the pose of each frame of image relative to the initial camera coordinate system based on the 2D-2D feature point correspondence obtained in sub-step 2; triangulation: use the pose information obtained in sub-step 3 to calculate the scale-free coordinates of the feature points in three-dimensional space through the triangulation method; point cloud construction: combine the obtained three-dimensional coordinate points to construct the scale-free SFM point cloud of the entire scene. Through the above processing, the accurate construction of the SFM point cloud is ensured, providing accurate geometric information for subsequent visual-inertial fusion and scale estimation.

[0043] Step 3: Visual-inertial fusion and scale estimation: Combining the motion information of the IMU and the SFM point cloud obtained in step 2, the displacement and rotation information between adjacent frames are calculated through the pre-integration method, and then the actual scale of the SFM point cloud, the speed of the system, and the direction of gravity are estimated.

[0044] In a preferred embodiment, step three specifically includes the following sub-steps: IMU data preprocessing: pre-integrate the acceleration and angular velocity data collected by the IMU to calculate the relative displacement and rotation information between adjacent frames; pose and scale estimation: combine the SFM point cloud obtained in step two and the pre-integrated information in sub-step one, minimize the reprojection error and IMU pre-integrated error through the optimization algorithm, and estimate the actual scale of the SFM point cloud; velocity and gravity vector estimation: use the pose and scale information estimated in step two to further estimate the speed and gravity direction of the system; error minimization: through the iterative optimization process, continuously adjust the estimated values ​​of pose, scale, velocity and gravity vector until the predetermined error threshold is reached. Through the above processing, the scale of the SFM point cloud, the speed of the system and the gravity direction are accurately estimated, providing accurate parameters for visual-inertial fusion.

[0045] Step 4: Visual-inertial alignment: Based on step 3, a multi-level sampling method is used for visual-inertial alignment, including first-level sampling and second-level sampling, to achieve rapid convergence of the initial camera pose and generate a coarse registration pose estimate.

[0046] In a preferred embodiment, step 4 specifically includes the following sub-steps: initial sampling space division: evenly divide the sampling space into multiple small cubes to form the basis of the first layer of sampling; visual feature point cloud transformation: transform the visual feature point cloud to the LiDAR (laser detection and ranging) coordinate system according to the initial estimated position and scale; KD-tree nearest neighbor search: for each visual feature point, use the KD-tree data structure to perform a nearest neighbor search to find the corresponding LiDAR point; local plane fitting: perform local plane fitting on the found LiDAR points through QR decomposition ; Verification of the validity of the registration points: Calculate the distance from the visual feature points to the fitting plane and compare it with the preset threshold to verify the validity of the registration points; Sub-layer sampling refinement: Based on the small cubes successfully sampled in the first layer, further subdivide them into smaller cubes for sub-layer sampling; Repeat the registration process: Repeat the registration process for each small cube sampled in the sub-layer to achieve a more precise pose estimation; Convergence of the optimal initial pose: Through the iterative process, quickly converge to the optimal initial camera pose; Through the above processing, the accuracy and robustness of the visual-inertial alignment are improved through multi-level sampling technology.

[0047] Preferably, the first layer sampling specifically includes: determining the dimension and resolution of the sampling space to evenly divide small cubic blocks that meet the required accuracy requirements; transforming the visual feature point cloud from the camera coordinate system to the LiDAR coordinate system based on the initial pose estimation to ensure the accuracy of the coordinate conversion; constructing a KD-tree data structure for each visual feature point to optimize the nearest neighbor search process; using the KD-tree, performing a nearest neighbor search for each visual feature point to determine its corresponding point in the LiDAR point cloud; performing QR decomposition on the searched LiDAR point set to fit a local plane model; calculating the vertical distance from the visual feature point to the fitted local plane to evaluate the registration accuracy; and determining the pairing of the visual feature point and the LiDAR point whose distance is within the set distance threshold as a valid registration point.

[0048] Preferably, the sub-layer sampling specifically includes: determining the area that needs further refinement based on the successful registration result obtained by the first-layer sampling; subdividing each small cube block that has been initially successfully registered into smaller cube blocks according to a predetermined refinement ratio to achieve higher-precision sampling; repeating the KD-tree nearest neighbor search, local plane fitting and registration point validity verification operations in the first-layer sampling for each subdivided small cube block; fine-tuning the registration points in each small cube block through iterative optimization to achieve more precise pose estimation; accumulating and analyzing the registration points of all subdivided small cube blocks to screen out the optimal initial camera pose solution; updating the initial pose estimate according to the optimal solution to achieve rapid convergence to an accurate initial camera pose.

[0049] In a preferred embodiment, the multi-level sampling method includes: initial sampling space construction: evenly divide the three-dimensional sampling space into multiple cubic blocks with certain geometric intervals and resolutions to form a first-level sampling grid; first-level sampling execution: in the first-level sampling grid, determine the sampling points according to the preset yaw sampling interval and resolution, and perform preliminary registration; sampling subspace screening: based on the registration results of the first-level sampling, screen out several sampling subspaces, which are used as the center points of the second-level sampling; second-level sampling refinement: the screened sampling subspaces are further subdivided into cubic blocks with smaller geometric intervals and higher resolution to form a second-level sampling grid; second-level sampling execution: in the second-level sampling grid, according to the refined yaw sampling interval and resolution, a more refined registration process is performed; optimal pose estimation: through iterative optimization of the first-level and second-level sampling, quickly converge to the optimal initial camera pose. Among them, the multi-level sampling realizes efficient and accurate estimation of the relative pose of the camera and LiDAR by gradually refining the sampling space, and accelerates the initialization process.

[0050] Step 5: Collaborative optimization: Using the coarse registration result obtained in step 4, a point-assisted BA (Bundle Adjustment) optimization structure is constructed. By minimizing the visual reprojection error, IMU pre-integration error, and point-surface distance error, fine optimization is performed to obtain accurate relative pose estimation.

[0051] In a preferred embodiment, step five specifically includes the following sub-steps: initial pose and scale determination: using the rough alignment result obtained in step four, determine the initial pose and scale parameters; point-surface association construction: based on the initial pose and scale, establish the point-surface association relationship between the visual feature point cloud and the LiDAR point cloud; BA optimization problem construction: construct a Bundle Adjustment (BA) optimization problem including visual reprojection error, IMU pre-integration error and point-surface distance error; error model definition: define the mathematical model of each error term to ensure that the error function can accurately reflect the difference between the observed data and the model prediction; optimization algorithm selection: select a suitable numerical optimization algorithm, such as Ceres Solver, to solve the BA optimization problem; error minimization solution: execute the optimization algorithm, minimize the total error by adjusting the pose and scale parameters, and obtain an accurate relative pose estimate. Through the above processing, through the refined processing of the collaborative optimization module, high-precision pose estimation of the visual-inertial system is achieved.

[0052] In some embodiments, the method is particularly suitable for unmanned systems that require high-precision positioning and navigation, including but not limited to collaborative tasks of unmanned aerial vehicles and unmanned ground vehicles, and can significantly improve their positioning accuracy and reliability of mission execution in complex environments.

[0053] The present invention provides an innovative visual odometer initialization method based on multi-layer sampling of point cloud, which is particularly suitable for the high-precision positioning requirements of unmanned systems such as unmanned aerial vehicles (UAVs) and unmanned ground vehicles (UGVs) in collaborative tasks. By combining structure from motion (SFM) technology and multi-layer sampling optimization strategy, the method effectively overcomes the dependence of traditional visual inertial odometer (VIO) systems on prior maps or manually input initial values ​​in the initialization stage, and significantly improves the applicability and robustness of the system in unknown environments. By fusion of IMU data and SFM point cloud, the present invention not only solves the scale estimation problem, but also reduces the positioning error caused by environmental complexity and dynamic changes, and improves the accuracy and efficiency of pose estimation. In addition, through the collaborative optimization framework, the method minimizes the visual reprojection error, IMU pre-integration error and point-surface distance error, and realizes fast and accurate relative pose estimation. These advantages make the present invention have important practical value and broad application prospects for unmanned system applications in the fields of agriculture, military, rescue, logistics, etc. under the background of rapid development of automation technology.

[0054] The specific embodiments of the present invention are further described below.

[0055] The specific algorithm implementation of the present invention includes the following main parts:

[0056] Visual front-end module

[0057] The KLT sparse optical flow algorithm is used to match feature points between multiple frames and construct a sliding window. The KLT algorithm uses optical flow tracking technology to track the position changes of feature points in consecutive frames to achieve feature point matching. This step ensures the stability and continuity of feature points, providing a reliable data foundation for subsequent triangulation and SFM point cloud construction.

[0058] The specific steps include: firstly, extracting several corner points in each frame image as feature points, and then using the optical flow method to track the position changes of these feature points in adjacent frame images to obtain matching pairs of feature points.

[0059] SFM Point Cloud Building Module

[0060] The scale-free SFM point cloud is constructed by using 2D-2D feature association, PnP (perspective n-point) and triangulation methods. In the sliding window, the latest 10 frames are selected, and the corresponding feature point pairs are found through 2D-2D feature association. The PnP algorithm is used to estimate the posture of each frame relative to the initial camera coordinate system, and the scale-free coordinates of the feature points in three-dimensional space are reconstructed through triangulation.

[0061] The specific steps include: selecting the latest 10 frames of images in the sliding window, finding the correspondence between each pair of feature points in different images through 2D-2D feature association, using the PnP algorithm to estimate the pose of each frame of image relative to the initial camera coordinate system, and reconstructing the scale-free coordinates of each feature point in three-dimensional space through triangulation.

[0062] Visual-Inertial Fusion and Scale Estimation Module

[0063] The scale, velocity and gravity vector of the SFM point cloud are estimated through the motion information of the IMU. The acceleration and angular velocity data provided by the IMU are used to calculate the displacement and rotation information between adjacent frames through the pre-integration method. This information is combined with the geometric features of the SFM point cloud to estimate the actual scale of the SFM point cloud, as well as the velocity and gravity direction of the system.

[0064] The specific steps include: pre-integrating the IMU data in a sliding window to calculate the displacement and rotation information between adjacent frames; combining this information with the geometric features of the SFM point cloud, and estimating the actual scale of the SFM point cloud, the system's speed, and the direction of gravity by minimizing the reprojection error and the IMU pre-integration error.

[0065] Visual-Inertial Alignment Module

[0066] This module depends on multi-layer sampling. Multi-layer sampling includes first-layer sampling and second-layer sampling.

[0067] First layer sampling: The initial sampling space is evenly divided into multiple small cubes. In each small cube, a preliminary registration is performed in the LiDAR point cloud according to the initial estimated position and scale. The specific steps include: according to the initial estimated position, the visual feature point cloud is transformed into the LiDAR coordinate system, the KD-tree nearest neighbor search is used to find the LiDAR point corresponding to each visual feature point, and the local plane is fitted by QR decomposition. The distance from the visual feature point to the fitting plane is calculated. If the distance is within the threshold range, the registration point is considered valid.

[0068] Sub-layer sampling: Based on the preliminary registration results, the sampling is further refined. For each small cube that is successfully preliminarily registered, it is subdivided into smaller cubes again, and the preliminary registration process is repeated. This can quickly converge to the optimal initial camera pose, ensuring high accuracy and robustness of the initial pose.

[0069] The specific steps of first-layer sampling include:

[0070] 1. Divide the initial sampling space evenly into multiple small cubes.

[0071] 2. Based on the initial estimated position, transform the visual feature point cloud to the LiDAR coordinate system.

[0072] 3. Use KD-tree nearest neighbor search to find the LiDAR point corresponding to each visual feature point.

[0073] 4. Fit the local plane through QR decomposition and calculate the distance from the visual feature point to the fitting plane.

[0074] 5. If the distance is within the threshold range, the registration point is considered valid.

[0075] The specific steps of sub-layer sampling include:

[0076] 1. Based on the preliminary registration results, further refine the sampling.

[0077] 2. For each small cube that is initially successfully registered, subdivide it into smaller cubes again.

[0078] 3. Repeat the initial registration process to quickly converge to the optimal initial camera pose.

[0079] Figure 2The effect of multi-level sampling is demonstrated for estimating the relative pose of the camera and LiDAR in the initialization stage. Specifically, this figure shows the sampling process in the experimental scene. The initial space is first evenly divided, and the 3D geometric sampling interval of the first layer is 4×4×4 meters, the resolution is 0.5 meters, the yaw sampling interval is 120 degrees, and the resolution is 10 degrees. Then, three sampling subspaces are screened as the center points of the second layer sampling. The 3D geometric sampling interval of the second layer is 0.5×0.5×0.5 meters, the resolution is 0.0625 meters, the yaw sampling interval is 12 degrees, and the resolution is 2 degrees. By constructing a multi-level sampling space, the most suitable initial camera pose can be found faster, thereby accelerating the initialization process. Experimental verification shows that under the same initial space and resolution conditions, the time for double-layer sampling is only 0.1% of the uniform sampling time, which greatly accelerates the initialization.

[0080] Collaborative Optimization Module

[0081] Based on the coarse registration results, a point-assisted BA (Bundle Adjustment) optimization structure is constructed to obtain accurate relative pose through optimization. The specific steps include: using the initial pose and scale obtained by visual-inertial alignment, constructing the point-surface association between the visual feature point cloud and the LiDAR point cloud, constructing the BA optimization problem, and solving it using Ceres Solver. During the optimization process, the BA framework simultaneously considers the visual reprojection error, IMU pre-integration error, and point-surface distance error, and obtains accurate relative pose estimation by minimizing these error functions.

[0082] This paper proposes a visual odometer initialization method based on multi-layer sampling of point cloud, and applies multi-layer sampling in the initialization process of the VIO system, especially its innovative design and implementation in the matching of visual feature points and LiDAR point cloud; an initialization method combining SFM and IMU is proposed, especially in unknown environments, without relying on prior maps or manual initial values, a method for correcting the scale of SFM point cloud and improving its accuracy using IMU data.

[0083] The initialization scheme combining multi-layer sampling and SFM method proposed in the present invention significantly improves the initialization accuracy and efficiency of the VIO system in an unknown environment, and has the following beneficial effects:

[0084] 1. High-precision initial pose estimation: Through the multi-layer sampling method, it quickly converges to the optimal initial camera pose, ensuring the high accuracy and robustness of the initial pose. Combining SFM point cloud and IMU data, it solves the scale estimation error and trajectory drift problems existing in traditional methods.

[0085] 2. Efficient initialization process: The multi-layer sampling method significantly reduces the sampling space, improves sampling efficiency, and shortens the initialization time. The collaborative optimization framework combines visual reprojection error, IMU pre-integration error, and point-surface distance error to quickly obtain accurate relative pose estimation by minimizing these error functions.

[0086] 3. Strong robustness: It does not rely on prior maps or manually provided initial values, which enhances the applicability of the system in unknown environments. A new collaborative optimization framework is proposed to effectively solve the drift problem existing in existing methods.

[0087] Through the implementation of the present invention, the initial pose estimation accuracy and efficiency of the VIO system in an unknown environment can be significantly improved, and the present invention has broad application prospects and practical value.

[0088] An embodiment of the present invention further provides a storage medium for storing a computer program, which at least performs the above method when executed.

[0089] An embodiment of the present invention further provides a control device, comprising a processor and a storage medium for storing a computer program; wherein the processor is configured to execute at least the method described above when executing the computer program.

[0090] An embodiment of the present invention further provides a processor, wherein the processor executes a computer program and at least executes the method described above.

[0091] The storage medium can be implemented by any type of non-volatile storage device, or a combination thereof. Among them, the non-volatile memory can be a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), a magnetic random access memory (FRAM), a ferromagnetic random access memory (Flash Memory), a magnetic surface memory, an optical disc, or a compact disc read-only memory (CD-ROM); the magnetic surface memory can be a disk memory or a tape memory. The storage medium described in the embodiments of the present invention is intended to include, but is not limited to, these and any other suitable types of memory.

[0092] In the several embodiments provided by the present invention, it should be understood that the disclosed systems and methods can be implemented in other ways. The device embodiments described above are only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation, such as: multiple units or components can be combined, or can be integrated into another system, or some features can be ignored or not executed. In addition, the coupling, direct coupling, or communication connection between the components shown or discussed can be through some interfaces, and the indirect coupling or communication connection of the devices or units can be electrical, mechanical or other forms.

[0093] The units described above as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units; some or all of the units may be selected according to actual needs to achieve the purpose of the present embodiment.

[0094] In addition, all functional units in the embodiments of the present invention may be integrated into one processing unit, or each unit may be separately used as a unit, or two or more units may be integrated into one unit; the above-mentioned integrated units may be implemented in the form of hardware or in the form of hardware plus software functional units.

[0095] A person skilled in the art can understand that: all or part of the steps of implementing the above method embodiment can be completed by hardware related to program instructions, and the aforementioned program can be stored in a computer-readable storage medium. When the program is executed, it executes the steps of the above method embodiment; and the aforementioned storage medium includes: mobile storage devices, read-only memories (ROM, Read-Only Memory), random access memories (RAM, Random Access Memory), disks or optical disks, etc. Various media that can store program codes.

[0096] Alternatively, if the above-mentioned integrated unit of the present invention is implemented in the form of a software function module and sold or used as an independent product, it can also be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the embodiment of the present invention can be essentially or partly reflected in the form of a software product that contributes to the prior art. The computer software product is stored in a storage medium and includes several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the methods described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as mobile storage devices, ROM, RAM, magnetic disks or optical disks.

[0097] The methods disclosed in the several method embodiments provided by the present invention can be arbitrarily combined without conflict to obtain new method embodiments.

[0098] The features disclosed in several product embodiments provided by the present invention can be arbitrarily combined without conflict to obtain new product embodiments.

[0099] The features disclosed in several method or device embodiments provided by the present invention can be arbitrarily combined without conflict to obtain new method embodiments or device embodiments.

[0100] The above contents are further detailed descriptions of the present invention in combination with specific preferred embodiments, and it cannot be determined that the specific implementation of the present invention is limited to these descriptions. For those skilled in the art of the present invention, several equivalent substitutions or obvious variations can be made without departing from the concept of the present invention, and the performance or use is the same, which should be regarded as belonging to the protection scope of the present invention.

[0101] The background section of the present invention may contain background information about the problem or environment of the present invention, but does not necessarily describe the prior art. Therefore, the content included in the background section is not an admission by the applicant that the prior art is available.

[0102] The above content is a further detailed description of the present invention in combination with specific / preferred embodiments, and it cannot be determined that the specific implementation of the present invention is limited to these descriptions. For ordinary technicians in the technical field to which the present invention belongs, without departing from the concept of the present invention, it can also make several substitutions or modifications to these described embodiments, and these substitutions or modifications should be regarded as belonging to the protection scope of the present invention. In the description of this specification, the description of the reference terms "an embodiment", "some embodiments", "preferred embodiments", "examples", "specific examples", or "some examples" means that the specific features, structures, materials or characteristics described in combination with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily target the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner. In the absence of mutual contradiction, those skilled in the art can combine and combine the different embodiments or examples described in this specification and the features of different embodiments or examples. Although the embodiments of the present invention and their advantages have been described in detail, it should be understood that various changes, substitutions and modifications can be made herein without departing from the scope of protection of the patent application.

Claims

1. A visual odometry initialization method based on multi-layer sampling of point clouds, suitable for unmanned systems of unmanned aerial vehicles (UAVs) and unmanned ground vehicles (UGVs) for collaborative tasks, characterized in that: The following steps are involved: Step 1: Use the KLT sparse optical flow algorithm to match feature points between multiple consecutive frames of images and build a sliding window to obtain stable and continuous feature point data; Step 2: Using the feature point data obtained in step 1, a scale-free SFM point cloud is constructed from the image sequence through 2D-2D feature association, PnP algorithm and triangulation method to provide a geometric basis for pose estimation; Step 3: Combine the motion information of the IMU and the SFM point cloud obtained in step 2, calculate the displacement and rotation information between adjacent frames through the pre-integration method, and then estimate the actual scale of the SFM point cloud, the speed of the system, and the direction of gravity; Step 4: Based on step 3, a multi-level sampling method is used for visual-inertial alignment, including first-level sampling and second-level sampling, to achieve rapid convergence of the initial camera pose and generate a coarse registration pose estimate; Step 5: Using the coarse registration result obtained in step 4, construct a point-assisted BA optimization structure, and perform fine optimization by minimizing the visual reprojection error, IMU pre-integration error, and point-surface distance error to obtain accurate relative pose estimation.

2. The visual odometer initialization method based on point cloud multi-layer sampling according to claim 1, characterized in that: Step 1 specifically includes: In each frame of the image, a number of corner points are extracted as initial feature points using the corner detection algorithm; Between adjacent frame images, the KLT sparse optical flow algorithm is applied to track the initial feature points to determine their position changes in consecutive frames; Using the position change information obtained by optical flow tracking, the corresponding feature point pairs are identified and matched in consecutive frames; The matched feature point pairs are stored in a sliding window to maintain the continuity and stability of the feature point data, providing a reliable data basis for subsequent processing steps.

3. The visual odometer initialization method based on point cloud multi-layer sampling according to claim 1 or 2, characterized in that: Step 2 specifically includes: Select a set of consecutive images in the sliding window as the basis for SFM point cloud construction; Perform 2D-2D feature association on the selected images to find the correspondence between the same feature points in different images; Using the PnP algorithm, based on the 2D-2D feature point correspondence obtained in sub-step 2, the pose of each frame image relative to the initial camera coordinate system is estimated; Using the pose information obtained in sub-step 3, the scale-free coordinates of the feature points in three-dimensional space are calculated by triangulation method; The obtained three-dimensional coordinate points are combined to construct a scale-free SFM point cloud of the entire scene.

4. The visual odometer initialization method based on point cloud multi-layer sampling according to any one of claims 1 to 3, characterized in that: Step three specifically includes: Pre-integrate the acceleration and angular velocity data collected by the IMU to calculate the relative displacement and rotation information between adjacent frames; Combining the SFM point cloud obtained in step 2 with the pre-integration information in sub-step 1, the actual scale of the SFM point cloud is estimated by minimizing the reprojection error and the IMU pre-integration error through an optimization algorithm. Using the pose and scale information estimated in step 2, further estimate the speed and gravity direction of the system; Through an iterative optimization process, the estimated values ​​of pose, scale, velocity, and gravity vector are continuously adjusted until a predetermined error threshold is reached.

5. The visual odometer initialization method based on point cloud multi-layer sampling according to any one of claims 1 to 4, characterized in that: Step 4 specifically includes: Evenly divide the sampling space into multiple small cubes to form the basis of the first layer of sampling; According to the initial estimated position and scale, the visual feature point cloud is transformed into the LiDAR coordinate system; KD-tree nearest neighbor search: For each visual feature point, use the KD-tree data structure to perform a nearest neighbor search to find the corresponding LiDAR point; Perform local plane fitting on the found LiDAR points through QR decomposition; Calculate the distance from the visual feature point to the fitting plane and compare it with the preset threshold to verify the validity of the registration point; Based on the small cubes successfully sampled in the first layer, they are further subdivided into smaller cubes for secondary layer sampling; Repeat the registration process for each small cube sampled in the next layer to achieve a more refined pose estimate; Through the iterative process, it quickly converges to the optimal initial camera pose.

6. The visual odometer initialization method based on point cloud multi-layer sampling according to claim 5, characterized in that: The first layer sampling specifically includes: Determine the dimension and resolution of the sampling space to evenly divide it into small cubes that meet the required accuracy requirements; According to the initial pose estimation, the visual feature point cloud is transformed from the camera coordinate system to the LiDAR coordinate system to ensure the accuracy of the coordinate conversion; For each visual feature point, a KD-tree data structure is constructed to optimize the nearest neighbor search process; Using KD-tree, perform nearest neighbor search for each visual feature point to determine its corresponding point in the LiDAR point cloud; Perform QR decomposition on the searched LiDAR point set and fit a local plane model; Calculate the vertical distance from the visual feature point to the fitted local plane to evaluate the registration accuracy; The pairing of visual feature points and LiDAR points whose distance is within the set distance threshold is determined as a valid registration point.

7. The visual odometer initialization method based on point cloud multi-layer sampling according to claim 5, characterized in that: The sub-layer sampling specifically includes: Based on the successful registration results obtained from the first layer sampling, determine the areas that need further refinement; For each small cube that is initially successfully registered, it is subdivided into smaller cubes according to a predetermined refinement ratio to achieve higher-precision sampling; For each small cube after subdivision, the KD-tree nearest neighbor search, local plane fitting and registration point validity verification operations in the first layer sampling are repeated; Through iterative optimization, the registration points in each small cube are fine-tuned to achieve more precise pose estimation; Accumulate and analyze the registration points of all subdivided small cubes to select the optimal initial camera pose solution; Update the initial pose estimate based on the optimal solution to achieve fast convergence to the accurate initial camera pose.

8. The visual odometer initialization method based on point cloud multi-layer sampling according to any one of claims 1 to 7, characterized in that: In step 4, the multi-level sampling method includes: The three-dimensional sampling space is evenly divided into multiple cubic blocks with certain geometric intervals and resolutions to form a first-layer sampling grid; In the first layer of sampling grid, the sampling points are determined according to the preset yaw sampling interval and resolution, and preliminary registration is performed; Based on the registration results of the first-layer sampling, several sampling subspaces are selected and used as the center points of the second-layer sampling; The selected sampling subspace is further subdivided into cubic blocks with smaller geometric intervals and higher resolution to form a sub-layer sampling grid; In the sub-layer sampling grid, a more refined registration process is performed according to the refined yaw sampling interval and resolution; Through iterative optimization of first-layer and second-layer sampling, it quickly converges to the optimal initial camera pose.

9. The visual odometer initialization method based on point cloud multi-layer sampling according to any one of claims 1 to 8, characterized in that: Step 5 specifically includes: Using the rough registration result obtained in step 4, determine the initial pose and scale parameters; Based on the initial pose and scale, a point-surface association relationship is established between the visual feature point cloud and the LiDAR point cloud; Construct a BA optimization problem that includes visual reprojection error, IMU pre-integration error, and point-surface distance error; Define the mathematical model for each error term to ensure that the error function accurately reflects the difference between the observed data and the model prediction; Solve the BA optimization problem through numerical optimization algorithm; An optimization algorithm is executed to minimize the total error and obtain an accurate relative pose estimate by adjusting the pose and scale parameters.

10. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the visual odometer initialization method based on point cloud multi-layer sampling is implemented as described in any one of claims 1 to 6.

Images