A visual UAV trajectory estimation method with common view constraint

Abstract

This invention discloses a visual drone trajectory estimation method with common-view constraints. The method first dynamically switches between pose estimation based on the nearest keyframe map point and a sliding window map point based on the visibility rate of the nearest keyframe map point from an image frame with a timestamp of t to obtain an initial pose. During this process, some image frames are selected as keyframes. Next, a gradient analysis is performed on the newly selected keyframes to obtain a common-view keyframe set, which provides common-view constraints. A distance map is generated using the map points within the sliding window and the common-view keyframe set to distinguish between landmark and non-landmark map points, improving optimization efficiency. Finally, the keyframes and map points within the sliding window and the common-view keyframe set are jointly optimized to obtain the final trajectory. The proposed trajectory estimation method achieves similar accuracy to similar algorithms while improving per-frame image processing speed.

Description

Technical Field

[0001] The present invention relates to the field of processing image data for point cloud map construction and trajectory estimation, and in particular to a visual UAV trajectory estimation method with common view constraints. Background Art

[0002] Monocular visual odometry can be primarily categorized into direct methods and feature point methods. While feature point-based visual odometry provides relatively accurate trajectory estimates in most scenarios, it can output incorrect poses in scenes lacking image features, such as motion blur and minimal environmental texture. Feature point methods retain only a small number of feature points to avoid mismatches, resulting in sparse point cloud maps, which is unsuitable for practical applications. Direct methods, on the other hand, fully utilize image texture information, minimize degradation, and output semi-dense point cloud maps. Direct methods also utilize real-time photometric parameter estimation to address photometric variations, making them more practical. Compared to feature point-based visual odometry, direct methods eliminate the feature matching stage in the front-end, resulting in faster tracking times. However, current direct methods suffer from time-consuming back-end optimization, resulting in overall efficiency inferior to feature point-based methods, making them less applicable for tasks requiring high real-time performance.

[0003] Furthermore, existing visual odometry still produces significant cumulative errors when estimating long trajectories. Most solutions eliminate this cumulative error by detecting the similarity of features between two images to perform historical scene recognition. However, this approach can lead to sudden changes in pose in practical applications, which can cause confusion in upper-level tasks during engineering implementation, reduce the overall stability and reliability of the system, and hinder smooth human-computer interaction. Summary of the Invention

[0004] The purpose of the present invention is to solve the above-mentioned defects in the prior art and provide a visual drone trajectory estimation method with common view constraint.

[0005] The purpose of the present invention can be achieved by taking the following technical solutions:

[0006] A method for visual UAV trajectory estimation with common view constraint, the trajectory estimation method comprising the following steps:

[0007] S1, let the drone move freely, collect images through the camera sensor on the drone, obtain an image sequence consisting of n frames, and record the timestamp of the image frame as , define the image frame with timestamp t as ;

[0008] S2, the first image frame in the image sequence With the second frame image frame Initialize to get the initial motion of the drone. and All of them are selected as key frames, and the gradient points in the key frames are extracted. The gradient points are triangulated using the initial motion to estimate the depth value. The gradient points with accurate depth values ​​are marked as map points, and the gradient points with inaccurate depth values ​​are marked as candidate points.

[0009] S3. Continue to read subsequent image frames according to the timestamp from small to large , used with The most recent timestamp Frame keyframes form a sliding window ,in is the jth key frame in the sliding window; The map points in the project are projected onto , calculate the proportion of map points that are successfully projected ;

[0010] S4. If , constructed using the successfully projected map points and The photometric residual between them; the photometric response function is defined as:

[0011] in is the original grayscale value of the pixel, is the true grayscale value of the pixel after correction, is the gain coefficient, is the offset, then The gain coefficient of the photometric response function is , The gain coefficient of the photometric response function is ;right and Perform optimization calculation based on photometric residual to obtain and timestamp as The initial pose of the UAV ;like ,Will All map points on the keyframes are projected onto , reconstruct the photometric residual, and Perform optimization calculation based on photometric residual to obtain and ;

[0012] S5. Use Gaussian distribution to describe the depth value of the candidate point. Perform block matching on the candidate points to update the standard deviation of the Gaussian distribution of the depth value ;definition and The moving distance of the UAV is , and The difference is ,right 、 and Perform weighted summation to obtain the tracking state value ,like , continue processing the next image frame according to steps S3-S5;

[0013] S6, if ,Will Select as keyframe and insert ;Will All map points on the keyframes are projected onto Get the occupied pixel set and calculate The Euclidean distance between each pixel in the occupies pixel set and each pixel in the occupied pixel set is used to obtain a distance map;

[0014] S7, yes Perform gradient analysis based on distance graph to obtain Key frames with a common viewing relationship constitute a common viewing key frame set and use Update the distance map for all map points on key frames;

