Visual unmanned aerial vehicle trajectory estimation method with common-view constraint

Abstract

The invention discloses a visual unmanned aerial vehicle trajectory estimation method with common-view constraint, and the method comprises the steps: carrying out the dynamic switching between the pose estimation based on a nearest key frame map point and a sliding window map point according to the visibility of an image frame with a timestamp t for the nearest key frame map point, so as to obtain an initial pose; in the process, part of image frames are selected as key frames; then, carrying out gradient analysis on the newly selected key frames to obtain a common-view key frame set, and providing common-view constraints; a distance map is generated by using map points in the sliding window and the common-view key frame set, so that the map points are divided into road sign map points and non-road sign map points, and the optimization efficiency is improved; and finally, carrying out joint optimization on the key frames and map points in the sliding window and the common-view key frame set to obtain a final track. According to the trajectory estimation method provided by the invention, the processing speed of each frame of image is better under the condition that the precision is similar to that of similar algorithms.

Description

Technical Field

[0001] The present invention relates to the field of processing image data to construct point cloud maps and estimate trajectories, and in particular to a visual unmanned aerial vehicle trajectory estimation method with common view constraints. Background Art

[0002] Monocular visual odometry can be mainly divided into two categories: direct method and feature point method. Although the visual odometry based on the feature point method can provide more accurate trajectory estimation in most scenes, it will output wrong pose in scenes with missing image features such as motion blur and less environmental texture. In order to avoid mismatching, the feature point method only retains a few feature points, resulting in the output of sparse point cloud maps, which is not conducive to practical applications. The visual odometry based on the direct method can make full use of image texture information, reduce degradation, and output semi-dense point cloud maps. The direct method also uses real-time photometric parameter estimation to cope with photometric changes, making it close to practical applications. Compared with the visual odometry based on the feature point method, the front end of the visual odometry based on the direct method removes the feature matching stage, and the tracking time is shorter. However, the current visual odometry based on the direct method has the problem of long back-end optimization time, and the overall efficiency is not as good as the visual odometry based on the feature point method, which makes it less used in tasks with high real-time requirements.

[0003] In addition, the existing visual odometer still produces large cumulative errors when estimating long trajectories. Most solutions detect the similarity of the features of two images to detect historical scene recognition to eliminate the cumulative errors. However, this method will cause the jump phenomenon of posture in practical applications, which will cause confusion in the upper-level tasks in engineering implementation, reduce the overall stability and reliability of the system, and is not conducive to the smooth progress of 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 constraints.

[0005] The purpose of the present invention can be achieved by adopting the following technical solutions: A visual UAV trajectory estimation method with common view constraint, the trajectory estimation method comprising the following steps: S1, let the drone move freely, collect images through the camera sensor mounted 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 obtain the initial motion of the drone. and All are selected as key frames, the gradient points in the key frames are extracted, the gradient points are triangulated with 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 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 gray value of the pixel after correction, is the gain factor, 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 position of the drone ;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. Block matching is performed on 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 status value ,like , continue to process 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 and each pixel in the occupied pixel set is calculated to obtain a distance map; S7, yes Perform gradient analysis based on distance graph to obtain Key frames with a common view relationship constitute a common view 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 satisfy , 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 and Keyframes with co-viewing relationship are added , until the number of landmark map points ;right and All key frames 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 the posture of the UAV and the depth value of the map points, while the photometric residuals constructed by non-landmark map points are only used to optimize the depth value 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.

[0006] Furthermore, the process of step S3 is as follows: S3.1. 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 keyframes: Selecting keyframes based on timestamps not only speeds up processing, but also effectively exploits the high visual similarity between frames close in time, thus providing accurate visual constraints for the drone’s pose optimization. S3.2, Assume that the drone is 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 posture change matrix between and The pose change matrix of the UAV ,in for and The position change matrix of the UAV in Map point in ,in for exist The pixel coordinates on for exist The depth value on the Project to : in for Project to The pixel coordinates on for After projection The depth value on is the camera intrinsic parameter matrix; when is not negative and exist In, it is believed Projection was 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. .