[0015] S8, If the candidate points on the inner key frame meet , then re-mark it as a map point and project it to Get pixel coordinates ; Query distance map The distance value ,like , then the map point is classified as a non-landmark map point; if , then the map point is classified as a landmark map point;

[0016] S9, continue to select Keyframes with co-viewing relationship and added , up to the number of landmark map points ;right and All keyframes and map points in the image are optimized based on photometric residuals. During the optimization process, the photometric residuals constructed by landmark map points are used to optimize both the UAV’s position and the depth of the map points, while the photometric residuals constructed by non-landmark map points are only used to optimize the depth of the map points. After optimization, the local motion trajectory of the UAV is obtained.

[0017] S10, looping through steps S3-S10 until all image frames in the image sequence are processed, and obtaining the global motion trajectory of the UAV.

[0018] Furthermore, the process of step S3 is as follows:

[0019] S3.1. Continue reading subsequent image frames according to the timestamps from smallest to largest. , used with The most recent timestamp Frame keyframes form a sliding window ,in The first Frame keyframes; selecting keyframes based on timestamps not only speeds up processing but also effectively utilizes the high visual similarity between temporally close frames, thus providing accurate visual constraints for UAV pose optimization;

[0020] S3.2, assuming that the drone and If the motion is kept at a constant speed, then ,in for and The pose change matrix of the UAV, for and The UAV pose change matrix between and The pose change matrix of the UAV ,in for and The pose change matrix of the UAV in the Map point in ,in for exist The pixel coordinates on for exist The depth value on Projection to :

[0021] in for Projection to The pixel coordinates on for After projection The depth value on is the camera intrinsic parameter matrix; when is not negative and exist Inside, it is believed Projection successful;

[0022] S3.3, follow step S3.2 to Process all map points in the projected image to obtain the proportion of map points that are successfully projected. .

[0023] Furthermore, the process of step S4 is as follows:

[0024] S4.1. If ,right The photometric residual is constructed as follows:

[0025]

[0026] in express The photometric residual, for A set of surrounding pixels, for Pixels in for exist The gray value on for of The gray value on yes The offset of the photometric response function, yes The offset of the photometric response function, represents the Huber norm; yes The gradient weight of is a constant, represents the Euclidean norm; when When using The map points in the sliding window are usually sufficient to provide sufficient visual constraints for the pose optimization of the UAV; compared to using all the map points in the sliding window for each image frame, the computational overhead is reduced while maintaining good pose optimization accuracy;

[0027] S4.2, yes Scale down by 2 times and 4 times, build The image pyramid of , the second layer is The third layer is 2 times smaller than 4 times smaller;

[0028] S4.3, yes Use T distribution to fit the photometric residual distribution and obtain The luminosity distribution weight:

[0029] in yes The luminosity distribution weight, is the degrees of freedom of the T distribution, is the scale of T distribution; the material of some objects may cause abnormal light reflection intensity. Even after photometric correction, the photometric residual constructed by some map points still cannot accurately reflect the true photometric residual between two image frames; in order to reduce the impact of such map points on the overall visual constraints, the distribution of photometric residuals is fitted using T distribution to effectively reduce the weight of abnormal map points; according to The image pyramid order is from the third layer to the first layer, and the following nonlinear least squares problem is solved for the image frame of each layer:

[0030]

[0031] in , is in Projection is successful and in Image Pyramid Map point set on the layer image frame; calculate the optimized ,in for The drone’s position during acquisition;

[0032] S4.4, if ,Will All map points on the keyframes are projected onto , reconstruct the photometric residual; Use T distribution to fit the photometric residual distribution and obtain The luminosity distribution weights of the map points successfully projected on all key frames in the The image pyramid order is from the third layer to the first layer, and the following nonlinear least squares problem is solved for each layer of image frame:

[0033] in , yes The projection is successful on all key frames in the image pyramid The map point set on the layer image frame, yes The map point in yes The luminosity distribution weight, yes The photometric residual of ;when When the drone is and The movement between The map points in cannot provide effective visual constraints; at this point, the credibility of the UAV pose optimization results is reduced, so it is necessary to introduce All map points on the key frames in the map participate in pose optimization; according to Adaptive direct pose optimization is performed to ensure stable pose optimization results.

[0034] Furthermore, the process of step S5 is as follows:

[0035] S5.1, use Gaussian distribution to describe the depth value of the candidate point, Perform block matching on the candidate points to update the standard deviation of the Gaussian distribution of the depth value ;

[0036] S5.2, yes 、 and The weighted summation is calculated according to the following formula :

[0037] , ,

[0038] in yes The translation component of yes The translation component of is in The map points in The average value of the upper depth value; generally let , ;pass To select key frames, we can reduce the complexity of trajectory optimization calculation while maintaining the accuracy of trajectory estimation. To achieve this goal, key frames should be evenly distributed in space to capture new perspectives and scene information. Therefore, the UAV should maintain an appropriate movement distance between two adjacent key frames in time, which will be achieved through right Have an impact, In addition, there should be enough overlapping area between two temporally adjacent key frames to ensure the accuracy of trajectory estimation. The smaller the overlapping area, the better. The smaller, the Finally, the keyframe should be able to reflect the important lighting changes in the environment to enhance the robustness of trajectory estimation in dynamic scenes. The larger the ;

[0039] S5.3, if , continue processing the next image frame according to steps S3-S5.

[0040] Furthermore, the process of step S7 is as follows:

[0041] S7.1. For existing The key frames are divided according to the sliding window, and the frames that are not in the sliding window are used. The key frames form a historical key frame set, and the map points on the key frames in the historical key frame set are Projection to superior, After projection The pixel coordinates on , query from the distance map The distance value at ;

[0042] S7.2 Definitions All map points on the keyframes are projected onto The number on , the distance judgment threshold is calculated according to the following formula :

[0043] Among them, ;like ,but yes interior point on ; definition The gradient at the key frame is , The grayscale at the key frame is , exist The grayscale on , exist The gradient on ;like ,calculate The ratio of gradient to grayscale at the key frame and exist The ratio of the gradient to the grayscale ; The ratio is used to normalize the gradient to the same scale for effective comparison; if the following conditions are met, then yes An interior point on yes External points on:

[0044] in yes The gain coefficient of the photometric response function of the key frame is introduced. It can effectively reduce misjudgment caused by changes in ambient light;

[0045] In the distance map, the area with smaller distance is the projection area of ​​the map point; if the map point on the historical key frame is projected to This area shows The edge of the same object in the historical keyframe reappears in the image, indicating that the map point is an interior point; on the contrary, the area with a larger distance is the area without map point projection. The situation of the area without map point projection can be divided into two categories: one is The area with larger gradient in the key frame has been extracted but is still a candidate point; due to the visual similarity between image frames with similar time, the area without map point projection is in The gradient is also larger in the area; if the key frame and The difference in the ratio of gradient to grayscale is small, indicating that the edge of the same object reappears, so the map point is an interior point; secondly, The region with smaller gradient in the key frame cannot extract the map points; due to the visual similarity between image frames with similar time, the region without map point projection is The gradient is also smaller in the area; if the key frame and The gradient and grayscale ratio of is quite different, indicating that the edge of the object is blocked by the plane of another object, so the map point is an outlier. Removing outliers will improve the trajectory estimation accuracy and reduce the additional optimization time. This is to appropriately relax the threshold when there are fewer map points on the keyframe within the sliding window, so that more map points can participate in trajectory optimization;

[0046] S7.3, according to the map point of the key frame The number of interior points on With external points Calculate the inlier ratio ;like , then the key frame and There is a co-viewing relationship between Select key frames with common viewing relationship The highest three key frames constitute the common view key frame set and use Update the distance map for all map points on keyframes within ; ensure There are at least three keyframes in the frame to provide sufficient common view constraints. Inner keyframe and The photometric consistency is improved, thereby effectively reducing the cumulative error of trajectory estimation. The cumulative error mainly comes from the small errors that continue to accumulate in the long-term trajectory estimation, causing the final estimated trajectory to deviate from the true path.

[0047] Furthermore, the process of step S9 is as follows:

[0048] S9.1, continue to select Keyframes with co-viewing relationship and added , until and The number of landmark map points on the inner keyframe ;at this time The CCP has Frame keyframe, then ,in for Neidi Frame keyframe; When the UAV moves violently, the image frame will be blurred, which will make it difficult to reduce the standard deviation of the Gaussian distribution of the candidate point depth values, resulting in an insufficient number of landmark map points; In order to avoid the reduction of trajectory estimation accuracy due to an insufficient number of landmark map points, more keyframes should be selected to ensure that there are enough landmark map points involved in the optimization;

[0049] S9.2, and All map points on the keyframes are projected onto Construct the photometric residual on the map and store it in the structure of the map point; define and The set of key frames other than ;for If the number of light residuals stored in the structure of the landmark map point on the inner key frame is greater than 8, then delete Photometric residual constructed on the inner key frame; If the number of light residuals stored in the structure of the landmark map point on the inner key frame is greater than 15, then delete Photometric residual constructed on the inner keyframe; in trajectory optimization, The photometric residual constructed on the inner keyframe only provides prior information to maintain the consistency of trajectory estimation before and after optimization. However, too much prior information will reduce the robustness of the optimization result to image noise and dynamic changes, thus affecting the final trajectory accuracy, so it should be appropriately deleted. For non-landmark map points, more constraints need to be maintained to ensure that the depth value can converge quickly during the optimization process, providing an accurate description of the environment for the pose optimization of subsequent image frames.

[0050] S9.3, yes and Solve the following nonlinear least squares problem for all keyframes and map points:

[0051]