[0007] Furthermore, the process of step S4 is as follows: S4.1. If ,right The photometric residual is constructed as follows: in express The photometric residual is for A set of surrounding pixels. for The 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 with using all the map points in the sliding window for each image frame, the computational overhead is reduced while maintaining good pose optimization accuracy; S4.2. Yes Reduce by 2 and 4 times and build The first layer of the image pyramid is , 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 is: 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, T distribution is used to fit the distribution of photometric residuals 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 each layer of image frames: 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 collection; 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 frames: 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 and The movement between the two is intense, and only by being in The map points in cannot provide effective visual constraints; at this time, the credibility of the drone 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.

[0008] Furthermore, the process of step S5 is as follows: S5.1. Use Gaussian distribution to describe the depth value of the candidate point. Block matching is performed on 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 projection are The average value of the upper depth value; generally let , ;pass To select key frames, the complexity of trajectory optimization calculation is reduced 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 drone should maintain an appropriate movement distance between two temporally adjacent key frames, which will be achieved through right have an impact In addition, there should be enough overlap between two temporally adjacent key frames to ensure the accuracy of trajectory estimation. The smaller the overlap, the better. The smaller, the Finally, the keyframes should be able to reflect important lighting changes in the environment to enhance the robustness of trajectory estimation in dynamic scenes. The larger the ; S5.3 If , continue to process the next image frame according to steps S3-S5.

[0009] Furthermore, 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 not in the sliding window are used. The key frames of the frames form a historical key frame set, and the map points on the key frames in the historical key frame set are Project to superior, After projection The pixel coordinates on , from the distance map, The distance value at ; S7.2. Definitions All map points on the keyframes are projected to The number on , the distance judgment threshold is calculated according to the following formula : Among them, ;like ,but yes Interior point on 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 The interior point on yes Outer points on: 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; 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 map, indicating that the map point is an inner 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 region with larger gradient in the inner key frame has already extracted gradient points but is still a candidate point; due to the visual similarity between image frames with similar time, the region without map point projection is in It is also the area with larger gradient; if the key frame is The difference between the gradient and grayscale ratio is small, indicating that the edge of the same object reappears, so the map point is an interior point; second, In the region with smaller gradient in the inner keyframe, the map points cannot be extracted; due to the visual similarity between image frames with similar time, the region without map point projection is The gradient is also small 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 additional optimization time. Introducing This is to appropriately relax the threshold when there are fewer map points on the keyframe in the sliding window, so that more map points can participate in trajectory optimization; S7.3, according to the map point of the key frame The number of interior points on With external points Calculate the interior point ratio ;like , then the key frame and There is a co-viewing relationship between Select from key frames with common view relationship The highest three key frames form the common view key frame set , and use Update the distance map for all map points on keyframes within the There are at least three key frames in the frame to provide sufficient common-view constraints. Inner keyframe and The photometric consistency can effectively reduce 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, which causes the final estimated trajectory to deviate from the true path.

[0010] Furthermore, the process of step S9 is as follows: S9.1. Continue to select and Keyframes with co-viewing relationship are added , until and The number of landmark map points on the inner keyframe ;at this time The Chinese Communist Party 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 value, resulting in an insufficient number of landmark map points; in order to avoid the reduction of trajectory estimation accuracy due to the insufficient number of landmark map points, more keyframes should be selected to ensure that there are enough landmark map points involved in the optimization; 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 For landmark map points on the inner keyframe, if the number of photometric residuals stored in the structure is greater than 8, delete The photometric residual constructed on the inner key frame; For landmark map points on the inner keyframe, if the number of photometric residuals stored in the structure is greater than 15, delete The 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, thereby affecting the final trajectory accuracy, so it should be deleted appropriately; 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 accurate environment description for the pose optimization of subsequent image frames; S9.3, Yes and Solve the following nonlinear least squares problem for all key frames and map points: in for The collection of map points on the for Map points within for The collection of map points on the for Map points within yes The structure stored in The photometric residual, yes The weight of the T distribution, yes The structure stored in The 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.

[0011] In order to optimize the UAV's posture and map point depth values, it is necessary to calculate the Jacobian matrix of the photometric residual for the UAV's posture and the map point depth values; since the Jacobian matrix of the photometric residual for the UAV's posture is large in dimension, the amount of calculation increases significantly; in trajectory estimation, map points can ensure accuracy by effectively describing the edges of objects, while over-description will consume a lot of computing resources and have limited effect; therefore, for map points that are too close, that is, non-landmark map points, it is only necessary to calculate the Jacobian matrix of the photometric residual for the depth value of the map point to reduce the computational burden.

[0012] Compared with the prior art, the present invention has the following advantages and effects: (1) In order to solve the problem that the existing direct-method visual odometry backend needs to optimize many map points, which results in a long optimization time, the present invention proposes a map point classification method based on a distance graph. This method divides map points into two categories: non-landmark map points and landmark map points for management, eliminates the calculation of the Jacobian matrix of the photometric residual for non-landmark map points, and reduces the optimization time.

[0013] (2) In order to solve the problem of cumulative error and pose jump in trajectory estimation, the present invention 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 decrease in trajectory estimation accuracy caused by visual degradation, and eliminate the long-term cumulative error of trajectory estimation. Experimental results show that the present invention greatly improves the trajectory estimation speed in complex scenes while still maintaining a high trajectory estimation accuracy, ensuring the stability and reliability of odometer trajectory estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the description of the embodiments will be briefly introduced below. 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 creative work.

[0015] Figure 1 It is a flow chart of a visual UAV trajectory estimation method with common-view constraint disclosed by the present invention; Figure 2 It is a pixel arrangement diagram around a map point in the present invention; Figure 3 It is the factor graph optimized based on the pose of the nearest key frame map point in the present invention; Figure 4 It is a factor graph based on the sliding window map point pose optimization in the present invention; Figure 5 is a local optimization factor graph of the co-viewing key frame set and the sliding window in the present invention; Figure 6 This is a point cloud comparison diagram of a visual UAV trajectory estimation method with common view constraints in an indoor motion scene in the present invention; Figure 7 It is a common view map point comparison diagram of a visual UAV trajectory estimation method with common view constraint disclosed by the present invention. DETAILED DESCRIPTION

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

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

[0018] Example like Figure 1 As shown, this embodiment specifically discloses a visual drone trajectory estimation method with common view constraint, and the trajectory estimation method includes the following steps: S1, let the drone move freely, collect images through the camera sensor mounted on the drone, and obtain The image sequence is composed of image frames, and the timestamp of the image frame is recorded as , define the timestamp as The image frame is ; S2, the first image frame in the image sequence With the second frame image frame Initialize to obtain the initial motion of the drone. and All are selected as key frames, the gradient points in the key frames are extracted, the gradient points are triangulated with 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 The first Frame keyframe; will be in The map points in are projected onto , calculate the proportion of map points that are successfully projected ; In this embodiment, the process of step S3 is as follows: S3.1. 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; S3.2, Assume that the drone is 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 posture change matrix between and The pose change matrix of the UAV ,in for and The position change matrix of the UAV in Map point in ,in for exist The pixel coordinates on for exist The depth value on the Project to : in for Project to The pixel coordinates on for After projection The depth value on is the camera intrinsic parameter matrix; when is not negative and exist In, it is believed Projection was successful; 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. .