[0052] in for The collection of map points, for Map points within for The collection of map points, for Map points within yes The structure stored in Photometric residual, yes The T distribution weight, yes The structure stored in Photometric residual, yes The T distribution weight of The state vector contained is , yes Previous The depth value of each map point, The state vector contained is , yes Previous The depth value of each map point, for The drone’s position during collection, yes The gain coefficient of the photometric response function, yes The offset of the photometric response function is During the solution process, the photometric residual constructed by landmark map points is used to optimize the UAV's posture and the depth value of the map points at the same time, while the photometric residual constructed by non-landmark map points is only used to optimize the depth value of the map points; after the solution, the local motion trajectory of the UAV is obtained.

[0053] To optimize the UAV's pose and map point depth values, it is necessary to calculate the Jacobian matrix of the photometric residual versus the UAV's pose and the map point depth values. Since the Jacobian matrix of the photometric residual versus the UAV's pose is large in dimension, the computational effort increases significantly. In trajectory estimation, effective description of object edges by map points ensures accuracy, while over-description consumes a lot of computing resources and has limited effectiveness. Therefore, for map points that are too close, that is, non-landmark map points, only the Jacobian matrix of the photometric residual versus the map point's depth value needs to be calculated to reduce the computational burden.

[0054] The present invention has the following advantages and effects compared to the prior art:

[0055] (1) To address the problem that the existing direct-method visual odometry backend requires a large number of map points to optimize, resulting in a long optimization time, this paper proposes a map point classification method based on a distance graph. This method classifies map points into two categories: non-landmark map points and landmark map points. This eliminates the calculation of the Jacobian matrix of the photometric residual for non-landmark map points, thus reducing optimization time.

[0056] (2) To address the cumulative error and pose jump problems in trajectory estimation, this paper proposes a method for constructing precise visual common view constraints and an adaptive direct pose optimization mechanism to strengthen mid-term data association, reduce the decline in trajectory estimation accuracy caused by visual degradation, and eliminate the long-term cumulative error in trajectory estimation. Experimental results show that this paper significantly improves the trajectory estimation speed in complex scenarios while still maintaining a high trajectory estimation accuracy, ensuring the stability and reliability of the odometry trajectory estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0058] Figure 1 This is a flowchart of a visual UAV trajectory estimation method with common view constraint disclosed by the present invention;

[0059] Figure 2 It is a pixel arrangement diagram around a map point in the present invention;

[0060] Figure 3 It is the factor graph optimized based on the pose of the nearest keyframe map point in the present invention;

[0061] Figure 4 It is the factor graph based on the sliding window map point pose optimization in the present invention;

[0062] Figure 5 is a local optimization factor graph of the common viewing key frame set and the sliding window in the present invention;

[0063] Figure 6 This is a point cloud comparison diagram of a visual drone trajectory estimation method with common view constraint in an indoor motion scene in the present invention;

[0064] Figure 7 This is a comparison diagram of common view map points of a visual UAV trajectory estimation method with common view constraint disclosed by the present invention. DETAILED DESCRIPTION

[0065] In order to enable those skilled in the art to better understand the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments in the present invention, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.

[0066] References to "embodiments" in this application mean that a particular feature, structure, or characteristic described in connection with the embodiment may be included in at least one embodiment of the application. The appearance of this phrase in various places in the specification does not necessarily refer to the same embodiment, nor does it constitute an independent or alternative embodiment that is mutually exclusive of other embodiments. It is understood, both explicitly and implicitly, by those skilled in the art that the embodiments described in this application may be combined with other embodiments.

[0067] Example

[0068] like Figure 1 As shown, this embodiment specifically discloses a visual drone trajectory estimation method with common view constraint, which includes the following steps:

[0069] S1, make the drone move freely, collect images through the camera sensor mounted on the drone, and obtain The image sequence consists of image frames, and the timestamp of the image frame is recorded as , define the timestamp as The image frame is ;

[0070] S2, the first image frame in the image sequence With the second frame image frame Initialize to get the initial motion of the drone. and All of them are selected as key frames, and the gradient points in the key frames are extracted. The gradient points are triangulated using the initial motion to estimate the depth value. The gradient points with accurate depth values ​​are marked as map points, and the gradient points with inaccurate depth values ​​are marked as candidate points.

[0071] S3. Continue to read subsequent image frames according to the timestamp from small to large , used with The most recent timestamp Frame keyframes form a sliding window ,in The first Frame keyframe; will be in The map points in the project are projected onto , calculate the proportion of map points that are successfully projected ;

[0072] In this embodiment, the process of step S3 is as follows:

[0073] S3.1. Continue reading subsequent image frames according to the timestamps from smallest to largest. , used with The most recent timestamp Frame keyframes form a sliding window ,in The first Frame keyframe;