[0019] 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 gray value of the pixel after correction, is the gain factor, 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 position of the drone ;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 ; In this embodiment, the process of step S4 is as follows: S4.1. If ,right The photometric residual is constructed as follows: in express The photometric residual is for Around Figure 2The set of pixels shown is composed of for The 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 Reduce by 2 and 4 times and build The first layer of the image pyramid is , 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 is: 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 loss caused by map points with large photometric residuals; according to The image pyramid sequence is from the third layer to the first layer. The following nonlinear least squares problem is solved for each layer of the image frame, and the optimization is gradually carried out. , the factor graph of the nonlinear least squares problem is as follows Figure 3 As shown: 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 are collectively referred to as photometric parameters in the factor graph; the optimized ,in for The drone’s position during collection; 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 is from the third layer to the first layer. The following nonlinear least squares problem is solved for each layer of image frames, and the optimization is gradually carried out. , the factor graph of the nonlinear least squares problem is as follows Figure 4 As shown: 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 .

[0020] S5. Use Gaussian distribution to describe the depth value of the candidate point. Block matching is performed on 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 status value ,like , continue to process the next image frame according to steps S3-S5; In this embodiment, the process of step S5 is as follows: S5.1. Use Gaussian distribution to describe the depth value of the candidate point. Block matching is performed on 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 projection are The average value of the upper depth value; generally let , ; S5.3 If , continue to process the next image frame according to steps S3-S5.

[0021] 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 and each pixel in the occupied pixel set is calculated to obtain a distance map; S7, yes Perform gradient analysis based on distance graph to obtain Key frames with a common view relationship constitute a common view key frame set , and use Update the distance map for all map points on key frames; In this embodiment, 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 not in the sliding window are used. The key frames of the frames form a historical key frame set, and the map points on the key frames in the historical key frame set are Project to superior, After projection The pixel coordinates on , from the distance map, The distance value at ; S7.2. Definitions All map points on the keyframes are projected to The number on , the distance judgment threshold is calculated according to the following formula : Among them, the general order ;like ,but yes Interior point on 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 The interior point on yes Outer 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 interior point ratio ;like , then the key frame and There is a co-viewing relationship between Select from key frames with common view relationship The highest three key frames form the common view key frame set , and use Update the distance map for all map points on keyframes within.

[0022] S8, If the candidate points on the inner key frame satisfy , 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 and Keyframes with co-viewing relationship are added , until the number of landmark map points ;right and All key frames 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 the posture of the UAV and the depth value of the map points, while the photometric residuals constructed by non-landmark map points are only used to optimize the depth value of the map points. After optimization, the local motion trajectory of the UAV is obtained. In this embodiment, the process of step S9 is as follows: S9.1. Continue to select and Keyframes with co-viewing relationship are added , until and The number of landmark map points on the inner keyframe ;at this time The Chinese Communist Party 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 For landmark map points on the inner keyframe, if the number of photometric residuals stored in the structure is greater than 8, delete The photometric residual constructed on the inner key frame; For landmark map points on the inner keyframe, if the number of photometric residuals stored in the structure is greater than 15, delete The photometric residual constructed on the inner keyframe reduces the size of the nonlinear least squares problem; 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: in for The collection of map points on the for Map points within for The collection of map points on the for Map points within yes The structure stored in The photometric residual, yes The T distribution weight, yes The structure stored in The 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.