[0074] S3.2, assuming that the drone and If the motion is kept at a constant speed, then ,in for and The pose change matrix of the UAV, for and The UAV pose change matrix between and The pose change matrix of the UAV ,in for and The pose change matrix of the UAV in the Map point in ,in for exist The pixel coordinates on for exist The depth value on Projection to :

[0075]

[0076] in for Projection to The pixel coordinates on for After projection The depth value on is the camera intrinsic parameter matrix; when is not negative and exist Inside, it is believed Projection successful;

[0077] S3.3, follow step S3.1 to Process all map points in the projected image to obtain the proportion of map points that are successfully projected. .

[0078] S4. If , constructed using the successfully projected map points and The photometric residual between them; the photometric response function is defined as:

[0079] in is the original grayscale value of the pixel, is the true grayscale value of the pixel after correction, is the gain coefficient, is the offset, then The gain coefficient of the photometric response function is , The gain coefficient of the photometric response function is ;right and Perform optimization calculation based on photometric residual to obtain and timestamp as The initial pose of the UAV ;like ,Will All map points on the keyframes are projected onto , reconstruct the photometric residual, and Perform optimization calculation based on photometric residual to obtain and ;

[0080] In this embodiment, the process of step S4 is as follows:

[0081] S4.1. If ,right The photometric residual is constructed as follows:

[0082]

[0083] in express The photometric residual, for Around Figure 2 The set of pixels shown is composed of for Pixels in for exist The gray value on for of The gray value on yes The offset of the photometric response function, yes The offset of the photometric response function, represents the Huber norm; yes The gradient weight of is a constant, represents the Euclidean norm;

[0084] S4.2, yes Scale down by 2 times and 4 times, build The image pyramid of , the second layer is The third layer is 2 times smaller than 4 times smaller;

[0085] S4.3, yes Use T distribution to fit the photometric residual distribution and obtain The luminosity distribution weight:

[0086] in yes The luminosity distribution weight, is the degrees of freedom of the T distribution, is the scale of the T distribution to reduce the accuracy drop caused by map points with large photometric residuals; according to The image pyramid order, from the third layer to the first layer, solves the following nonlinear least squares problem for each layer of image frame, and gradually optimizes , the factor graph of the nonlinear least squares problem is as follows Figure 3 As shown:

[0087] in , is in Projection is successful and in Image Pyramid The map point set on the layer image frame, the offset and gain coefficient of the photometric response function in the factor graph are collectively referred to as photometric parameters; the optimized ,in for The drone’s position during acquisition;

[0088] S4.4, if ,Will All map points on the keyframes are projected onto , to ensure that there are enough map points to participate in the optimization calculation and reconstruct the photometric residual; Use T distribution to fit the photometric residual distribution and obtain The luminosity distribution weights of the map points successfully projected on all key frames in the The image pyramid order, from the third layer to the first layer, solves the following nonlinear least squares problem for each layer of image frame, and gradually optimizes , the factor graph of the nonlinear least squares problem is as follows Figure 4 As shown:

[0089] in , yes The projection is successful on all key frames in the image pyramid The map point set on the layer image frame, yes The map point in yes The luminosity distribution weight, yes The photometric residual of .

[0090] S5. Use Gaussian distribution to describe the depth value of the candidate point. Perform block matching on the candidate points to update the standard deviation of the Gaussian distribution of the depth value ;definition and The moving distance of the UAV is , and The difference is ,right 、 and Perform weighted summation to obtain the tracking state value ,like , continue processing the next image frame according to steps S3-S5;

[0091] In this embodiment, the process of step S5 is as follows:

[0092] S5.1, use Gaussian distribution to describe the depth value of the candidate point, Perform block matching on the candidate points to update the standard deviation of the Gaussian distribution of the depth value ;

[0093] S5.2, yes 、 and The weighted summation is calculated according to the following formula :

[0094] , ,

[0095] in yes The translation component of yes The translation component of is in The map points in The average value of the upper depth value; generally let , ;

[0096] S5.3, if , continue processing the next image frame according to steps S3-S5.

[0097] S6, if ,Will Select as keyframe and insert ;Will All map points on the keyframes are projected onto Get the occupied pixel set and calculate The Euclidean distance between each pixel in the occupies pixel set and each pixel in the occupied pixel set is used to obtain a distance map;

[0098] S7, yes Perform gradient analysis based on distance graph to obtain Key frames with a common viewing relationship constitute a common viewing key frame set and use Update the distance map for all map points on key frames;

[0099] In this embodiment, the process of step S7 is as follows:

[0100] S7.1. For existing The key frames are divided according to the sliding window, and the frames that are not in the sliding window are used. The key frames form a historical key frame set, and the map points on the key frames in the historical key frame set are Projection to superior, After projection The pixel coordinates on , query from the distance map The distance value at ;

[0101] S7.2 Definitions All map points on the keyframes are projected onto The number on , the distance judgment threshold is calculated according to the following formula :

[0102] Among them, the general ;like ,but yes interior point on ; definition The gradient at the key frame is , The grayscale at the key frame is , exist The grayscale on , exist The gradient on ;like ,calculate The ratio of gradient to grayscale at the key frame and exist The ratio of the gradient to the grayscale If the following conditions are met, yes An interior point on yes External points on:

[0103] in yes The gain coefficient of the photometric response function of the key frame;

[0104] S7.3, according to the map point of the key frame The number of interior points on With external points Calculate the inlier ratio ;like , then the key frame and There is a co-viewing relationship between Select key frames with common viewing relationship The highest three key frames constitute the common view key frame set and use Update the distance map for all map points on keyframes within .

[0105] S8, If the candidate points on the inner key frame meet , then re-mark it as a map point and project it to Get pixel coordinates ; Query distance map The distance value ,like , then the map point is classified as a non-landmark map point; if , then the map point is classified as a landmark map point;

[0106] S9, continue to select Keyframes with co-viewing relationship and added , up to the number of landmark map points ;right and All keyframes and map points in the image are optimized based on photometric residuals. During the optimization process, the photometric residuals constructed by landmark map points are used to optimize both the UAV’s position and the depth of the map points, while the photometric residuals constructed by non-landmark map points are only used to optimize the depth of the map points. After optimization, the local motion trajectory of the UAV is obtained.

[0107] In this embodiment, the process of step S9 is as follows:

[0108] S9.1, continue to select Keyframes with co-viewing relationship and added , until and The number of landmark map points on the inner keyframe ;at this time The CCP has Frame keyframe, then ,in for Neidi Frame keyframe;

[0109] S9.2, and All map points on the keyframes are projected onto Construct the photometric residual on the map and store it in the structure of the map point; define and The set of key frames other than ;for If the number of light residuals stored in the structure of the landmark map point on the inner key frame is greater than 8, then delete Photometric residual constructed on the inner key frame; If the number of light residuals stored in the structure of the landmark map point on the inner key frame is greater than 15, then delete The photometric residual constructed on the inner keyframe reduces the scale of the nonlinear least squares problem;

[0110] S9.3, yes and Solve the following nonlinear least squares problem for all key frames and map points in the . The factor graph of the nonlinear least squares problem is as follows Figure 5 As shown:

[0111]

[0112] in for The collection of map points, for Map points within for The collection of map points, for Map points within yes The structure stored in Photometric residual, yes The T distribution weight, yes The structure stored in Photometric residual, yes The T distribution weight of The state vector contained is , yes Previous The depth value of each map point, The state vector contained is , yes Previous The depth value of each map point, for The drone’s position during collection, yes The gain coefficient of the photometric response function, yes The offset of the photometric response function is During the solution process, the photometric residual constructed by landmark map points is used to optimize the UAV's posture and the depth value of the map points at the same time, while the photometric residual constructed by non-landmark map points is only used to optimize the depth value of the map points; after the solution, the local motion trajectory of the UAV is obtained.

[0113] S10, looping through steps S3-S10 until all image frames in the image sequence are processed, and obtaining the global motion trajectory of the UAV;

[0114] S11. In this embodiment, Figure 6 and Figure 7 The direct visual inertial odometry trajectory estimation method with common view constraint is simulated in real time in Ubuntu.

[0115] This example compares the proposed method with publicly available direct-method visual odometry methods on the EuRoC dataset. The methods used for comparison are DSO (Direct Sparse Odometry) and DSM (Direct Sparse Mapping). This was performed on an AMD Ryzen 9 7900X CPU and 32GB of RAM desktop computer, running Ubuntu 20.04. The trajectory estimation results and per-frame image processing time of each method were evaluated in various indoor environments. The actual trajectory of the dataset was compared with the trajectory output by the algorithm, and the root mean square error (RMSE) of the absolute trajectory error was calculated as a measure of trajectory estimation accuracy. The per-frame image processing time was calculated by dividing the total processing time of the image sequence by the total number of images in the sequence. The results are as follows:

[0116] Table 1. RMSE and per-frame image processing schedule of each method in the EuRoC dataset

[0117]