[0023] 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; S11. In this embodiment, Figure 6 and Figure 7 The direct visual inertial odometer trajectory estimation method with common view constraint is simulated in real time in Ubuntu; This example compares the proposed method with the public direct-based visual odometer method in the EuRoC dataset. The methods used for comparison are DSO (Direct Sparse Odometry) and DSM (Direct Sparse Mapping). The test was conducted on a desktop computer with an AMD Ryzen 9 7900X CPU and 32GB memory, using Ubuntu 20.04. The trajectory estimation results and image processing time per frame of each method in various indoor environments were evaluated. 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 obtained. RMSE was used as an indicator to measure the accuracy of trajectory estimation. The image processing time per frame was obtained by dividing the total processing time of the image sequence by the total number of images in the sequence. The results are as follows: Table 1. RMSE of each method and image processing schedule for each frame in the EuRoC dataset The RMSE data of DSO in Table 1 comes from the original paper, while the number of key frames close to the current key frame time of DSM is changed from 3 to 4 for the convenience of 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 the common view constraint, the trajectory estimation accuracy of the present invention is higher than that of DSO in most sequences, and the average RMSE of all sequences is reduced by one order of magnitude, such as Figure 6 As shown, Figure 6 The middle left half is the point cloud image output by DSO. Due to the low accuracy of trajectory estimation, ghosting occurs in the dotted box of the point cloud image. 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, and has only increased by 0.01m in absolute value, while still maintaining good accuracy. In particular, the present invention has achieved higher accuracy in the V102, MH02 and MH03 sequences. 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.

[0024] The technical features of the above embodiments may 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.

[0025] 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 equivalent replacement methods and are included in the protection scope 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 mounted 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 obtain the initial motion of the drone. and All are selected as key frames, the gradient points in the key frames are extracted, the gradient points are triangulated with 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 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 gray value of the pixel after correction, is the gain factor, 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 position of the drone ;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. Block matching is performed on 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 status value ,like , continue to process 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 and each pixel in the occupied pixel set is calculated to obtain a distance map; S7, yes Perform gradient analysis based on distance graph to obtain Key frames with a common view relationship constitute a common view 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 satisfy , 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 and Keyframes with co-viewing relationship are added , until the number of landmark map points ;right and All key frames 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 the posture of the UAV and the depth value of the map points, while the photometric residuals constructed by non-landmark map points are only used to optimize the depth value 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. A visual UAV trajectory estimation method with common view constraint according to claim 1, characterized in that: The process of step S3 is as follows: S3.

1. 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; S3.2, Assume that the drone is 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 posture change matrix between and The pose change matrix of the UAV ,in for and The posture change matrix of the UAV in the Map point in ,in for exist The pixel coordinates on for exist The depth value on the Project to : in for Project to The pixel coordinates on for After projection The depth value on is the camera intrinsic parameter matrix; when is not negative and exist In, it is believed that Projection was 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 visual UAV trajectory estimation method 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 is for A set of surrounding pixels. for The 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 Reduce by 2 and 4 times and build The first layer of the image pyramid is , 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 is: 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 each layer of image frames: 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 frames: 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. Block matching is performed on 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 projection are The average value of the upper depth value; , ; S5.3 If , continue to process the next image frame according to steps S3-S5.

5. The visual UAV trajectory estimation method 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 not in the sliding window are used. The key frames of the frames form a historical key frame set, and the map points on the key frames in the historical key frame set are Project to superior, After projection The pixel coordinates on , from the distance map, The distance value at ; S7.

2. Definitions All map points on the keyframes are projected to 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 keyframe and exist The ratio of the gradient to the grayscale If the following conditions are met, yes The interior point on yes Outer 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 interior point ratio ;like , then the key frame and There is a co-viewing relationship between Select from key frames with common view relationship The highest three key frames form the common view key frame set , and use Update the distance map for all map points on keyframes within.

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

1. Continue to select and Keyframes with co-viewing relationship are added , until and The number of landmark map points on the inner keyframe ;at this time The Chinese Communist Party 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 For landmark map points on the inner keyframe, if the number of photometric residuals stored in the structure is greater than 8, delete The photometric residual constructed on the inner key frame; For landmark map points on the inner keyframe, if the number of photometric residuals stored in the structure is greater than 15, delete Photometric residual constructed on the inner keyframe; S9.3, Yes and Solve the following nonlinear least squares problem for all key frames and map points: in for The collection of map points on the for Map points within for The collection of map points on the for Map points within yes The structure stored in The photometric residual, yes The weight of the T distribution, yes The structure stored in The photometric residual, yes The weight of the T distribution; definition 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