[0118] The RMSE data of DSO in Table 1 are from the original paper, while the number of key frames of DSM is changed from 3 to 4 to facilitate comparison. The data of image processing time per frame of all algorithms are measured after turning off the point cloud display. It can be seen from the data in Table 1 that due to the addition of common view constraint, the trajectory estimation accuracy of the proposed method is higher than that of DSO in most sequences, and the average RMSE of all sequences is reduced by an order of magnitude. Figure 6 As shown, Figure 6 The middle left half is the point cloud map output by DSO. Due to the low accuracy of trajectory estimation, ghosting occurs in the dotted box of the point cloud map. The average image processing time per frame of all sequences of the present invention is slightly higher than that of DSO. This is mainly because the present invention uses the Ceres optimization library in order to optimize the flexibility of problem construction during program implementation, which results in the need to reconstruct the optimization matrix before each optimization. Compared with DSM, which also uses the Ceres optimization library, the average image processing time per frame of all sequences of the present invention is reduced by 38.05%. This is mainly because the present invention divides map points into two categories: non-landmark map points and landmark map points for management, which greatly improves the optimization efficiency. Under the premise of improved optimization efficiency, the average RMSE of all sequences of the present invention has only increased by 14.5% compared with DSM. In absolute terms, it has only increased by 0.01m, and still maintains good accuracy. In particular, in the V102, MH02 and MH03 sequences, the present invention has achieved higher accuracy. This is mainly due to a more accurate common-view constraint construction method and an adaptive direct pose optimization mechanism, such as Figure 7 As shown, Figure 7 The occluded map points in the common view keyframe in the middle right half are removed in the left image.

[0119] The technical features of the above embodiments can be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0120] The above embodiments are preferred implementation modes of the present invention, but the implementation modes of the present invention are not limited to the above embodiments. Any other changes, modifications, substitutions, combinations, and simplifications that do not deviate from the spirit and principles of the present invention should be considered as equivalent replacement methods and are included in the scope of protection of the present invention.

Claims

1. A visual UAV trajectory estimation method with common view constraint, characterized in that: The trajectory estimation method comprises the following steps: S1, let the drone move freely, collect images through the camera sensor on the drone, obtain an image sequence consisting of n frames, and record the timestamp of the image frame as , define the image frame with timestamp t as ; S2, the first image frame in the image sequence With the second frame image frame Initialize to get the initial motion of the drone. and All of them are selected as key frames, and the gradient points in the key frames are extracted. The gradient points are triangulated using the initial motion to estimate the depth value. The gradient points with accurate depth values ​​are marked as map points, and the gradient points with inaccurate depth values ​​are marked as candidate points. S3. Continue to read subsequent image frames according to the timestamp from small to large , used with The most recent timestamp Frame keyframes form a sliding window ,in is the jth key frame in the sliding window; The map points in the project are projected onto , calculate the proportion of map points that are successfully projected ; S4. If , constructed using the successfully projected map points and The photometric residual between them; the photometric response function is defined as: in is the original grayscale value of the pixel, is the true grayscale value of the pixel after correction, is the gain coefficient, is the offset, then The gain coefficient of the photometric response function is , The gain coefficient of the photometric response function is ;right and Perform optimization calculation based on photometric residual to obtain and timestamp as The initial pose of the UAV ;like ,Will All map points on the keyframes are projected onto , reconstruct the photometric residual, and Perform optimization calculation based on photometric residual to obtain and ; S5. Use Gaussian distribution to describe the depth value of the candidate point. Perform block matching on the candidate points to update the standard deviation of the Gaussian distribution of the depth value ;definition and The moving distance of the UAV is , and The difference is ,right 、 and Perform weighted summation to obtain the tracking state value ,like , continue processing the next image frame according to steps S3-S5; S6, if ,Will Select as keyframe and insert ;Will All map points on the keyframes are projected onto Get the occupied pixel set and calculate The Euclidean distance between each pixel in the occupies pixel set and each pixel in the occupied pixel set is used to obtain a distance map; S7, yes Perform gradient analysis based on distance graph to obtain Key frames with a common viewing relationship constitute a common viewing key frame set and use Update the distance map for all map points on key frames; S8, If the candidate points on the inner key frame meet , then re-mark it as a map point and project it to Get pixel coordinates ; Query distance map The distance value ,like , then the map point is classified as a non-landmark map point; if , then the map point is classified as a landmark map point; S9, continue to select Keyframes with co-viewing relationship and added , up to the number of landmark map points ;right and All keyframes and map points in the image are optimized based on photometric residuals. During the optimization process, the photometric residuals constructed by landmark map points are used to optimize both the UAV’s position and the depth of the map points, while the photometric residuals constructed by non-landmark map points are only used to optimize the depth of the map points. After optimization, the local motion trajectory of the UAV is obtained. S10, looping through steps S3-S10 until all image frames in the image sequence are processed, and obtaining the global motion trajectory of the UAV.

2. The method for visual UAV trajectory estimation with common view constraint according to claim 1, characterized in that: The process of step S3 is as follows: S3.

1. Continue reading subsequent image frames according to the timestamps from smallest to largest. , used with The most recent timestamp Frame keyframes form a sliding window ,in The first Frame keyframe; S3.2, assuming that the drone and If the motion is kept at a constant speed, then ,in for and The pose change matrix of the UAV, for and The UAV pose change matrix between and The pose change matrix of the UAV ,in for and The pose change matrix of the UAV in the Map point in ,in for exist The pixel coordinates on for exist The depth value on Projection to : in for Projection to The pixel coordinates on for After projection The depth value on is the camera intrinsic parameter matrix; when is not negative and exist Inside, it is believed Projection successful; S3.3, follow step S3.2 to Process all map points in the projected image to obtain the proportion of map points that are successfully projected. .

3. The method for visual UAV trajectory estimation with common view constraint according to claim 1, characterized in that: The process of step S4 is as follows: S4.

1. If ,right The photometric residual is constructed as follows: in express The photometric residual, for A set of surrounding pixels, for Pixels in for exist The gray value on for of The gray value on yes The offset of the photometric response function, yes The offset of the photometric response function, represents the Huber norm; yes The gradient weight of is a constant, represents the Euclidean norm; S4.2, yes Scale down by 2 times and 4 times, build The image pyramid of , the second layer is The third layer is 2 times smaller than 4 times smaller; S4.3, yes Use T distribution to fit the photometric residual distribution and obtain The luminosity distribution weight: in yes The luminosity distribution weight, is the degrees of freedom of the T distribution, is the scale of the T distribution; according to The image pyramid order is from the third layer to the first layer, and the following nonlinear least squares problem is solved for the image frame of each layer: in , is in Projection is successful and in Image Pyramid Map point set on the layer image frame; calculate the optimized ,in for The drone’s position during acquisition; S4.4, if ,Will All map points on the keyframes are projected onto , reconstruct the photometric residual; Use T distribution to fit the photometric residual distribution and obtain The luminosity distribution weights of the map points successfully projected on all key frames in the The image pyramid order is from the third layer to the first layer, and the following nonlinear least squares problem is solved for each layer of image frame: in , yes The projection is successful on all key frames in the image pyramid The map point set on the layer image frame, yes The map point in yes The luminosity distribution weight, yes The photometric residual of .

4. The method for visual UAV trajectory estimation with common view constraint according to claim 1, characterized in that: The process of step S5 is as follows: S5.1, use Gaussian distribution to describe the depth value of the candidate point, Perform block matching on the candidate points to update the standard deviation of the Gaussian distribution of the depth value ; S5.2, yes 、 and The weighted summation is calculated according to the following formula : in yes The translation component of yes The translation component of is in The map points in The average value of the upper depth value; let , ; S5.3, if , continue processing the next image frame according to steps S3-S5.

5. The method for visual UAV trajectory estimation with common view constraint according to claim 1, characterized in that: The process of step S7 is as follows: S7.

1. For existing The key frames are divided according to the sliding window, and the frames that are not in the sliding window are used. The key frames form a historical key frame set, and the map points on the key frames in the historical key frame set are Projection to superior, After projection The pixel coordinates on , query from the distance map The distance value at ; S7.2 Definitions All map points on the keyframes are projected onto The number on , the distance judgment threshold is calculated according to the following formula : Among them ;like ,but yes interior point on ; definition The gradient at the key frame is , The grayscale at the key frame is , exist The grayscale on , exist The gradient on ;like ,calculate The ratio of gradient to grayscale at the key frame and exist The ratio of the gradient to the grayscale If the following conditions are met, yes An interior point on yes External points on: in yes The gain coefficient of the photometric response function of the key frame; S7.3, according to the map point of the key frame The number of interior points on With external points Calculate the inlier ratio ;like , then the key frame and There is a co-viewing relationship between Select key frames with common viewing relationship The highest three key frames constitute the common view key frame set and use Update the distance map for all map points on keyframes within .

6. The method for visual UAV trajectory estimation with common view constraint according to claim 1, characterized in that: The process of step S9 is as follows: S9.1, continue to select Keyframes with co-viewing relationship and added , until and The number of landmark map points on the inner keyframe ;at this time The CCP has Frame keyframe, then ,in for Neidi Frame keyframe; S9.2, and All map points on the keyframes are projected onto Construct the photometric residual on the map and store it in the structure of the map point; define and The set of key frames other than ;for If the number of light residuals stored in the structure of the landmark map point on the inner key frame is greater than 8, then delete Photometric residual constructed on the inner key frame; If the number of light residuals stored in the structure of the landmark map point on the inner key frame is greater than 15, then delete Photometric residual constructed on the inner keyframe; S9.3, yes and Solve the following nonlinear least squares problem for all keyframes and map points: in for The collection of map points, for Map points within for The collection of map points, for Map points within yes The structure stored in Photometric residual, yes The T distribution weight, yes The structure stored in Photometric residual, yes The T distribution weight of The state vector contained is , yes Previous The depth value of each map point, The state vector contained is , yes Previous The depth value of each map point, for The drone’s position during collection, yes The gain coefficient of the photometric response function, yes The offset of the photometric response function is During the solution process, the photometric residual constructed by landmark map points is used to optimize the UAV's posture and the depth value of the map points at the same time, while the photometric residual constructed by non-landmark map points is only used to optimize the depth value of the map points; after the solution, the local motion trajectory of the UAV is obtained.

Images