A camera-based motion pose estimation method and apparatus

Abstract

The application is suitable for the technical field of computer vision, and provides a camera-based motion posture estimation method and device, which comprises: acquiring a previous frame image and a current frame image collected by a camera. According to an effective depth point set of a depth map in the previous frame image and an effective depth point set of a depth map in the current frame image, a depth motion point proportion value in the current frame image is calculated. According to a first feature point of an intensity map in the previous frame image and a second feature point of an intensity map in the current frame image, a feature motion point proportion value in the current frame image is calculated. The depth motion point proportion value and the feature motion point proportion value are used to determine a current frame motion state type of the camera. According to the current frame motion state type of the camera, a current frame motion posture of the camera is estimated. The motion state of the camera can be accurately distinguished, the motion posture estimation algorithm is adaptively determined according to the motion state of the camera, the real-time tracking response speed is improved, and the global accuracy of the motion posture of the camera is ensured.

Description

Technical Field

[0001] This application belongs to the field of computer vision technology, and in particular relates to a camera-based motion pose estimation method and apparatus. Background Technology

[0002] With the popularization of 3D security monitoring technology, protective cameras equipped with depth sensors are being deployed in large numbers. They need to continuously output their six degrees of freedom attitude to build a unified three-dimensional monitoring coordinate system to achieve warning area calibration, motion object trajectory reconstruction, and post-event traceability and location.

[0003] However, in actual monitoring scenarios, there are often complex conditions such as weak texture walls, dynamic interference targets such as pedestrians and vehicles, continuous long-term shooting for several hours to several days, and intermittent start and stop of camera movement. These conditions make it difficult for existing motion pose estimation algorithms to adapt to the real-time and global accuracy requirements of tracking response speed. Summary of the Invention

[0004] This application provides a camera-based motion attitude estimation method and apparatus, which can solve the technical problem that existing camera attitude estimation processes cannot simultaneously achieve real-time tracking response speed and global accuracy due to the lack of differentiation between the camera's motion state.

[0005] In a first aspect, embodiments of this application provide a camera-based motion pose estimation method, including: Acquire the previous frame image and the current frame image captured by the camera, wherein the previous frame image and the current frame image respectively contain a pair of depth map and intensity map; The percentage of depth motion points in the current frame image is calculated based on the effective depth point set of the depth map in the previous frame image and the effective depth point set of the depth map in the current frame image. Based on the first feature point of the intensity map extracted from the previous frame image and the second feature point of the intensity map in the current frame image, the proportion of feature motion points in the current frame image is calculated; wherein, the proportion of depth motion points and the proportion of feature motion points are used to determine the motion state type of the camera in the current frame. The current frame motion pose of the camera is estimated based on the current frame motion state type of the camera.

[0006] In one possible implementation of the first aspect, the motion state type includes dynamic working condition, short-term static working condition, and motion stop working condition; The step of estimating the current frame motion pose of the camera based on the current frame motion state type includes: If the motion state type of the current frame is the dynamic working condition, the initial motion posture of the current frame corresponding to the current frame image predicted by the pose prediction model is used as the initial value for optimization. Sliding window bundle adjustment optimization is performed on multiple consecutive frames including the current frame image to obtain the motion posture of the current frame corresponding to the current frame image. The pose prediction model is a kinematic model for predicting the motion posture of the next frame based on three frames of historical motion posture. If the motion state type of the current frame is the short-term static condition, then the motion posture of the previous frame corresponding to the previous frame image is taken as the motion posture of the current frame. If the motion state type of the current frame is the motion stop condition, then based on all frame images during the entire camera motion process and the motion postures of all frames corresponding to all frame images, a global iterative optimization is performed to obtain the motion posture of the current frame.

[0007] In one possible implementation of the first aspect, determining the current frame motion state type of the camera based on the depth motion point ratio and the feature motion point ratio includes: When the percentage of depth motion points is greater than or equal to a preset depth threshold, and / or the percentage of feature motion points is greater than or equal to a preset feature threshold, the current frame image is determined to be a moving image, and the current frame motion state type of the camera is determined to be the dynamic condition. When the percentage of depth motion points is less than the preset depth threshold and the percentage of feature motion points is less than the preset feature threshold, the current frame image is determined to be a still image. When the cumulative frame value of the still image is less than the preset frame value, the current operating state type of the camera is determined to be the short-term still condition. When the cumulative frame value of the still image is greater than or equal to the preset frame value, the current operating state type of the camera is determined to be the motion-stopped condition.

[0008] In one possible implementation of the first aspect, calculating the proportion of depth motion points in the current frame image based on the effective depth point set of the depth map in the previous frame image and the effective depth point set of the depth map in the current frame image includes: The effective depth point set in the previous frame image is compared with the effective depth point set in the current frame image, and the depth difference between the effective depth point set in the previous frame image and the effective depth point set in the current frame image is calculated point by point. The number of depth points whose depth difference is greater than a preset depth difference threshold is counted to obtain the number of valid depth points; The depth motion point ratio is calculated based on the ratio of the number of effective depth points to the total number of depth points in the effective depth point set of the current frame image.

[0009] In one possible implementation of the first aspect, the step of calculating the proportion of feature motion points in the current frame image based on the extracted first feature points of the intensity map in the previous frame image and the second feature points of the intensity map in the current frame image includes: The first feature point and the second feature point are matched using a feature optical flow algorithm to obtain a matching point pair, and the displacement between the matching point pairs is calculated. The number of motion feature points whose displacement is greater than a preset displacement threshold is counted to obtain the number of effective motion feature points. The percentage of the feature motion points is calculated based on the ratio of the number of effective motion feature points to the total number of matching point pairs.

[0010] In one possible implementation of the first aspect, the pose prediction model is a second-order uniformly accelerated motion model; The process of predicting the initial motion pose of the current frame image corresponding to the current frame image through the pose prediction model includes: Extract three frames of the camera's historical motion pose; The equivalent acceleration of the camera is calculated based on the three historical motion postures. Based on the principle of short-time uniform acceleration motion, the Lie algebra increment of the current frame corresponding to the motion posture of the current frame is predicted according to the equivalent acceleration. Based on the state transition equation constructed using the current frame Lie algebra increment and the left Jacobian matrix, the predicted motion attitude value for the current frame is obtained. The initial motion attitude of the current frame is obtained by performing extended Kalman filter iterative estimation based on the current frame motion attitude prediction value, observation equation and preset attitude increment damping constraint; wherein, the observation equation is obtained by fusing the point-to-plane geometric residual, the plane normal vector angle residual and the feature point reprojection residual.

[0011] In one possible implementation of the first aspect, calculating the equivalent acceleration of the camera based on the three historical motion poses includes: Based on the three historical motion poses, calculate the Lie group transform increment between adjacent frames; The Lie group transformation increment is converted into a Lie algebra increment through a logarithmic mapping. The equivalent acceleration of the camera is obtained by performing second-order difference calculation on the Lie algebra increment.

[0012] In one possible implementation of the first aspect, if the motion state type of the current frame is the dynamic condition, the initial motion posture of the current frame corresponding to the current frame image predicted by the pose prediction model is used as the initial value for optimization, and sliding window bundle adjustment optimization is performed on multiple consecutive frames including the current frame image to obtain the motion posture of the current frame corresponding to the current frame image, including: Using multiple consecutive frames, including the current frame image, as a sliding window, an objective function is constructed that includes the point-to-plane geometric residual, the plane normal vector angle residual, and the feature point reprojection residual. Based on the scene texture and the number of planes, the residual weights corresponding to the point-to-plane geometric residual, the plane normal vector angle residual, and the feature point reprojection residual are adaptively assigned to obtain the optimization model to be solved. Based on the initial motion posture of the current frame, the variables to be optimized within the sliding window are initialized to obtain the optimized initial solution for the variables to be optimized; Based on the optimized initial solution, the variable values ​​after each iteration are obtained by using the Gauss-Newton method iterative calculation and the Schul elimination operation. When the preset iteration convergence condition is met, the iteration calculation is terminated, and the motion posture of the current frame is obtained.

[0013] In one possible implementation of the first aspect, if the motion state type of the current frame is the motion stop condition, then based on all frame images during the entire camera motion process and all frame motion poses corresponding to all frame images, a global iterative optimization is performed to obtain the motion pose of the current frame, including: Based on the local frame stack length, all frame image sequences are divided into multiple local frame stacks, wherein the local frame stack length is obtained as a percentage of the total number of frames accumulated throughout the entire camera motion process; Based on the motion posture of each frame within each local frame stack, the objective function is used to perform local sliding window beam adjustment optimization to obtain the local optimized posture, and the proportion of non-zero elements and the average squared residual of each frame's Shure elimination matrix are updated. Based on the proportion of non-zero elements in the Shure elimination matrix corresponding to the motion posture of each frame in the local frame stack and the average squared residual, the global keyframe set in the local frame stack is determined. Based on the global keyframe set, the global optimal keyframe pose is obtained by solving the incremental global sliding window beam adjustment optimization model. The globally optimal keyframe pose is written back to the corresponding local frame stack. The objective function is reconstructed by distinguishing and setting keyframe weights and ordinary frame weights to obtain the updated objective function. Based on the updated objective function, the local sliding window beam adjustment optimization process is re-executed to obtain the corrected local optimized pose. Based on the globally optimal keyframe pose and the corrected locally optimized pose, the optimized motion pose is obtained, and the current frame motion pose is updated with the optimized motion pose.

[0014] Secondly, embodiments of this application provide a camera-based motion pose estimation device, comprising: The acquisition module is used to acquire the previous frame image and the current frame image captured by the camera, wherein the previous frame image and the current frame image respectively contain a pair of depth map and intensity map; The depth ratio determination module is used to calculate the proportion of depth motion points in the current frame image based on the effective depth point set of the depth map in the previous frame image and the effective depth point set of the depth map in the current frame image. The feature ratio determination module is used to calculate the feature motion point ratio in the current frame image based on the first feature point of the intensity map extracted from the previous frame image and the second feature point of the intensity map in the current frame image; wherein, the depth motion point ratio and the feature motion point ratio are used to determine the current frame motion state type of the camera; The estimation module is used to estimate the current frame motion pose of the camera based on the current frame motion state type of the camera.

[0015] Thirdly, embodiments of this application provide a terminal device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the camera-based motion pose estimation method described in any of the above claims.

[0016] Fourthly, embodiments of this application provide a computer-readable storage medium storing a computer program that, when executed by a processor, implements the camera-based motion pose estimation method described in any of the preceding claims.

[0017] Fifthly, embodiments of this application provide a computer program product that, when run on a terminal device, causes the terminal device to execute the camera-based motion pose estimation method described in any one of the first aspects.

[0018] The beneficial effects of the embodiments in this application compared with the prior art are: This application provides a camera-based motion pose estimation method. The method includes: first, acquiring a previous frame image and a current frame image captured by the camera, wherein the previous frame image and the current frame image each contain a pair of depth maps and intensity maps. Then, based on the effective depth point set of the depth map in the previous frame image and the effective depth point set of the depth map in the current frame image, calculating the proportion of depth motion points in the current frame image; and based on the extracted first feature point of the intensity map in the previous frame image and the second feature point of the intensity map in the current frame image, calculating the proportion of feature motion points in the current frame image, wherein the proportion of depth motion points and the proportion of feature motion points are used to determine the current frame motion state type of the camera. Finally, based on the current frame motion state type of the camera, estimating the current frame motion pose. This method can accurately distinguish the camera's motion state, and the adaptive motion pose estimation algorithm based on different camera motion states can improve real-time tracking response speed while ensuring the global accuracy of the camera's motion pose. Attached Figure Description

[0019] To more clearly illustrate the technical solutions in the embodiments of this application, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0020] Figure 1 This is a schematic flowchart of a camera-based motion pose estimation method provided in an embodiment of this application; Figure 2 This is a schematic diagram of the structure of a camera-based motion pose estimation device according to an embodiment of this application; Figure 3 This is a schematic diagram of the structure of a terminal device provided in an embodiment of this application. Detailed Implementation

[0021] In the following description, specific details such as particular system architectures and techniques are set forth for illustrative purposes and not for limitation, in order to provide a thorough understanding of the embodiments of this application. However, those skilled in the art will understand that this application may also be implemented in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, apparatuses, circuits, and methods have been omitted so as not to obscure the description of this application with unnecessary detail.

[0022] It should be understood that, when used in this application specification and the appended claims, the term "comprising" indicates the presence of the described features, integrals, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or a collection thereof.

[0023] It should also be understood that the term “and / or” as used in this application specification and the appended claims means any combination of one or more of the associated listed items and all possible combinations, and includes such combinations.

[0024] As used in this application specification and the appended claims, the term "if" may be interpreted, depending on the context, as "when," "once," "in response to determination," or "in response to detection." Similarly, the phrase "if determined" or "if detected [the described condition or event]" may be interpreted, depending on the context, as meaning "once determined," "in response to determination," "once detected [the described condition or event]," or "in response to detection [the described condition or event]."

[0025] Furthermore, in the description of this application and the appended claims, the terms "first," "second," "third," etc., are used only to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0026] References to "one embodiment" or "some embodiments" as described in this specification mean that one or more embodiments of this application include a specific feature, structure, or characteristic described in connection with that embodiment. Therefore, the phrases "in one embodiment," "in some embodiments," "in other embodiments," "in still other embodiments," etc., appearing in different parts of this specification do not necessarily refer to the same embodiment, but rather mean "one or more, but not all, embodiments," unless otherwise specifically emphasized. The terms "comprising," "including," "having," and variations thereof mean "including but not limited to," unless otherwise specifically emphasized.

[0027] With the popularization of 3D security monitoring technology, protective cameras equipped with depth sensors are being deployed in large numbers. They need to continuously output their six degrees of freedom attitude to build a unified three-dimensional monitoring coordinate system to achieve warning area calibration, motion object trajectory reconstruction, and post-event traceability and location.

[0028] The attitude estimation of security cameras needs to meet both real-time and final accuracy requirements. However, in actual monitoring situations, the scene generally has complex conditions such as weak texture walls, dynamic interference targets such as pedestrians and vehicles, continuous shooting over long periods of time for several hours to several days, and intermittent start and stop of camera movement. This makes it difficult for existing motion attitude estimation algorithms to adapt to the real-time and global accuracy requirements of tracking response speed. Existing motion attitude estimation algorithms have the following technical defects: (1) They do not distinguish between stationary and moving camera states, and repeatedly optimize stationary images, resulting in wasted computing power and affecting real-time tracking response speed. (2) Nonlinear optimization (such as BA) relies on random initialization or simple motion models for initial values, which is prone to getting trapped in local optima and has a slow convergence speed, and cannot meet the real-time requirements of monitoring scenarios. (3) They rely only on local sliding window optimization, and the accumulated error increases continuously during real-time tracking, resulting in the offset of the three-dimensional monitoring area and affecting the accuracy of security protection. (4) They lack a global calibration mechanism after the movement stops, resulting in insufficient final attitude accuracy and failing to meet the high-precision positioning requirements of monitoring playback and event tracing. (5) Covariance estimation lacks physical truth support and cannot guarantee positive definiteness, which can easily lead to filter collapse and interruption of real-time tracking. (6) Global optimization does not combine local frame quality screening, resulting in low optimization efficiency and insufficient robustness, and is not suitable for long-term data processing in monitoring scenarios. (7) Traditional attitude optimization only uses a single optimization objective, which is not robust enough when used in different scenarios, and the attitude solution is highly ambiguous.

[0029] Therefore, to address the aforementioned shortcomings, this application proposes a camera-based dynamic attitude estimation method that balances real-time tracking response speed, 3D monitoring area stability, final attitude accuracy, and positive definite covariance estimation. This application employs a three-level closed-loop architecture: motion detection preprocessing, real-time EKF initialization and three-frame sliding window BA optimization, and offline global-local iterative optimization. The architecture mainly includes: the first two levels (motion detection and EKF-BA cascade), i.e., the real-time computation module, used for real-time attitude tracking during camera movement, supporting real-time delineation and dynamic adjustment of the 3D monitoring area; and the last level (offline global-local iterative optimization), i.e., the offline calibration module, used for final attitude global optimization after camera movement stops (e.g., when the monitoring camera is fixed or moving monitoring ends), ensuring final positioning accuracy.

[0030] Please see Figure 1 , Figure 1 This is a schematic flowchart illustrating a camera-based motion pose estimation method according to an embodiment of this application. As an example and not a limitation, this method can be applied to or run in terminal devices such as cameras, and the method includes: S11. Acquire the previous frame image and the current frame image captured by the camera, wherein the previous frame image and the current frame image respectively contain a pair of depth map and intensity map.

[0031] S12. Based on the effective depth point set of the depth map in the previous frame image and the effective depth point set of the depth map in the current frame image, calculate the proportion of depth motion points in the current frame image.

[0032] S13. Based on the first feature point of the intensity map in the previous frame image and the second feature point of the intensity map in the current frame image, calculate the proportion of feature motion points in the current frame image; wherein, the proportion of depth motion points and the proportion of feature motion points are used to determine the motion state type of the camera in the current frame.

[0033] S14. Estimate the current frame motion pose of the camera based on the current frame motion state type of the camera.

[0034] The current frame image refers to the latest set of image data that is currently being processed. The previous frame image refers to the set of image data that was acquired earlier than the current frame and immediately followed by the previous output. The current frame image and the previous frame image are temporally adjacent frames, used to compare changes between frames and determine whether the camera has moved or rotated.

[0035] The camera can be a color-depth camera (structured light, ToF depth camera, or RGBD depth camera). Each exposure simultaneously outputs two matching images (depth map and intensity map), with a one-to-one pixel correspondence (paired). This means that images are captured at the same time, pixels are strictly registered, and the same pixel coordinate simultaneously contains both reflection intensity information and spatial distance information. The intensity map is a two-dimensional image reflecting the optical reflection intensity of the target surface. In this embodiment, the intensity map can be an RGB image or an infrared intensity image. RGB images can characterize the reflection intensity in the visible light band, while infrared intensity images can characterize the reflection intensity in the near-infrared band. The pixel grayscale values ​​in the intensity map can represent the strength of light wave reflection at that location, used to extract visual features such as texture, corners, and edges. The depth map and intensity map have the same resolution and are pixel-aligned. Each pixel in the depth map stores a depth value, which can represent the straight-line distance from the corresponding spatial point to the camera's optical center, directly reconstructing the scene's three-dimensional spatial coordinates.

[0036] Depth motion points can be understood as pixels whose depth value changes significantly at the same pixel location compared to the depth map of the previous frame and the depth map of the current frame. Depth motion points can be generated due to the following two situations: (1) the camera itself is translated or rotated; (2) objects in the scene move or are occluded. The existence of motion is determined by the depth three-dimensional distance. The depth motion point ratio can be understood as the ratio of the total number of pixels with depth changes between frames to the total number of pixels in the image. The depth motion point ratio is a proportional value between 0 and 1. The depth motion point ratio can quantify the proportion of pixels that move at the depth level in the entire image, reflecting the overall change in the three-dimensional space level.

[0037] Feature motion points can be understood as visual feature points (such as corner points, SIFT / ORB feature points) extracted based on intensity maps. These feature points are then compared with those in the previous and current frames using optical flow matching or feature matching, and their positions shift significantly after matching. Motion is determined by the displacement of the 2D image texture features. The feature motion point ratio can be understood as the ratio of the number of effectively matched feature points with displacement between frames to the total number of successfully matched feature points in the two frames. This feature motion point ratio is a proportion ranging from 0 to 1. This feature motion point ratio quantifies the overall shift of the 2D image texture features, characterizing the amplitude of the 2D texture motion in the image.

[0038] The effective depth point set refers to the set of pixels in the depth map with valid depth values. Invalid pixels (occlusion, over-range, sensor noise holes, loss of reflection causing empty or abnormal depth values) are removed, and only pixels that can truly reflect the three-dimensional distance of the scene are retained for subsequent inter-frame three-dimensional matching operations.

[0039] The first feature point refers to a stable visual feature point (ORB, corner point, or SIFT, etc.) extracted from the intensity map of the previous frame, carrying pixel coordinates and feature descriptors, serving as a reference for 2D texture matching. The second feature point refers to a visual feature point of the same type extracted synchronously from the intensity map of the current frame, used for cross-frame matching with the first feature point.

[0040] The motion state type of the current frame can be understood as being determined by jointly using two quantitative indicators: the proportion of depth motion points and the proportion of feature motion points, thus classifying the motion of the current frame. In this embodiment, typical classifications include: dynamic conditions, short-term static conditions, and motion-stopped conditions.

[0041] Camera pose refers to the camera's position in three-dimensional space, consisting of six degrees of freedom parameters: three translational quantities (spatial displacement along the X, Y, and Z axes) and three rotational quantities (tilt, yaw, and roll rotation angles). Motion pose estimation involves solving for the rotation matrix and translation vector of the current frame relative to the previous frame.

[0042] Specifically, firstly, the camera data stream is read in real time, and two sets of complete image data packets with adjacent time sequences are cached, namely the previous frame image and the current frame image. Each set of data packets internally binds synchronously acquired and pixel-registered intensity and depth maps to ensure a one-to-one correspondence between intensity texture and 3D depth information. This step provides a dual-modal temporal data source of 2D texture (intensity map) and 3D space (depth map), laying the foundation for subsequent dual motion criteria.

[0043] Then, through two independent parallel computation paths—the depth branch and the image feature branch—the percentage of depth motion points and the percentage of feature motion points are output, respectively. Specifically, based on the effective depth point set in the depth map of the previous frame and the effective depth point set in the current frame, the percentage of depth motion points in the current frame is calculated, thus quantifying the degree of image change in the three-dimensional space. Furthermore, based on the first feature point extracted from the intensity map of the previous frame and the second feature point extracted from the intensity map of the current frame, the percentage of feature motion points in the current frame is calculated, thus quantifying the degree of image offset in the two-dimensional texture layer. It should be understood that by independently outputting the percentage of motion points through the two branches—the depth branch and the image feature branch—one observing planar texture motion and the other observing three-dimensional spatial motion, the other set of indicators can still be used normally even if a single mode fails, significantly improving robustness. Moreover, the depth branch only uses matrix projection and pixel difference comparison, and the feature branch only uses descriptor matching and pixel distance calculation, without time-consuming operations such as global BA and global optimization. These can be completed synchronously within each frame acquisition cycle, without slowing down the overall pose estimation process and meeting real-time requirements.

[0044] Next, the motion state type is determined by jointly using the depth motion point ratio and the feature motion point ratio. Specifically, by combining the threshold ranges corresponding to the ratios of these two values, the system distinguishes between dynamic, short-term static, and stationary states of the camera, thus completing the classification of the motion state of the current frame. It should be understood that this step, by cross-validating using two independent sets of metrics—two-dimensional texture and three-dimensional depth—avoids false detections caused by single-point noise and localized noise, thereby outputting standardized motion classification labels as a prerequisite for pose estimation.

[0045] Finally, based on the motion state type output above, the corresponding attitude estimation strategy is adapted, and the motion attitude is estimated according to the scenario. It should be understood that this step uses the motion state type as a priori condition to dynamically switch the parameters, constraints, and iteration strategies of the attitude estimation strategy, balancing accuracy, robustness, and computational efficiency.

[0046] It is understood that this application provides a camera-based motion pose estimation method, which includes: first, acquiring a previous frame image and a current frame image captured by the camera, wherein the previous frame image and the current frame image respectively contain a pair of depth maps and intensity maps. Then, based on the effective depth point set of the depth map in the previous frame image and the effective depth point set of the depth map in the current frame image, calculating the proportion of depth motion points in the current frame image; and based on the extracted first feature point of the intensity map in the previous frame image and the second feature point of the intensity map in the current frame image, calculating the proportion of feature motion points in the current frame image, wherein the proportion of depth motion points and the proportion of feature motion points are used to determine the current frame motion state type of the camera. Finally, based on the current frame motion state type of the camera, estimating the current frame motion pose. This method can accurately distinguish the camera's motion state, and the motion pose estimation algorithm adaptively determines the motion pose according to different camera motion states, which can improve the real-time tracking response speed while ensuring the global accuracy of the camera's motion pose.

[0047] In one possible implementation, the motion state types include dynamic working conditions, short-term static working conditions, and motion-stopped working conditions.

[0048] Based on the current frame motion state type of the camera, estimate the current frame motion pose of the camera, including: If the motion state type of the current frame is dynamic, the initial motion pose of the current frame image predicted by the pose prediction model is used as the initial value for optimization. Sliding window bundle adjustment optimization is then performed on multiple consecutive frames, including the current frame image, to obtain the motion pose of the current frame image. The pose prediction model is a kinematic model built based on three historical motion poses to predict the motion pose of the next frame.

[0049] If the motion state type of the current frame is a short-term static condition, then the motion posture of the previous frame corresponding to the previous frame image is taken as the motion posture of the current frame.

[0050] If the motion state type of the current frame is a motion stop condition, then a global iterative optimization is performed based on all frame images during the entire camera motion process and the motion poses of all frames corresponding to all frame images to obtain the motion pose of the current frame.

[0051] Camera motion states can be categorized into dynamic, short-term static, and motion-stopped states. Dynamic states refer to continuous camera movement or rotation, with significant translation and rotation between frames, resulting in global shifts in image depth and features; this is not considered a static state. Short-term static states involve brief, temporary camera stillness, with only a few consecutive frames showing no significant motion. The camera is likely to resume movement afterward, requiring no prolonged stillness and global optimization; reusing the previous frame's pose is sufficient for accuracy. Motion-stopped states indicate the camera has completed all movement tasks and completely stopped, with no further significant pose changes. This signifies the end of the motion phase of the entire acquisition process and requires global correction of accumulated errors from all historical frames.

[0052] The pose prediction model can be understood as a prediction module built based on temporal kinematics. It takes three consecutive historical frames of solved six-DOF camera poses as input, fits the camera's translational and rotational velocities or accelerations, and calculates the predicted pose for the next frame, thus outputting the initial motion pose. The initial motion pose refers to the coarsely estimated pose before pose optimization, serving as the starting point for bundle adjustment iterations. A reasonable initial value for the initial motion pose can significantly improve the optimization convergence speed and avoid solution divergence. The kinematic model can be understood as a mathematical model describing the spatial motion of an object. It calculates linear and angular velocities using three consecutive frames of pose and extrapolates the camera pose for the next frame using the assumption of uniform velocity or uniform acceleration.

[0053] Sliding window beam adjustment optimization, also known as sliding window beam adjustment (BA) optimization, is a core local optimization algorithm in visual SLAM. It uses 3D points and camera pose as optimization variables to minimize multiple errors, including planar geometric residuals, plane normal vector angle residuals, and feature point reprojection residuals, to solve for the optimal camera motion pose. The sliding window can be understood as retaining only a finite number of consecutive frames within the window, discarding older frames, thus limiting the computational load and ensuring real-time performance. In other words, it only solves for the joint constraints of multiple frames within the window, without loading the full historical frame set.

[0054] A series of consecutive frames (i.e., a sliding window frame set) are temporally adjacent images cached within the sliding window, containing several historical frames and the latest current frame, serving as a joint observation data source for BA optimization.

[0055] Global iterative optimization can be understood as not limiting the number of frames, retrieving all historical images, poses and 3D feature points of all historical frames throughout the camera's operation, and fitting local planes (for inter-frame homogeneous plane matching and plane geometric residual optimization) and unit normal vectors (for plane normal vector angle residual optimization) using the Random Sample Consensus (RANSAC) algorithm. It then iterates to minimize multiple errors such as plane geometric residual, plane normal vector angle residual, and feature point reprojection residual in one go, so as to uniformly eliminate long-term cumulative drift error.

[0056] The motion pose of the current frame represents the position and orientation of the camera in three-dimensional space at the corresponding frame time, while the motion pose of the previous frame is the optimal pose that has been solved in the previous time.

[0057] Specifically, in this embodiment, the camera's motion state type is first determined as either dynamic, short-term static, or motion-stopped. After determining the camera's motion state type, the corresponding attitude estimation algorithm is executed, and the motion attitude of the current frame is output in a differentiated manner.

[0058] In some examples, for cameras in dynamic motion conditions, the process begins by inputting the calculated motion poses of three consecutive historical frames into a pose prediction model built based on kinematic laws. This model calculates the velocity and acceleration based on the translation and rotation changes of the previous three frames, predicting a rough initial motion pose for the current frame. Then, using this predicted initial motion pose as the starting point for iteration, a sliding window is constructed by selecting a segment of multiple consecutive frames, including the current frame. Bundle adjustment optimization is then performed on all frames within this window, matching 3D feature points, and using RANSAC-fitted local planes (for inter-frame source plane matching and planar geometric residual optimization) and unit normal vectors (for planar normal vector angle residual optimization). This continuously minimizes multiple errors, including planar geometric residuals, planar normal vector angle residuals, and feature point reprojection residuals. After iterative convergence, the optimized and more accurate motion pose of the current frame is output.

[0059] It should be understood that traditional methods typically use the motion pose of the previous frame as the initial value. Due to the large difference in pose between frames when the camera moves rapidly or shakes violently, iteration is prone to getting stuck in local optima or even failing to solve the problem. In this step, by using three consecutive frames to fit the motion velocity prediction as the initial value, it can closely match the actual motion trend of the camera and significantly improve the optimization convergence success rate. At the same time, the sliding window BA optimization can balance accuracy and real-time response speed. That is, it only optimizes a limited number of frames within the window, without loading the full historical frames, so the computational load is controllable and it is suitable for embedded and low-computing-power depth cameras to run in real time. Compared with single-frame pose calculation, multi-frame joint constraints can effectively suppress the pose jitter caused by single-frame noise, resulting in a smoother trajectory. Furthermore, by repeatedly observing the same 3D point in space in multiple frames within the window, the plane geometric residual, the plane normal vector angle residual, and the feature point reprojection residual are jointly minimized to offset the disturbances caused by single-frame infrared reflection, depth noise, and local dynamic objects, so that the pose accuracy is stable during dynamic movement.

[0060] In some examples, for cameras in a short-term static state, where the camera is only temporarily still with no effective displacement or rotation between frames, the motion pose solved in the previous frame can be directly reused without prediction or sliding window BA optimization. The reused motion pose from the previous frame is directly output as the motion pose for the current frame. It should be understood that when the image is static, there is no motion information, and re-optimization can lead to slight pose drift due to image noise; directly reusing the pose from the previous frame ensures a stable and consistent output pose throughout, without trajectory jumps, resulting in strong stability. Simultaneously, skipping the entire iterative optimization process of pose prediction and sliding window BA, without matrix solving or error-free iterative calculations, significantly reduces CPU / GPU load, making it suitable for robots and AR terminals in long-term standby scenarios, extending battery life.

[0061] In some examples, when the camera's motion state is in a stopped state, the camera completely stops moving and no further significant pose changes occur. First, all frame images stored throughout the device's operation are retrieved, along with the historical motion poses for each frame, all matched 3D spatial points, and the RANSAC-fitted local planes (used for inter-frame source plane matching and planar geometric residual optimization) and unit normal vectors (used for plane normal angle residual optimization). Then, global bundle adjustment iterative optimization is performed with all historical data as constraints, uniformly correcting the accumulated inter-frame errors throughout the entire time period. After global optimization convergence, the corrected motion pose of the current frame is updated and output. It should be understood that sliding window BA only corrects errors within a local window, and errors from older frames continue to accumulate. After motion stops, global optimization uniformly constrains all historical frames and 3D points, correcting the accumulated rotation and translational drift from the root, significantly improving the overall trajectory global consistency. All image features and depth observation data from the camera throughout the entire process participate in the optimization. Compared to local window optimization, the overall reconstruction accuracy of the 3D map and camera trajectory is significantly improved, making it suitable for scenarios with high requirements for global trajectory accuracy, such as photogrammetry, industrial inspection, and surveying. Meanwhile, global optimization involves a large amount of computation and is time-consuming. It is only triggered when the camera stops moving completely and does not occupy real-time computing resources during the movement process. It can achieve real-time correction during the running phase and high-precision correction during the shutdown phase, taking into account both real-time performance and global accuracy.

[0062] Understandably, in this embodiment, moderate computing power is invested during the dynamic working condition phase to ensure no tracking loss; zero optimization is performed during the short-term static phase to reduce the burden and power consumption; and concentrated computing power is used for high-precision global correction during the motion-stopped phase. This allows computing resources to be allocated on demand, balancing the three major indicators of real-time performance, power consumption, and accuracy. By differentiating the processing of the three working conditions, the defects caused by uniformly using a single BA strategy are avoided. By improving the single optimization objective in BA optimization, the objective function used integrates multiple optimization objectives such as point-to-plane geometric residual, plane normal vector angle residual, and feature point reprojection residual. This allows the optimization algorithm to adapt to different scenarios where planar structures and texture features coexist in the monitoring scene, enhancing the robustness of the algorithm.

[0063] In one possible implementation, the current frame motion state type of the camera is determined based on the depth motion point ratio and the feature motion point ratio, including: When the percentage of depth motion points is greater than or equal to a preset depth threshold, and / or the percentage of feature motion points is greater than or equal to a preset feature threshold, the current frame image is determined to be a moving image, and the current frame motion state type of the camera is determined to be dynamic.

[0064] When the proportion of depth motion points is less than a preset depth threshold and the proportion of feature motion points is less than a preset feature threshold, the current frame image is determined to be a still image.

[0065] If the cumulative number of frames of still images is less than the preset number of frames, the current operating state of the camera is determined to be a short-term still state.

[0066] When the cumulative number of frames of a still image is greater than or equal to a preset number of frames, the current operating state of the camera is determined to be motion-stopped.

[0067] The percentage of motion points in depth characterizes the degree of change in the image at the 3D spatial level. A preset depth threshold can be understood as a pre-set offline or adaptively configured proportional threshold used to determine whether there is significant overall motion at the depth level. If the percentage of motion points in depth is greater than or equal to the preset depth threshold, it indicates that there is a large-scale change in the 3D space of the image.

[0068] The proportion of moving feature points can characterize the global motion amplitude at the two-dimensional texture level. The preset feature threshold can be understood as a pre-configured critical value for the feature offset ratio, used to determine whether a global shift has occurred in the image texture features. If the proportion of moving feature points is greater than or equal to the preset feature threshold, it indicates that the camera has undergone overall translation or rotation.

[0069] A moving image refers to a current frame image that simultaneously exhibits significant changes in either depth or feature dimension, indicating that the camera is undergoing noticeable motion. A still image refers to an image where the percentages of both depth and feature motion points are below their respective preset thresholds, with no significant global motion between frames, indicating that there is no overall camera displacement or rotation in the current frame.

[0070] The cumulative frame value of a still image refers to the count of consecutive frames that are continuously determined to be still images. The count is incremented by 1 only when a new frame is still an image. Once a moving image appears, the cumulative frame value is cleared to 0 and the counting starts again.

[0071] The preset frame value refers to a pre-defined threshold for the number of consecutive static frames, serving as a boundary standard distinguishing between short pauses (short-term static conditions) and complete shutdowns (motion stop conditions). In this embodiment, the preset frame value... It can be set to 5 frames; there is no specific limitation on this.

[0072] In this embodiment, the overall logic for determining the motion state type of the current frame can be divided into two layers: the first layer uses a dual percentage threshold to distinguish whether the image is a moving image or a still image; the second layer counts consecutive still frames and distinguishes between short-term stillness and motion stoppage based on the comparison between the cumulative frame value and the preset frame value.

[0073] Specifically, based on the proportion of depth motion points ≥ a preset depth threshold, and / or the proportion of feature motion points ≥ a preset feature threshold, the current frame image is determined to be a moving image. In other words, as long as there is a large-scale change in the three-dimensional depth level, or any one of the global offsets of the two-dimensional image features is true, the current frame is marked as a moving image. At this time, the motion state type of the current frame is directly determined to be a dynamic condition.

[0074] Otherwise, if the percentage of depth motion points is less than the preset depth threshold and the percentage of feature motion points is less than the preset feature threshold, meaning there is no significant global change in either the depth or texture dimensions, the current frame image is marked as a still image.

[0075] Increment the cumulative frame value C of the still image by 1 (C+=1, the continuous still count is incremented and not cleared), and proceed to the next branch to distinguish between the two still working conditions based on the cumulative frame value.

[0076] If the cumulative number of frames C of still images is less than the preset number of frames. If the motion state type of the current frame is determined to be a short-term static condition, that is, the current frame has been static for several consecutive frames, but the duration is short and does not meet the stopping judgment criteria. The current frame state is determined to be a short-term static condition. The cumulative count of the cumulative frame value C is not reset. If the next frame is still static, the count continues to accumulate.

[0077] The cumulative frame value C of the still image is greater than or equal to the preset frame value. If the current frame motion state type is determined to be motion stop condition, that is, the number of consecutive static frames reaches the set threshold, indicating that the camera has not moved for a long time, and the operation is judged to be over. This condition is maintained for subsequent continuous static frames until the moving image appears again, at which point the count is reset to zero and the dynamic condition is switched back.

[0078] It should be understood that in this embodiment, the condition judgment is completed by comparing the proportion of moving points and simple counting, resulting in extremely low computational cost. Status labels are output to the attitude calculation module in advance, enabling high-precision sliding window BA in dynamic situations, zero-computation attitude reuse in short-term static situations, and one-time global optimization during shutdown. This allows the system's computing power to be allocated as needed, balancing real-time performance, power consumption, and accuracy. Simultaneously, through a dual noise reduction mechanism of joint verification using depth and image features and continuous frame counting filtering, misjudgments of the condition caused by depth noise, lighting interference, dynamic foreground objects, and minor mechanical jitter can be suppressed, avoiding abnormal issues such as tracking loss, attitude jumps, and frequent triggering of global optimization.

[0079] In one possible implementation, the proportion of motion points in the current frame image is calculated based on the effective depth point set of the depth map in the previous frame image and the effective depth point set of the depth map in the current frame image, including: The effective depth point set in the previous frame image is compared with the effective depth point set in the current frame image, and the depth difference between the effective depth point set in the previous frame image and the effective depth point set in the current frame image is calculated point by point.

[0080] The number of depth points whose depth difference is greater than a preset depth difference threshold is counted to obtain the number of valid depth points.

[0081] The percentage of depth motion points is calculated based on the ratio of the number of effective depth points to the total number of effective depth points in the current frame image.

[0082] The effective depth point set in the current frame image refers to the set of legally valid depth pixels synchronously selected in the current frame depth map, which is the real depth observation data acquired by the sensor in real time. The depth difference refers to the absolute value obtained by subtracting the depth value corresponding to the effective depth point in the effective depth point set of the previous frame image and the depth value corresponding to the effective depth point in the effective depth point set of the current frame image at the same pixel location, representing the magnitude of the distance deviation between the current frame image and the previous frame image.

[0083] The preset depth difference threshold can be understood as a pre-calibrated fixed critical value used to distinguish between depth deviations caused by minor noise and significant depth deviations caused by motion in real space. The number of effective depth points (motion point count) refers to the total number of pixels whose depth difference exceeds the preset depth difference threshold after point-by-point comparison; that is, the total number of depth motion points. The total number of depth points in the effective depth point set of the current frame image can be understood as the total number of all legally valid depth pixels in the current frame's depth map, used as the denominator in the calculation of the proportion.

[0084] Specifically, first, all valid depth point sets in the previous frame image and all valid depth point sets in the current frame image are obtained. The valid depth point sets in the previous frame image and the valid depth point sets in the current frame image are compared, and the depth difference between the valid depth point sets in the previous frame image and the valid depth point sets in the current frame image is calculated point by point.

[0085] Then, the depth difference of all matching pixels is traversed, and pixels with a depth difference greater than a preset depth difference threshold are identified as valid depth motion points. The number of all pixels that meet the condition is accumulated to obtain the number of valid depth points (i.e., the total number of depth motion points). Points with a difference less than or equal to the preset depth difference threshold are identified as static points and are not included in the statistics.

[0086] Finally, using the number of effective depth points as the numerator and the total number of depth pixels in the effective depth point set of the current frame image as the denominator, the percentage of depth motion points between 0 and 1 is output as a quantitative indicator for subsequent judgment of camera motion conditions.

[0087] That is, select the set of all valid depth points in the depth map of the previous frame image. ,in, This refers to the valid depth points (depth range 0.5~10.0 meters, suitable for long-distance coverage requirements in monitoring scenarios) in the depth map of the previous frame image. It also includes the set of all valid depth points in the depth map of the current frame image. And calculate the depth difference point by point: ;in, This represents the depth values ​​of each point in the set of valid depth points in the previous frame image. This represents the depth value of each point in the set of valid depth points in the current frame image.

[0088] The statistical depth difference is greater than the preset depth difference threshold. Number of effective depth points Calculate the percentage of depth motion points , Where M is the total number of depth points in the effective depth point set of the current frame image, if ( If a preset depth threshold is used, it is determined that there is effective depth motion in the current frame image, and the motion state type of the camera in the current frame can be determined to be dynamic.

[0089] It should be understood that conventional pixel-wise depth difference calculations will increase the proportion of changing pixels as soon as local objects move, which can easily lead to misjudging that the camera is in a dynamic state. The embodiments of this application construct a motionless theoretical model based on the motion pose of the previous frame. Only global camera displacement will generate a large area of ​​depth difference exceeding the threshold. Local foreground motion will only generate a small number of discrete motion points, which account for a low proportion and will not falsely trigger dynamic state judgment. The entire process has no time-consuming operations such as feature extraction, bundle adjustment, and global optimization. The processing time per frame is extremely low and can be executed in parallel with the calculation of feature motion proportion without slowing down the overall pose estimation pipeline.

[0090] In one possible implementation, the proportion of feature motion points in the current frame image is calculated based on the first feature point of the intensity map extracted from the previous frame image and the second feature point of the intensity map in the current frame image, including: The first feature point and the second feature point are matched using the feature optical flow algorithm to obtain a matching point pair, and the displacement between the matching point pairs is calculated.

[0091] The number of motion feature points whose displacement exceeds a preset displacement threshold is counted to obtain the number of valid motion feature points.

[0092] The proportion of feature motion points is calculated based on the ratio of the number of effective motion feature points to the total number of matched point pairs.

[0093] The first feature point refers to a stable visual feature point (such as corner points or ORB feature points) extracted from the intensity map of the previous frame. It contains pixel coordinates and grayscale gradient information and serves as the baseline feature for optical flow tracking. The second feature point refers to the image feature corresponding to the current frame's intensity map, which is tracked by the optical flow algorithm without the need for a complete re-extraction of the descriptor. The feature optical flow algorithm is a lightweight feature tracking algorithm based on the grayscale invariance assumption (typically LK optical flow). It utilizes the grayscale continuity between adjacent frames to track the pixel position of the same feature point in the preceding and following frames, achieving fast cross-frame matching.

[0094] A matched point pair can be understood as a set of first and second feature points that have been successfully tracked by optical flow and can be matched between consecutive frames. It represents the imaging position of the same spatial scene point on the intensity maps of the two frames. The displacement between matched point pairs refers to the pixel offset distance of the same feature point from the previous frame to the current frame on the image plane, measured using Euclidean distance. The preset displacement threshold refers to a pre-set critical value for pixel distance, used to distinguish between small shifts caused by sensor jitter and noise, and large pixel displacements caused by actual camera movement.

[0095] The effective number of motion feature points refers to the total number of feature points in a matched point pair whose pixel displacement exceeds a preset displacement threshold, i.e., the motion feature point count. The total number of matched point pairs refers to the total number of feature point pairs that are successfully matched and effectively paired by optical flow tracking, and is used as the denominator in the percentage calculation. The feature motion point percentage is the ratio of the effective number of motion feature points to the total number of matched point pairs, ranging from 0 to 1, and can quantify the overall motion amplitude of the image at the texture level of a two-dimensional image.

[0096] Specifically, firstly, the first feature point extracted from the intensity map of the previous frame is input, and optical flow algorithms such as LK optical flow are used to track the corresponding position in the intensity map of the current frame, outputting the second feature point to form a one-to-one matching point pair. Feature points that failed to track, disappeared from the frame, or were lost due to sudden gray-level changes are filtered out, retaining only reliable matching point pairs. For each matching point pair, the planar pixel displacement value is calculated based on the pixel coordinates of the preceding and following frames. It should be understood that lightweight tracking is performed based on the continuity of image gray levels, quickly obtaining the inter-frame offset of feature points, replacing the time-consuming feature descriptor matching.

[0097] Then, the pixel displacements of all matching point pairs are iterated. If the displacement between a matching point pair is greater than a preset displacement threshold, the matching point pair is determined to be a moving feature point. All points that meet the condition are accumulated to obtain the number of valid moving feature points. Small displacement points caused by minor noise offsets and slight mechanical vibrations are directly filtered out and not included in the moving point statistics.

[0098] Finally, using the number of effective motion feature points as the numerator and the total number of matching point pairs successfully tracked by optical flow as the denominator, the proportion of feature motion points between 0 and 1 is obtained. This proportion of feature motion points serves as a quantitative motion index for the two-dimensional texture dimension and is provided to the subsequent working condition judgment logic.

[0099] In some examples, the intensity map is extracted from the current frame image. The second feature point in the image is compared with the intensity map in the previous frame. The first feature point (adapted for weak texture monitoring scenarios) is identified. A feature optical flow algorithm is used to match the first and second feature points, resulting in matching point pairs, and the displacement between these pairs is calculated.

[0100] The statistical displacement is greater than the preset displacement threshold. Number of motion feature points Calculate the proportion of characteristic moving points , ;in, To the total number of matching point pairs, if ( If a preset feature threshold is used, then it is determined that there is effective feature motion within the frame, thereby determining that the current frame motion state type of the camera is dynamic.

[0101] It should be understood that in this embodiment, the low computational cost of the feature branch across the entire link allows for parallel computation with the depth branch. Optical flow tracking and pixel distance comparison are both basic image operations, without time-consuming operations such as matrix optimization and global reconstruction. The synchronous computation of the two branches does not consume a large amount of hardware resources, meeting the real-time positioning requirements of the device. At the same time, optical flow has the characteristic of continuous inter-frame tracking. Compared with re-extracting feature matching in each frame, the feature motion ratio between consecutive frames will not change drastically, making the condition judgment result smoother and more stable, and avoiding frequent state switching.

[0102] In one possible implementation, the pose prediction model is a second-order uniformly accelerated motion model.

[0103] The process of predicting the initial motion pose of the current frame image using a pose prediction model includes: Extract three frames of the camera's historical motion pose.

[0104] The equivalent acceleration of the camera is calculated based on three frames of historical motion posture.

[0105] Based on the principle of short-time uniform acceleration motion, the Lie algebra increment of the current frame corresponding to the motion posture of the current frame is obtained by predicting the equivalent acceleration.

[0106] The state transition equation is constructed based on the Lie algebra increment and the left Jacobian matrix of the current frame, and the predicted motion attitude of the current frame is obtained.

[0107] The initial motion attitude of the current frame is obtained by iterative estimation using extended Kalman filtering based on the predicted motion attitude of the current frame, the observation equation, and the preset attitude increment damping constraint. The observation equation is obtained by fusing the point-to-plane geometric residual, the angle residual between the plane normal vectors, and the feature point reprojection residual.

[0108] Among them, the second-order uniformly accelerated motion model (pose prediction model) is a temporal dynamic model of the six-degree-of-freedom spatial motion of the camera. It assumes that the camera performs uniformly accelerated rigid body motion within a short time window (second-order motion: displacement is determined by velocity and acceleration). Unlike the first-order uniform velocity model, it can simultaneously model linear acceleration and angular acceleration, and is used for inter-frame pose extrapolation prediction. The three-frame historical motion pose refers to the three frames of converged and solved six-degree-of-freedom camera poses that are temporally continuous and immediately preceding the current frame. Each frame contains rotation and translation, and is stored in the form of Lie group SE(3). It is the temporal observation data source for solving velocity and acceleration.

[0109] Equivalent acceleration refers to the equivalent angular acceleration and equivalent linear acceleration of the camera obtained by solving the temporal difference of poses in three consecutive frames. The average acceleration measure is incorporated into the uniform acceleration dynamics framework to uniformly characterize the rate of change of six degrees of freedom motion. The short-time uniform acceleration motion principle refers to the fact that the acquisition interval between adjacent frames is extremely short (millisecond level). The camera velocity and acceleration will not change drastically in a short time. It can be approximately considered that the motion satisfies the law of uniform acceleration dynamics and can be used as a physical assumption for extrapolating motion posture. Lie algebra increment refers to the relative change in camera posture between two frames when the pose of SE(3) is represented by a Lie group / Lie algebra. It includes translation increment and rotation increment and is used to describe the small motion transformation between adjacent frames.

[0110] The left Jacobian matrix is ​​the left-multiplied Jacobian matrix corresponding to the SE(3) Lie group. Its function is to realize the linear mapping between the Lie algebra increment and the Lie group transformation, and to compensate for the nonlinear error when small motion increments are superimposed. It is the core matrix for constructing the linear state transition equation. The state transition equation refers to the core formula of the prediction stage of the extended Kalman filter (EKF). Using the state, motion acceleration, and Jacobian matrix of the previous moment, the prior prediction value of the motion attitude of the next frame is recursively obtained, describing the evolution law of the camera motion attitude over time.

[0111] The current frame motion pose prediction can be understood as a pose prior estimate obtained solely through recursion from the uniform acceleration dynamics model, without incorporating image observation residual correction, and is only used as the initial prior for filtering iterations. The observation equation refers to a unified observation model constructed by fusing multi-dimensional geometric constraints, which transforms image, depth, and planar geometric information into residual observation values, serving as the observation input for EKF, and establishing the mathematical relationship between camera motion pose and scene geometric observation.

[0112] Point-to-plane geometric residual refers to the vertical distance error between a 3D point and its corresponding plane after the depth point is projected onto the local plane. It utilizes the planar structure of the depth map to constrain motion posture and belongs to 3D geometric observation residuals. Plane normal vector angle residual refers to the difference in the angle between the normal vectors of matching planes in adjacent frames. It characterizes the scene's planar orientation consistency constraint and strengthens pose constraints in large planar scenes (walls, ground). Feature point reprojection residual refers to the two-dimensional distance error between the predicted pixel coordinates and the measured pixel coordinates of image feature points after projection onto the camera's motion posture. It represents a classic visual two-dimensional texture constraint.

[0113] Attitude increment damping constraint refers to a pre-defined regularization constraint term that limits the magnitude of the inter-frame pose Lie algebra increment. This can suppress excessive jumps in attitude increment during EKF iteration, preventing filter divergence and abrupt changes in motion prediction. Extended Kalman Filter (EKF) iterative estimation can be understood as a linear approximation filtering algorithm for a pose nonlinear observation model. It consists of a prediction step and an update step: first, it dynamically predicts the prior motion attitude, then it fuses multiple residual observations to correct the prior, outputting the optimal posterior estimate. The initial motion attitude of the current frame refers to the high-precision prior motion attitude output after EKF filtering convergence, serving as the initial value for sliding window BA optimization iterations.

[0114] The complete prediction process from the historical three frames of pose to the initial pose of BA is as follows: Step 1: Extract the three historical motion poses of the camera. That is, read the three consecutive frames of solved and converged stable SE(3) camera poses from the cached temporal motion pose queue, remove abnormal historical poses that jump or lose tracking, and use them as input for the dynamic model.

[0115] Step 2: Calculate the camera's equivalent acceleration based on three frames of historical motion posture. This involves performing Lie algebraic differences on the three frames to solve for the inter-frame translational velocity and rotational angular velocity. Using the velocity difference between adjacent frames, calculate the linear acceleration and angular acceleration. Merge the translational and rotational accelerations to obtain a unified six-degree-of-freedom equivalent acceleration, representing the short-term motion change trend of the camera.

[0116] Step 3: Based on the principle of short-time uniform acceleration motion, predict the Lie algebra increment of the current frame. That is, take the time interval between adjacent frames, the equivalent linear / angular acceleration, and the motion velocity of the previous frame as inputs, substitute them into the uniform acceleration motion formula, and calculate the small motion change amount of the current frame relative to the previous frame, that is, the Lie algebra increment. This Lie algebra increment is the amount of inter-frame motion attitude change that the camera should theoretically occur.

[0117] Step 4: Construct the state transition equation by combining the left Jacobian matrix and solve for the pose prediction value. That is, introduce the SE(3) left Jacobian matrix to perform linear correction on the Lie algebra increment and compensate for the nonlinear superposition error of the Lie group. Substitute the optimal pose of the previous frame and the corrected Lie algebra increment into the state transition equation to recursively obtain the motion pose prediction value of the current frame without image observation correction (EKF prior state).

[0118] Step 5: Fuse the multi-residual observation equations and apply damping constraints to perform EKF iterations, outputting the initial motion attitude. This involves constructing a fused observation equation, simultaneously calculating three types of residuals: feature reprojection residuals, point-to-plane distance residuals, and plane normal vector angle residuals, and then weighted and fused them into a unified observation vector. An attitude increment damping constraint is added, i.e., a regularization term is added to the EKF cost function to limit the maximum magnitude of the inter-frame attitude increment and suppress abrupt changes.

[0119] Next, an extended Kalman filter iterative update is performed. In the prediction step, the predicted motion pose of the current frame obtained above is used as the prior motion pose. In the update step, the multi-source geometric residual observations are substituted, the Kalman gain is calculated, and the prior motion pose is corrected. After iterative convergence, the optimal posterior estimate is output, which is the initial motion pose of the current frame supplied to the sliding window BA.

[0120] It should be understood that traditional methods use only two frames of uniform extrapolation, resulting in significant prediction errors during camera acceleration, deceleration, turning, and starting / stopping. In this embodiment, based on a three-frame second-order uniform acceleration model, it can accurately fit variable-speed motion and output a priori pose that is closer to reality. Simultaneously, the entire prediction chain is based on rigorous geometric derivation using Lie groups and Lie algebras, and compensates for small motion nonlinearities through Jacobian, resulting in smaller rotation and translation coupling errors compared to Euler angles / quaternion recursion. Outputting high-quality initial values ​​for the Body Attack (BA) via the EKF significantly optimizes the performance of the sliding window BA, accelerating convergence, preventing tracking loss, and smoothing the trajectory. The entire prediction process has extremely low frame latency, without hindering the real-time camera pose estimation pipeline, balancing real-time performance and accuracy, and is suitable for low-computing-power embedded depth cameras, inspection robots, and AR devices. Moreover, in extreme conditions such as severe shaking, large-area foreground occlusion, and sudden increases in depth noise, damping constraints can limit abnormal abrupt changes in pose increments, preventing severe oscillations in the EKF, ensuring that the initial pose is not completely distorted, and maintaining tracking continuity.

[0121] In one possible implementation, the equivalent acceleration of the camera is calculated based on three frames of historical motion pose, including: Calculate the Lie group transform increment between adjacent frames based on the three historical motion poses.

[0122] The Lie group transformation increment is converted into a Lie algebra increment through a logarithmic mapping.

[0123] The equivalent acceleration of the camera is obtained by performing second-order difference calculation on the Lie algebra increment.

[0124] The three-frame historical motion pose refers to the camera motion pose of the first three frames in a sequential manner without tracking loss. The motion pose of each frame is expressed in the form of an SE(3) Lie group, which fully describes the camera's rotation and translation in the world coordinate system. The Lie group transformation increment refers to the relative rigid body transformation matrix between two adjacent SE(3) motion poses. For adjacent frames, such as the third frame historical motion pose... Second frame historical motion posture First frame of historical motion posture The first Lie group transform increment between the historical motion pose of the second frame and the historical motion pose of the third frame The second Lie group transform increment between the historical motion pose of the first frame and the historical motion pose of the second frame .

[0125] Logarithmic mapping refers to the standard mapping operation from SE(3) Lie group to SE(3) Lie algebra, that is, inputting a rigid body transformation matrix (Lie group) and outputting a six-dimensional infinitesimal motion vector (Lie algebra increment) to realize the conversion from matrix form transformation to vector form motion increment, which is the basis of Lie group differential and difference operations.

[0126] Lie algebra increments refer to six-dimensional se(3) vectors, where the first three dimensions are rotation increments related to rotational angular velocity, and the last three dimensions are translation increments related to translational velocity. Linear vectors are used to represent minute rotations and displacements between two frames, facilitating numerical operations such as differencing and differentiation. Second-order differencing refers to performing a difference operation on two consecutive sets of Lie algebra increments to obtain the velocity change, and then performing another difference operation to obtain the acceleration measure. Physically, this corresponds to first-order differencing = inter-frame velocity, and second-order differencing = inter-frame acceleration.

[0127] Equivalent acceleration refers to the unified acceleration in the form of a six-dimensional se(3) vector, which includes three-dimensional equivalent angular acceleration and three-dimensional equivalent linear acceleration. It can integrate the camera rotation and translation speed changes into a unified temporal dynamic parameter, which is used to predict the motion increment of the next frame in a second-order uniformly accelerated motion model.

[0128] It should be understood that traditional methods using Euler angles and quaternions to directly calculate acceleration can lead to rotational coupling, periodic angle jumps, and gimbal lock errors. In this embodiment, however, the SE(3) standard geometric operation is used, which accurately decouples rotation and translation, and the acceleration calculation is free from geometric distortion. At the same time, by using three frames of second-order difference to extract acceleration, it is possible to model camera acceleration, deceleration, turning, and other conditions, providing accurate dynamic parameters for the subsequent pose prediction model and significantly improving the accuracy of initial pose prediction under high-speed and variable-speed motion.

[0129] In some examples, the camera motion attitude is calculated in real time to support the real-time delineation, tracking, and security warning of the three-dimensional monitoring area, reducing the processing time per frame to meet real-time requirements. High-precision initial values ​​are provided by fitting the differential model of SE(3) rigid body motion, and multi-dimensional residual constraints are integrated to improve the accuracy and stability of attitude solution.

[0130] (1) State definition: The six-DOF pose of the camera is represented by the SE(3) Lie algebra, and the state vector is... ,in, ( It is a translation vector. (where k is the rotation Lie algebra, and k represents the current frame image).

[0131] Three-frame sliding window: Maintaining the optimized pose window based on three frames of history. It is used to construct an inter-frame incremental model of approximate rigid body motion, used for EKF motion model calculation, does not participate in covariance propagation, and ensures real-time performance.

[0132] Observation residual definition: This involves fusing three types of constraints: point-to-plane geometric residual (depth geometric residual, point-to-plane distance), plane normal vector angle residual, and feature point reprojection residual. A multi-dimensional joint observation equation is constructed to adapt to monitoring scenarios where planar structure and texture features coexist. The observation equation is as follows: ( ),in, These are the observed values, namely the depth geometry, the angle between the normal vectors, and the reprojection coordinates of the feature points observed in the current K frames; The variable to be estimated; This indicates that the estimated attitude is Below, the values ​​of depth geometry, normal vector angle, and feature point reprojection coordinates observed in K frames are calculated. This represents random error.

[0133] 3D monitoring area association: Real-time calculation of the current frame's motion pose This formula can be directly used to update the world coordinate system position of the real-time 3D monitoring area, as follows: ; in, This is the initial three-dimensional monitoring area (such as a cuboid protective space). The SE(3) transformation matrix corresponding to the Lie algebra is used to realize the real-time synchronization of the monitoring area with the camera attitude.

[0134] (2) EKF initialization: quickly provides high-precision optimized initial values ​​(based on the SE(3) inter-frame incremental second-order difference model).

[0135] Based on the rigid body motion characteristics of the camera SE(3), a second-order uniformly accelerated motion model with higher physical fit is constructed by incremental mapping Lie algebra through inter-frame pose transformation. It is independent of IMU sensor and adapts to the working conditions of monitoring cameras.

[0136] Specifically, obtain three consecutive frames of historical motion poses (optimal Lie group poses), i.e. ,in, This indicates the historical motion posture of the third frame (i.e., the third frame before the current frame). This indicates the historical motion posture of the second frame (i.e., the second frame before the current frame). This indicates the historical motion posture of the first frame (i.e., the first frame before the current frame).

[0137] Calculate the rigid body transformation increment (i.e., Lie group transformation increment) between adjacent frames, that is: the first Lie group transformation increment between the historical motion pose of the second frame and the historical motion pose of the third frame. The second Lie group transform increment between the historical motion pose of the first frame and the historical motion pose of the second frame. .

[0138] The Lie group transformation increment is converted into a Lie algebra increment (a linear vector in the tangent space) via a logarithmic mapping, i.e., the first Lie algebra increment. Second Lie algebra increment .

[0139] By performing second-order difference calculations on the first and second Lie algebra increments, the equivalent acceleration characterizing the camera motion is obtained. ,Right now .

[0140] Since the typical frame rate of the camera is relatively high, the Lie algebra increment of the current frame is approximately predicted based on the principles of discrete piecewise constant velocity and uniform acceleration kinematics: ; The predicted Lie algebra pose for the current frame is: ,because To compare small quantities, we can approximate it as: ; Taking the logarithm of both sides, we obtain the following state transition equation: ; in, for Left Jacobi, ( To adapt to the process noise covariance, the state transition Jacobian can be dynamically adjusted based on the motion intensity. It has a simple form and takes less time to calculate.

[0141] The subsequent steps involve using the EKF method, combined with the nonlinear observation iteration and incremental damping method described below, to estimate the initial EKF attitude based on the state transition equations (motion equations) and observation equations above. This refers to the initial motion posture of the current frame.

[0142] The nonlinear observation iteration and the preset attitude increment damping constraint are as follows: Iterative solution of attitude increment If the attitude increment exceeds the limit, damping is applied (K is the Kalman gain, r is the observation error). Damping coefficient To avoid sudden changes in attitude that could cause a jump in the monitored area, iterate 1-5 times (or the specified total time) and output the initial EKF attitude. The initial motion posture of the current frame is used as the initial value for the three-frame sliding window BA optimization, ensuring that the BA optimization converges quickly.

[0143] In one possible implementation, if the motion state type of the current frame is dynamic, the initial motion pose of the current frame image predicted by the pose prediction model is used as the initial value for optimization. Sliding window bundle adjustment optimization is then performed on multiple consecutive frames, including the current frame image, to obtain the motion pose of the current frame image, including: The system uses multiple consecutive frames, including the current frame, as a sliding window and constructs an objective function that includes the point-to-plane geometric residual, the plane normal vector angle residual, and the feature point reprojection residual.

[0144] Based on the scene texture and the number of planes, the residual weights corresponding to the point-to-plane geometric residual, the plane normal vector angle residual, and the feature point reprojection residual are adaptively assigned to obtain the optimization model to be solved.

[0145] Based on the initial motion posture of the current frame, the variables to be optimized within the sliding window are initialized to obtain the initial optimized solution for the variables to be optimized.

[0146] Based on the optimized initial solution, the variable values ​​are obtained after each iteration using Gauss-Newton method iterative calculation and Schul elimination.

[0147] When the preset iteration convergence condition is met, the iteration calculation is terminated, and the motion posture of the current frame is obtained.

[0148] A sliding window refers to a time-series set of consecutive image frames. The window can contain several historical frames and the latest current frame. The length of the window can be fixed. When adding a new frame, the oldest frame is discarded, limiting the total number of frames participating in the optimization, balancing BA accuracy and real-time performance, and is only enabled in dynamic scenarios.

[0149] The variables to be optimized refer to the unknowns that need to be solved iteratively in the sliding window BA, such as the motion pose of the camera SE(3) in each frame within the window; the three-dimensional feature points and three-dimensional plane parameters obtained by matching all frames. The objective function refers to the cost function composed of the weighted sum of three types of geometric residuals. The optimization objective is to minimize the value of the objective function, which represents the minimum overall error between all observation constraints and the current motion pose and three-dimensional model.

[0150] Feature point reprojection residual refers to the Euclidean distance error between the predicted pixel coordinates of a 2D image feature point projected onto the imaging plane by the camera pose and the measured pixel coordinates of the image. It can constrain the consistency of 2D texture matching. Point-to-plane geometric residual refers to the perpendicular distance from a 3D depth point projected onto the corresponding scene plane to the plane. It can be used to calculate the depth... Figure 3 3D planar structure constrains pose, compensating for the lack of features in weakly textured scenes. The plane normal vector angle residual refers to the angle difference between the normal vectors of the matching scene planes in two frames. It can constrain the orientation of large planes such as walls, ground, and workbench to be stable and suppress rotational drift.

[0151] Residual weights refer to the weighting coefficients assigned to each type of residual, representing the confidence and importance of that type of geometric constraint in the optimization. Higher residual weights indicate a stronger constraint on the optimization result. Adaptive weight allocation can be understood as automatically adjusting the weights of the three types of residuals based on real-time scene observation conditions (texture richness, number of planes). The residual weights are not fixed constants and can adapt to different scene environments.

[0152] The optimization model to be solved refers to the least squares solution model consisting of a weighted multi-residual objective function, constraint terms, and variables to be optimized, and is the input of the Gauss-Newton iteration. The initial optimization solution refers to the initial values ​​of all variables to be optimized within the sliding window. The initial motion posture of the current frame output by the pose prediction model is used as the core initial value, and is uniformly initialized with the optimized postures of historical frames and the initial values ​​of 3D points within the window.

[0153] The Gauss-Newton method is a classic iterative algorithm for solving nonlinear least squares problems. It makes a first-order Taylor linear approximation of the objective function, constructs a normal equation, and reduces the total residual by continuously updating variables. The optimal solution is obtained after convergence. Schur elimination is a matrix-accelerated solution method optimized by BA. It utilizes the block sparsity of camera pose variables and 3D point variables to eliminate a large number of 3D point variables, significantly reducing the dimension of the normal equation, decreasing the computational cost of matrix inversion, and accelerating iteration.

[0154] The updated variable values ​​after each iteration can be understood as the new variable values ​​obtained by superimposing the updated values ​​of motion posture, three-dimensional points, and planar parameters obtained by solving the normal equations after a single Gauss-Newton iteration calculation, onto the original variables.

[0155] The preset iteration convergence condition refers to the pre-defined criteria for stopping the iteration. The preset iteration convergence condition can be that the decrease in the objective function in a single iteration is less than a minimum threshold, or that the number of iterations reaches the maximum limit, etc. In this embodiment, the specific content of the preset iteration convergence condition is not limited.

[0156] The current frame motion pose refers to the optimal six-degree-of-freedom SE(3) pose of the current frame within the sliding window after iterative convergence, which is the final output camera pose under dynamic conditions.

[0157] When the motion state type of the current frame is dynamic, the specific process of calculating the camera's motion pose in the current frame using sliding window BA optimization is as follows: (1) Construct a sliding window and fuse multiple residual objective functions. Select multiple consecutive time-series images, including the current frame image, to form a sliding window. Fix the window size, and discard the earliest frame if the size exceeds the capacity.

[0158] The image features and depth plane observations of all frames within the window are traversed, and three types of residuals are calculated simultaneously: feature reprojection residual, point-to-plane distance residual, and plane normal vector angle residual.

[0159] The three types of residuals are combined to form a basic unweighted least-squares objective function, which serves as the error benchmark for optimization. By simultaneously introducing triple geometric constraints—two-dimensional texture, three-dimensional points, and three-dimensional plane—a multi-dimensional error evaluation system is constructed.

[0160] (2) Adaptively allocate residual weights according to scene conditions to generate an optimized model. Real-time statistics of window scene indicators, such as the total number of effective feature points (representing texture richness) and the total number of matchable planes (representing the amount of planar structure).

[0161] In some examples, the scene-adaptive weighting rule can be: if there is abundant texture and few planes, increase the weight of the reprojection residual and decrease the weight of the plane residual; if there is weak texture and many planes, increase the weight of the residuals from point to plane and the angle between normal vectors, and weaken the feature constraints; if there are no complete planes, reset the weight of the plane-related residuals to zero and rely solely on two-dimensional features for optimization.

[0162] The adaptive weights are multiplied into the corresponding residual terms to generate a complete optimization model to be solved with weighted coefficients.

[0163] (3) Initialize all variables to be optimized with the initial motion pose of the current frame. Use the initial motion pose of the current frame output by the pose prediction model as the initial pose value. Based on the initial motion pose of the current frame, back-calculate the initial estimates of the three-dimensional feature points and planar parameters within the window, complete the initialization of the motion pose of all cameras, three-dimensional points, and planar parameters within the window, and obtain the initial optimization solution.

[0164] (4) Gauss-Newton iteration combined with Schur elimination to update variables. Based on the current variable values, all weighted residuals and Jacobian matrices are recalculated, and the block sparse normal equations are assembled; Schur elimination is performed to eliminate 3D point variables, significantly reducing the equation dimension and quickly solving for the camera pose increment; the obtained increment is superimposed on the original variables to be optimized, and all motion poses, 3D points, and planar parameters are updated to obtain the new variable values ​​for this iteration. Then, residual calculation, matrix assembly, Schur elimination, and variable update are performed cyclically, and multiple iterations are used to continuously reduce the error of the total objective function.

[0165] (5) When the preset iteration convergence condition is met, the iteration is terminated and the motion posture of the current frame is output. That is, the convergence condition is checked after each iteration, such as whether the decrease in the total residual of this round is less than the error threshold, or whether the number of iterations has reached the upper limit. If the preset iteration convergence condition is met, the Gauss-Newton iteration is stopped. The optimal SE(3) pose corresponding to the current frame in the window is extracted as the final output motion posture of the current frame under dynamic conditions.

[0166] In some examples, the optimization window is defined as a sliding window consisting of three consecutive frames, including the current frame, i.e., k-2, k-1, and k. The variable to be optimized is... The three-frame sliding window is small in size, which can ensure real-time performance.

[0167] Construct the objective function: ; in, For point-to-plane geometric residuals, The residual is the angle between the plane normal vectors. For the feature point reprojection residual, These are the adaptive weighting coefficients for the point-to-plane geometric residuals. The adaptive weighting coefficients are the residuals of the angle between the plane normal vectors. These are the adaptive weighting coefficients for the feature point reprojection residuals. These three weighting coefficients can be dynamically adjusted based on the scene texture richness and the number of planes.

[0168] The process of solving the objective function is as follows: (1) Using EKF initial value Initialize the variables to be optimized to speed up the solution; (2) The Gauss-Newton method is used for iterative solution, and the Schur elimination operation is used to accelerate the solution of sparse matrix, so as to obtain the variable value after each iteration and reduce the computational complexity. (3) Convergence criterion: at the iteration step size If the number of iterations is ≥20, terminate the iteration calculation and output the motion pose of the current frame. .

[0169] It should be noted that the intermediate results are stored, specifically the proportion of non-zero elements in the matrix after Schur elimination during BA optimization. ( It can reflect the sufficiency of constraints. Larger constraints mean more sufficient constraints, compared to the optimized average residual squared. This provides a quality basis for subsequent global optimization and enables real-time monitoring of the response, i.e., the motion posture of the current frame. Input into the monitoring system to update the real-time 3D monitoring area. It enables real-time response for functions such as intrusion detection and regional early warning.

[0170] It should be understood that in this embodiment, the sliding window can limit the computational scale, Shure elimination can accelerate matrix solving, the prediction of initial values ​​can reduce the number of iterations, and multiple methods can reduce computing power. At the same time, triple adaptive weighted geometric constraints ensure that the trajectory within the window is smooth and has little drift, thus resolving the contradiction between accuracy and real-time performance in dynamic moving scenarios. This achieves a balance between real-time tracking speed and local positioning accuracy under dynamic conditions.

[0171] In one possible implementation, if the motion state type of the current frame is a motion-stopped condition, then a global iterative optimization is performed based on all frame images during the entire camera motion process and the motion poses of all frames corresponding to all frame images to obtain the motion pose of the current frame, including: Based on the local frame stack length, all frame image sequences are divided into multiple local frame stacks, where the local frame stack length is obtained as a percentage of the total number of frames accumulated throughout the entire camera motion process.

[0172] Based on the motion pose of each frame within each local frame stack, a local sliding window beam adjustment optimization process is performed using an objective function to obtain the locally optimized pose, and the proportion of non-zero elements and the average squared residual of the Shure elimination matrix for each frame are updated.

[0173] The global keyframe set in the local frame stack is determined based on the proportion of non-zero elements in the Shure elimination matrix corresponding to the motion posture of each frame in the local frame stack and the average residual squared score of the single frame comprehensive quality score.

[0174] Based on the global keyframe set, the optimal global keyframe pose is obtained by using an incremental global sliding window beam adjustment optimization model.

[0175] The globally optimal keyframe pose is written back to the corresponding local frame stack. The objective function is reconstructed by distinguishing between setting keyframe weights and ordinary frame weights to obtain the updated objective function. Based on the updated objective function, the local sliding window beam adjustment optimization process is re-executed to obtain the corrected local optimized pose.

[0176] Based on the globally optimal keyframe pose and the corrected locally optimized pose, the optimized motion pose is obtained, and the current frame's motion pose is updated with the optimized motion pose.

[0177] A local frame stack refers to a continuous set of subframes obtained by segmenting the complete global frame sequence. It serves as the basic unit for performing local pre-optimization and quality assessment in stages, avoiding memory and computing power explosion caused by loading all frames at once. The length of a local frame stack refers to the number of frames contained in a single segmented subframe set, which is dynamically calculated as a percentage of the total number of frames in the entire process. The larger the total number of frames, the larger the number of frames in a single stack, ensuring that the segmentation granularity adapts to the acquisition duration.

[0178] The proportion of non-zero elements in the Schur elimination matrix refers to the ratio of the total number of non-zero numerical elements in the sparse matrix after Schur elimination of the BA normal equations to the total number of elements in the matrix. This proportion reflects the richness of observation constraints within the frame heap; a higher proportion indicates more constraints on features or planes. The mean squared residual refers to the squared mean of all reprojection, point-to-plane, and normal vector residuals within the heap after local BA convergence. This mean squared residual characterizes the degree of fit between the current local pose and the image observations; a smaller mean squared residual indicates higher motion pose accuracy.

[0179] The single-frame comprehensive quality score is a score that combines the proportion of non-zero values ​​in the Shure elimination matrix (constraint adequacy) and the average squared residual (fitting error), which can quantify the observation reliability and positioning accuracy of a single frame.

[0180] The global keyframe set refers to a subset of frames selected from all local frames that have high overall quality scores, rich observation constraints, and represent key perspectives of the scene. These frames are used for global centralized optimization to reduce the scale of global solution variables.

[0181] The incremental global sliding window bundle adjustment optimization model can be understood as a global BA model that does not load all historical frames at once, but sequentially incorporates them into the global window in segments according to the frame stack and iteratively optimizes them step by step. That is, it first optimizes the existing key frames, and then incorporates the newly segmented key frames for incremental updates, so as to reduce the scale of a single matrix solution.

[0182] The globally optimal keyframe pose refers to the high-precision SE(3) pose after all keyframes have been corrected following the convergence of the incremental global BA iteration, in order to eliminate cross-frame stacking drift and ensure the global consistency of the trajectory throughout the entire process.

[0183] Keyframe weights and ordinary frame weights are weighting coefficients used to reconstruct the global objective function. Since keyframes have high confidence and significant constraint value, they are assigned high weights. Ordinary frames have observation redundancy and larger errors, so they are assigned low weights to reduce their interference with the global optimization.

[0184] The updated objective function refers to the multi-residual cost function after differentiating keyframes and ordinary frames with weighted differences, which rebalances the contribution of frames of different quality to the overall error.

[0185] The corrected local optimized pose refers to the process of re-performing weighted local BA on each local frame stack based on the global optimal keyframe, so as to obtain the segmented frame motion pose after eliminating global drift.

[0186] The optimized motion pose is obtained by fusing the global optimal keyframe pose and the locally optimized pose after each segment correction, resulting in a unified corrected pose throughout the entire process. This pose is used to cover and update the original pose of all frames, and finally outputs the optimized pose of the current frame.

[0187] In this embodiment, when the motion state type of the current frame is motion stop, the specific process of global-local iterative optimization can include six sub-steps: full frame segmentation pre-optimization, single frame quality scoring to select global keyframes, incremental global optimization of keyframes, differentiated weighted secondary local correction, and output of globally unified optimized posture. Specifically: Step 1: Divide the local frame stack according to the percentage of the total number of frames. Count the total number of image frames accumulated during the complete motion cycle of the camera, set the segment percentage (e.g., each segment accounts for 10%~20% of the total number of frames), and calculate the number of frames corresponding to the fixed percentage, i.e., the length of the local frame stack.

[0188] The continuous complete frame sequence is divided sequentially according to this length. The remaining frames at the end that are less than the length of a single pile are treated as a separate last local frame pile, and several non-overlapping local frame piles that cover the entire acquisition sequence are output. By dividing the massive full frame data into blocks, memory overflow caused by loading all image, point cloud, and matrix data at once can be avoided.

[0189] Step 2: Perform sliding window BA optimization within the local frame stack, output the optimized local pose, and calculate two quality metrics. For each local frame stack, enable sliding window BA optimization separately, and input the original frame pose, image features, and depth plane observations within the local frame stack.

[0190] Gauss-Newton operations and Schur elimination iterations are performed until convergence, yielding the preliminary locally optimized pose for each frame within the local frame stack. After iteration, two quality metrics are recorded: the percentage of non-zero elements in the Schur elimination matrix for each frame within the local frame stack and the average squared residual. These two metrics can serve as the underlying basis for evaluating frame localization quality.

[0191] Step 3: Calculate the overall quality score for a single frame and filter the global keyframe set. Normalize each frame within the heap, mapping the proportion of non-zero elements in the Shure elimination matrix to a positive score of 0-1 (a higher proportion of non-zero elements results in a higher score), and inversely normalize the squared average residual (a smaller residual results in a higher score). Introduce preset weighting coefficients to balance these two quality metrics, and then sum them using weighted averages to obtain the overall quality score for a single frame.

[0192] Set a scoring threshold, traverse all local frame stacks, filter frames whose overall quality score is higher than the scoring threshold, and summarize them to form a unified global keyframe set.

[0193] Step 4: Incremental Global Sliding Window (BA) to solve for the global optimal keyframe pose. Using the selected global keyframes as the optimization subject, an incremental global window is constructed, that is, keyframes are gradually incorporated into the frame stack, and the global normal equations are assembled step by step.

[0194] The system integrates multiple residual objective functions, including point-to-plane, normal vector, and reprojection, and continuously iterates the global basis assessment (BA). After all keyframes are incorporated and the iteration converges, it outputs the globally optimal keyframe pose without cross-segment drift, uniformly correcting trajectory offsets between different frame stacks.

[0195] Step 5: Write back the global keyframe pose, reconstruct the objective function using differential weighting, and obtain the corrected local pose after secondary local BA. Write back the globally optimal keyframe pose to the corresponding local frame stack as the benchmark for segmented optimization.

[0196] By differentiating between high-keyframe weights and low-quality ordinary frame weights, the objective function is reconstructed to amplify the constraint effect of high-quality keyframes and reduce the error interference of low-quality ordinary frames. Using the updated weighted objective function, a sliding window BA is performed again on each local frame stack. After iterative convergence, the corrected local optimization posture, eliminating global segmentation bias, is obtained.

[0197] Step 6: Merge global keyframes and corrected local poses to update the motion pose of the current frame. Using the globally optimal keyframe pose as the global benchmark, and combining it with the corrected local poses of each segment, a unified optimized motion pose is generated for all frames throughout the process. Locate the latest acquired frame in the complete frame queue, and overwrite the original local pose with the corresponding optimized motion pose, which serves as the final output motion pose for the current frame under the motion-stop condition.

[0198] In some examples, when the motion state type of the current frame is a motion-stopped condition, no real-time response is required; the focus is on improving the final pose accuracy. The specific process is as follows: (1) Local frame heap partitioning.

[0199] Set the local frame stack length parameter: Set the local frame stack length as a percentage of the total number of frames accumulated during the motion, denoted as γ (for example, if γ=10%, that is, if the total number of frames accumulated is 1000, then the local frame stack length is 100 frames).

[0200] Sliding partitioning of local frame stacks: Using γ as the step size, the image sequence during motion is divided into continuously overlapping local frame stacks. ,in, j=1,2,...,m, adjacent stacks overlap by 50% to ensure inter-frame continuity and avoid attitude breakage after calibration.

[0201] (2) Local heap optimization and quality screening.

[0202] Local heap pre-optimization: heap for each local frame To optimize results in real time Using the initial values, perform local BA optimization (objective function as described above), and output the locally optimized pose. And update the proportion of non-zero elements in each frame. with the square of the mean residual .

[0203] Single-frame overall quality score: for each local frame stack Within the frames, based on the proportion of non-zero elements and the average squared residual Calculate the overall quality score for a single frame ,Right now: Where ω is the proportion of non-zero elements. The weighting coefficient is ω=0.6 (non-zero elements have a higher weighting, and frames with sufficient constraints and reliable poses are selected first). For local frame stack The maximum mean squared residuals within the stack are used to normalize the residual terms and avoid the impact of differences in residual scales between different stacks on the score.

[0204] Global keyframe filtering: based on overall quality score of a single frame. Sort the frames in descending order and extract the frames representing η percent (e.g., η=30%) from the local frame stack as global keyframes. , recorded as This reduces the computational cost of global optimization.

[0205] (3) Global optimization and local reverse correction.

[0206] Global BA optimization: Collect the global keyframe set of all local frame stacks. A global BA optimization problem is constructed (considering the long-term characteristics of the monitoring scenario, an incremental global sliding window BA optimization is used to solve the problem, reducing memory usage). The objective function continues the joint constraints of three types of residuals: ; Solving for the global optimal keyframe pose yields the solution. To eliminate long-term accumulated errors.

[0207] Local frame stack reverse correction: adjust the global optimal keyframe pose Return to the original local frame stack Increase the weight of keyframes in local BA optimization (e.g., keyframe weight β=2.0, non-keyframe (normal frame) weight). =1.0), strengthen the constraint of global accuracy on local conditions, reconstruct the local heap objective function, and obtain the corrected local optimized pose. The reconstructed local heap objective function is as follows: ; Among them, hour, ;exist hour, .

[0208] Iterative optimization involves resolving the local BA optimization and outputting the final optimized motion pose using a local heap. This completes global-local iterative corrections, ensuring the global consistency and local continuity of the final pose.

[0209] Monitoring data calibration: will optimize motion posture Replace the original real-time attitude data and update the attitude labels of the surveillance video to provide high-precision positioning basis for event tracing.

[0210] It should be understood that traditional solutions often neglect the details of local image observations in ordinary frames when only performing global optimization, and segmented optimization alone can lead to segmented drift. In this embodiment, however, a unified overall trajectory benchmark is first established using global keyframes, and then the process is refined to perform secondary local fitting of single-frame observations in each segment. This eliminates accumulated errors during real-time tracking, calibrates the final attitude, and ensures high-precision positioning for monitoring playback and event tracing. This achieves the dual goals of zero global drift and minimal local residuals, while also considering both single-frame observation fit and overall trajectory consistency. Furthermore, the hierarchical, progressive global optimization approach addresses the computational and memory bottlenecks associated with massive frame global optimization, breaking the limitation that traditional global BA can only run on a PC.

[0211] It should also be noted that when the cumulative number of frames for a still image is less than the preset number of frames ( When the motion state type of the current frame is a short-term static condition, the motion attitude of the previous frame is reused, meaning the motion attitude of the current frame is equal to the motion attitude of the previous frame. The covariance of the current frame is equal to the covariance of the previous frame. No real-time optimization is required.

[0212] It should also be noted that, in this embodiment, data-driven adaptive positive definite covariance estimation is also performed. Specifically, this includes: (1) Offline calibration stage.

[0213] Multiple scenes (low light, low texture, multi-plane) are continuously captured. During each capture, the camera is rigidly fixed at a typical position in the monitored scene. Multiple still images are captured repeatedly. After moving the camera and fixing it, n frames are captured continuously. After each movement, it is necessary to ensure that the images share a certain field of view with the previous images. By moving the camera multiple times in one capture, a set of n images can be obtained from multiple different positions in multiple different scenes.

[0214] The process of acquiring training data each time is as follows: Three consecutive positions are randomly selected within a scene. At each position, one frame from the acquired n frames is randomly selected as the training data. Thus, at positions j, j+1, and j+2 in the i-th scene, the k-th acquisition of training data will randomly generate three images captured from consecutive different positions. Thus, the total data obtained from N training data sessions is: For each of them Using the three-frame sliding window BA optimization method mentioned earlier, we obtain pose samples. and the residual of the optimization result Calculate the mean, i.e., the attitude mean. Optimized mean squared residual True value covariance Then, consider the true covariance. Perform Cholesky decomposition to obtain the lower triangular decomposition coefficient matrix. Decompose the lower triangular coefficient matrix This serves as the network output label. Repeating the above process for different scenes i and different locations j yields the dataset used to train the lightweight neural network. The network inputs a depth map and an intensity map or RGB image, along with the optimized mean squared residual. Output the 21-dimensional Cholesky lower triangular decomposition coefficients of the six-DOF attitude covariance matrix. By applying Softplus / ReLU activation to the diagonal elements, the strictly positive definite decomposition coefficients are ensured. A symmetric positive definite attitude covariance matrix is ​​obtained through matrix reconstruction. The Frobenius norm of the Cholesky decomposition coefficient space is used to construct the loss function. It is adapted to the characteristics of monitoring scenarios.

[0215] (2) Real-time covariance inference. For each frame of real-time motion posture... Input depth map features, intensity map features, and average residual squared value. The neural network outputs the Cholesky lower triangular decomposition coefficients. Reconstructing positive definite covariance The positive definite covariance Covariance propagation is used for EKF next frame initialization to ensure real-time tracking stability.

[0216] (3) Window update. Real-time stage: Real-time motion posture With positive definite covariance Store three frames in a sliding window as historical input for the next frame's EKF initialization.

[0217] In some examples, the estimation process of camera motion attitude is illustrated using a camera in a practical application. (I) Hardware Environment. A secure depth camera is used, adapted to mobile monitoring scenarios, with ≥16GB of memory and ≥512GB of storage for storing long-term monitoring data and optimization results.

[0218] (II) Software implementation steps.

[0219] (1) System initialization. Acquire the first three frames of images, and optimize each frame to obtain the initial motion posture. , , Initialize the three-frame window and covariance Preset initial 3D monitoring area (e.g., a cuboid: 5 meters long × 3 meters wide × 2.5 meters high), bind to the initial motion posture of the current frame. .

[0220] (2) Real-time processing flow (camera in motion).

[0221] a. Image Acquisition: Obtain the depth map of the current frame. Compared with the current frame intensity map .

[0222] b. Motion detection: Performs dual-criteria detection based on depth and the proportion of motion points in the characteristic optical flow to determine and mark the motion state.

[0223] c. Still Frame Processing: Accumulate the cumulative frame value C of the still image. If it is less than or equal to the preset frame value, reuse the motion posture of the previous frame and the covariance of the previous frame, and update the window.

[0224] d. Motion frame processing: EKF initialization: EKF initialization is completed based on the SE(3) inter-frame incremental second-order difference model to obtain the high-precision initial motion posture of the current frame; Three-frame sliding window BA optimization: Three-frame sliding window BA optimization is performed by using three types of residual joint constraints to solve the motion pose of the current frame. Record the proportion of non-zero elements with the square of the mean residual ; Monitoring Area Update: Calculate the real-time 3D monitoring area Output to the security protection system; Covariance inference: Update covariance, update window.

[0225] e. Execute in a loop until the camera status is marked as motion stopped.

[0226] (3) Offline optimization process (after camera movement stops).

[0227] a. Data preparation: Extract all real-time motion postures during the motion process. With quality parameters ; b. Local frame stacking: Divide the continuously overlapping local frame stacks according to the percentage of the cumulative total number of frames γ=10%; c. Local frame stack pre-optimization: Perform local BA optimization on each local frame stack and update the quality parameters; d. Keyframe selection: Extract a set of global keyframes, representing η=30% of the total quality score; e. Global BA optimization: Solving for the globally optimal keyframe pose; f. Local reverse correction: Increase the weight of key frames, re-optimize the local frame stack, and obtain the final optimized motion pose; g. Data storage: Replace the original real-time data with the final optimized motion posture and calibrated monitoring video.

[0228] (III) Parameter Setting Example. For example, motion detection parameter: preset depth difference threshold. meters, preset displacement threshold The preset depth difference threshold corresponding to the percentage of pixels and depth motion points. The preset feature threshold corresponding to the proportion of feature motion points =0.2, preset frame value =5 frames.

[0229] EKF parameter: Incremental threshold Damping coefficient Translation prior standard deviation Rotational prior standard deviation Among them, the translation prior standard deviation and the rotation prior standard deviation are the prior noise standard deviations of the three-frame sliding window EKF, which are used to construct the information matrix of the EKF; BA optimization parameters: maximum number of iterations (20), convergence step size threshold. ; Global-local optimization parameters: γ = 10% (local heap length), η = 30% (keyframe percentage), keyframe weight β = 2.0, percentage of non-zero elements. The weighting coefficient ω = 0.6; Covariance parameter: Minimum covariance matrix This is used to prevent the covariance obtained by BA from being too close to 0 (numerical singularity, overconfidence). 3D monitoring area parameters: Initial cuboid monitoring area The center point is at coordinates (0,0,2) meters, and the length, width, and height are (5,3,2.5) meters.

[0230] It should be noted that the camera-based motion attitude estimation method provided in this application can solve the following technical problems: (1) How to quickly distinguish between the stationary and moving states of the camera, avoid invalid calculations, ensure real-time tracking response speed, and adapt to the dynamic monitoring needs of security cameras; (2) How to fit the rigid body motion essence of the camera SE (3) to construct a high-precision motion prediction model, provide high-precision initial values ​​for nonlinear BA optimization, accelerate convergence, avoid local optima, and realize real-time calculation of camera motion attitude and real-time update of the three-dimensional monitoring area; (3) How to integrate multi-dimensional residual constraints to compensate for single One constraint defect, improve the stability and accuracy of pose solution in complex scenarios, such as the high ambiguity of pose solution in weak texture and low feature scenarios; (4) How to eliminate the accumulated error in the real-time tracking process through global-local iterative optimization after the motion stops, improve the final pose accuracy, and meet the needs of monitoring playback and event tracing; (5) How to select high-quality local frames to participate in global optimization, balance optimization efficiency and accuracy, and adapt to long-term monitoring data processing scenarios; (6) How to achieve adaptive, physically meaningful, and strictly positive covariance estimation, ensure the stability of real-time tracking, and avoid the offset of the three-dimensional monitoring area delineation.

[0231] The motion attitude estimation method based on a camera provided in this application is more suitable for real-time tracking of the three-dimensional monitoring area of ​​a security camera and global accuracy calibration after motion stops, and is adapted to complex monitoring conditions such as weak texture, dynamic motion, and long-term continuous shooting.

[0232] It is understood that the camera-based motion pose estimation method provided in this application has the following advantages compared to traditional camera motion pose estimation schemes: (1) Optimization of motion detection and real-time module. The processing time per frame is relatively small, which meets the real-time response requirements of monitoring scenarios, improves computing efficiency, and avoids invalid calculations.

[0233] (2) Proportional motion detection. Camera motion is identified by the proportion of effective motion points. It has strong noise resistance, greatly reduces the probability of false detection and missed detection, and ensures the stability of real-time tracking.

[0234] (3) SE(3) Rigid Body Incremental Motion Model. Based on Lie group inter-frame transform and logarithmic mapping Lie algebraic difference, it fits the real rigid body motion law of the camera and can effectively improve the convergence speed of BA.

[0235] (4) Multi-residual joint optimization. It integrates three types of constraints: planar geometry, normal vector rotation, and feature point reprojection, and adapts to all scene conditions such as sufficient / weak texture and rich planes. The ambiguity of pose solution is greatly reduced and the real-time tracking accuracy is significantly improved.

[0236] (5) Real-time tracking and monitoring area linkage. EKF-BA cascading provides high-precision real-time attitude, realizes dynamic synchronization of three-dimensional monitoring area, has small monitoring area delineation error, and improves the accuracy of safety protection.

[0237] (6) Global calibration after motion stops. Global-local iterative optimization eliminates accumulated errors and improves the final attitude accuracy, meeting the high-precision requirements of monitoring playback and event tracing.

[0238] (7) Frame quality screening mechanism. High-quality frames with sufficient constraints and small residuals are selected first to participate in global optimization, thereby reducing the computational load of global optimization while ensuring accuracy.

[0239] (8) Positive definite covariance guarantee. The offline calibration and Cholesky constrained network are trained with real-world data to achieve adaptive and physically reliable positive definite covariance, avoiding real-time tracking interruptions and improving the stability of the monitoring system.

[0240] (9) Strong adaptability to monitoring scenarios. Depth and optical flow multi-criteria noise-resistant motion detection, multi-scene and multi-constraint optimization (weak texture / weak light environment adaptation), and long time series data processing optimization are fully matched to the actual application scenarios of security cameras.

[0241] It should be understood that the sequence number of each step in the above embodiments does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this application.

[0242] Corresponding to the camera-based motion pose estimation method in the above embodiment, Figure 2 The diagram shows a schematic of a camera-based motion attitude estimation device according to an embodiment of this application. For ease of explanation, only the parts related to the embodiment of this application are shown.

[0243] Reference Figure 2 The camera-based motion pose estimation device 3 in this embodiment includes: The acquisition module 31 is used to acquire the previous frame image and the current frame image captured by the camera, wherein the previous frame image and the current frame image respectively contain a pair of depth map and intensity map.

[0244] The depth ratio determination module 32 is used to calculate the proportion of depth motion points in the current frame image based on the effective depth point set in the depth map of the previous frame image and the effective depth point set in the depth map of the current frame image.

[0245] The feature ratio determination module 33 is used to calculate the proportion of feature motion points in the current frame image based on the first feature point of the intensity map extracted from the previous frame image and the second feature point of the intensity map in the current frame image. The proportion of depth motion points and the proportion of feature motion points are used to determine the current frame motion state type of the camera.

[0246] The estimation module 34 is used to estimate the motion pose of the camera in the current frame based on the motion state type of the camera in the current frame.

[0247] Furthermore, the motion state types include dynamic working conditions, short-term static working conditions, and motion-stopped working conditions.

[0248] Estimation module 34 includes: The first estimation submodule is used to perform sliding window bundle adjustment optimization on multiple consecutive frames, including the current frame image, if the motion state type of the current frame is dynamic, and to obtain the motion posture of the current frame image corresponding to the current frame image. The initial motion posture of the current frame image is predicted by the pose prediction model. The pose prediction model is a kinematic model for predicting the motion posture of the next frame based on the three historical motion postures.

[0249] The second estimation submodule is used to take the motion posture of the previous frame corresponding to the previous frame image as the motion posture of the current frame if the motion state type of the current frame is a short-term static condition.

[0250] The third estimation submodule is used to perform global iterative optimization based on all frame images and the motion poses of all frames corresponding to all frame images during the entire camera motion process if the motion state type of the current frame is a motion stop condition, so as to obtain the motion pose of the current frame.

[0251] Furthermore, the camera-based motion pose estimation device 3 includes: The first determining submodule is used to determine that the current frame image is a moving image when the proportion of depth motion points is greater than or equal to a preset depth threshold, and / or the proportion of feature motion points is greater than or equal to a preset feature threshold, and then determine that the current frame motion state type of the camera is dynamic.

[0252] The second determination submodule is used to determine the current frame image as a still image when the proportion of depth motion points is less than a preset depth threshold and the proportion of feature motion points is less than a preset feature threshold.

[0253] The third determination submodule is used to determine the current operating state type of the camera as short-term static condition when the cumulative frame value of the still image is less than the preset frame value.

[0254] The fourth determination submodule is used to determine the current operating state type of the camera as motion-stopped condition when the cumulative frame value of the still image is greater than or equal to the preset frame value.

[0255] Furthermore, the depth ratio determination module 32 includes: The depth difference calculation unit is used to compare the effective depth point set in the previous frame image with the effective depth point set in the current frame image, and calculate the depth difference between the effective depth point set in the previous frame image and the effective depth point set in the current frame image point by point.

[0256] The point counting calculation unit is used to count the number of depth points whose depth difference is greater than a preset depth difference threshold, and to obtain the number of valid depth points.

[0257] The depth ratio determination unit is used to calculate the depth motion point ratio based on the ratio of the number of effective depth points to the total number of depth points in the effective depth point set of the current frame image.

[0258] Furthermore, the feature ratio determination module 33 includes: The feature matching unit is used to match the first feature point with the second feature point using a feature optical flow algorithm to obtain a matching point pair and calculate the displacement between the matching point pairs.

[0259] The feature statistics unit is used to count the number of motion feature points whose displacement is greater than a preset displacement threshold, thereby obtaining the number of valid motion feature points.

[0260] The feature ratio unit is used to calculate the proportion of feature motion points based on the ratio of the number of effective motion feature points to the total number of matched point pairs.

[0261] Furthermore, the pose prediction model is a second-order uniformly accelerated motion model.

[0262] The camera-based motion pose estimation device 3 includes: The pose extraction module is used to extract the three historical motion poses of the camera.

[0263] The acceleration calculation module is used to calculate the equivalent acceleration of the camera based on three frames of historical motion posture.

[0264] The incremental calculation module is used to predict the Lie algebra increment of the current frame based on the principle of short-time uniform acceleration motion and the equivalent acceleration.

[0265] The attitude prediction module is used to obtain the motion attitude prediction value of the current frame based on the state transition equation constructed from the Lie algebra increment and the left Jacobian matrix of the current frame.

[0266] The initial attitude estimation module is used to perform extended Kalman filter iterative estimation based on the current frame motion attitude prediction value, observation equation and preset attitude increment damping constraint to obtain the initial motion attitude of the current frame; wherein, the observation equation is obtained by fusing the point-to-plane geometric residual, the plane normal vector angle residual and the feature point reprojection residual.

[0267] Furthermore, the acceleration calculation module includes: The transformation increment calculation unit is used to calculate the Lie group transformation increment between adjacent frames based on the three frames of historical motion posture.

[0268] The transformation increment conversion unit is used to convert the transformation increment of a Lie group into a Lie algebra increment through a logarithmic mapping.

[0269] The acceleration calculation unit is used to perform second-order difference calculations on the Lie algebra increment to obtain the equivalent acceleration of the camera.

[0270] Furthermore, the first estimation submodule includes: The objective function construction unit is used to construct an objective function that includes point-to-plane geometric residuals, plane normal vector angle residuals, and feature point reprojection residuals by using multiple consecutive frames, including the current frame image, as a sliding window.

[0271] The optimization model determination unit is used to adaptively allocate residual weights corresponding to the point-to-plane geometric residual, the plane normal vector angle residual, and the feature point reprojection residual based on the scene texture and the number of planes, so as to obtain the optimization model to be solved.

[0272] The initial solution calculation unit is used to initialize the variables to be optimized within the sliding window based on the initial motion posture of the current frame, so as to obtain the initial solution of the variables to be optimized.

[0273] The variable value calculation unit is used to obtain the updated variable values ​​for each iteration based on the initial optimization solution, using the Gauss-Newton method and Schul elimination.

[0274] The first motion attitude determination unit is used to terminate the iterative calculation and obtain the motion attitude of the current frame when the preset iterative convergence condition is met.

[0275] Furthermore, the third estimation submodule includes: The local frame stack partitioning unit is used to divide the entire frame image sequence into multiple local frame stacks based on the local frame stack length. The local frame stack length is obtained as a percentage of the total number of frames accumulated throughout the entire camera motion process.

[0276] The local optimized attitude determination unit is used to perform local sliding window beam adjustment optimization processing based on the motion attitude of each frame in each local frame stack, using an objective function to obtain the local optimized attitude, and update the proportion of non-zero elements and the average squared residual of the Shure elimination matrix of each frame.

[0277] The global keyframe determination unit is used to determine the set of global keyframes in the local frame stack based on the proportion of non-zero elements in the Shure elimination matrix corresponding to the motion posture of each frame in the local frame stack and the comprehensive quality score of a single frame calculated by the average squared residual.

[0278] The optimal keyframe pose determination unit is used to solve for the global optimal keyframe pose based on the global keyframe set using an incremental global sliding window beam adjustment optimization model.

[0279] The update and correction unit is used to write back the globally optimal keyframe pose to the corresponding local frame stack. By distinguishing and setting keyframe weights and ordinary frame weights, the objective function is reconstructed to obtain the updated objective function. Based on the updated objective function, the local sliding window beam adjustment optimization process is re-executed to obtain the corrected local optimized pose.

[0280] The third motion pose determination unit is used to obtain the optimized motion pose based on the globally optimal keyframe pose and the corrected local optimized pose, and to update the motion pose of the current frame with the optimized motion pose.

[0281] It should be noted that the information interaction and execution process between the modules in the above-mentioned camera-based motion pose estimation device 3 are based on the same concept as the method embodiment of this application. For details on their specific functions and technical effects, please refer to the method embodiment section, and they will not be repeated here.

[0282] This application also provides a terminal device, such as... Figure 3 As shown, Figure 3 This is a schematic diagram of the structure of a terminal device provided in an embodiment of this application. (Refer to...) Figure 3 The terminal device 4 in this embodiment includes a memory 41, a processor 42, and a computer program stored in the memory 41 and executable on the processor 42. When the processor 42 executes the computer program, it implements the steps in any of the above-mentioned camera-based motion pose estimation method embodiments.

[0283] This application also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, can implement the steps in the above-described method embodiments.

[0284] This application provides a computer program product that, when run on a mobile terminal, enables the mobile terminal to implement the steps described in the various method embodiments.

[0285] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments of this application can be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include at least: any entity or device capable of carrying the computer program code to a photographic device / terminal device, a recording medium, a computer memory, a read-only memory (ROM), a random access memory (RAM), an electrical carrier signal, a telecommunication signal, and a software distribution medium. Examples include USB flash drives, portable hard drives, magnetic disks, or optical disks.

[0286] In the above embodiments, the descriptions of each embodiment have different focuses. For parts that are not described in detail or recorded in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0287] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0288] In the embodiments provided in this application, it should be understood that the disclosed apparatus / network devices and methods can be implemented in other ways. For example, the apparatus / network device embodiments described above are merely illustrative. For instance, the division of modules or units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between apparatuses or units may be electrical, mechanical, or other forms.

[0289] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0290] The technical features of the various embodiments described above in this application can be combined arbitrarily without conflict. For the sake of brevity, this specification does not describe all possible combinations, but as long as these combinations do not violate the technical spirit of this application, they should all be considered within the scope of this application. Based on the content disclosed in this application, those skilled in the art can reasonably combine, delete, or replace the technical features of the above embodiments according to actual needs, and such modifications and variations all fall within the protection scope of this application.

[0291] The above embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be included within the protection scope of this application.

Claims

1. A camera-based motion pose estimation method, characterized in that, include: Acquire the previous frame image and the current frame image captured by the camera, wherein the previous frame image and the current frame image respectively contain a pair of depth map and intensity map; The percentage of depth motion points in the current frame image is calculated based on the effective depth point set of the depth map in the previous frame image and the effective depth point set of the depth map in the current frame image. Based on the first feature point of the intensity map extracted from the previous frame image and the second feature point of the intensity map in the current frame image, the proportion of feature motion points in the current frame image is calculated; wherein, the proportion of depth motion points and the proportion of feature motion points are used to determine the motion state type of the camera in the current frame. The current frame motion pose of the camera is estimated based on the current frame motion state type of the camera.

2. The camera-based motion pose estimation method as described in claim 1, characterized in that, The motion state types include dynamic working conditions, short-term static working conditions, and motion-stopped working conditions; The step of estimating the current frame motion pose of the camera based on the current frame motion state type includes: If the motion state type of the current frame is the dynamic working condition, the initial motion posture of the current frame corresponding to the current frame image predicted by the pose prediction model is used as the initial value for optimization. Sliding window bundle adjustment optimization is performed on multiple consecutive frames including the current frame image to obtain the motion posture of the current frame corresponding to the current frame image. The pose prediction model is a kinematic model for predicting the motion posture of the next frame based on three frames of historical motion posture. If the motion state type of the current frame is the short-term static condition, then the motion posture of the previous frame corresponding to the previous frame image is taken as the motion posture of the current frame. If the motion state type of the current frame is the motion stop condition, then based on all frame images during the entire camera motion process and the motion postures of all frames corresponding to all frame images, a global iterative optimization is performed to obtain the motion posture of the current frame.

3. The camera-based motion pose estimation method as described in claim 2, characterized in that, Based on the depth motion point ratio and the feature motion point ratio, the current frame motion state type of the camera is determined, including: When the percentage of depth motion points is greater than or equal to a preset depth threshold, and / or the percentage of feature motion points is greater than or equal to a preset feature threshold, the current frame image is determined to be a moving image, and the current frame motion state type of the camera is determined to be the dynamic condition. When the percentage of depth motion points is less than the preset depth threshold and the percentage of feature motion points is less than the preset feature threshold, the current frame image is determined to be a still image. When the cumulative frame value of the still image is less than the preset frame value, the current operating state type of the camera is determined to be the short-term still condition. When the cumulative frame value of the still image is greater than or equal to the preset frame value, the current operating state type of the camera is determined to be the motion-stopped condition.

4. The camera-based motion pose estimation method as described in claim 3, characterized in that, The step of calculating the proportion of depth motion points in the current frame image based on the effective depth point set of the depth map in the previous frame image and the effective depth point set of the depth map in the current frame image includes: The effective depth point set in the previous frame image is compared with the effective depth point set in the current frame image, and the depth difference between the effective depth point set in the previous frame image and the effective depth point set in the current frame image is calculated point by point. The number of depth points whose depth difference is greater than a preset depth difference threshold is counted to obtain the number of valid depth points; The depth motion point ratio is calculated based on the ratio of the number of effective depth points to the total number of depth points in the effective depth point set of the current frame image.

5. The camera-based motion pose estimation method as described in claim 3, characterized in that, The step of calculating the proportion of feature motion points in the current frame image based on the first feature point of the intensity map extracted from the previous frame image and the second feature point of the intensity map extracted from the current frame image includes: The first feature point and the second feature point are matched using a feature optical flow algorithm to obtain a matching point pair, and the displacement between the matching point pairs is calculated. The number of motion feature points whose displacement is greater than a preset displacement threshold is counted to obtain the number of effective motion feature points. The percentage of the feature motion points is calculated based on the ratio of the number of effective motion feature points to the total number of matching point pairs.

6. The camera-based motion pose estimation method as described in claim 2, characterized in that, The pose prediction model is a second-order uniformly accelerated motion model; The process of predicting the initial motion pose of the current frame image corresponding to the current frame image through the pose prediction model includes: Extract three frames of the camera's historical motion pose; The equivalent acceleration of the camera is calculated based on the three historical motion postures. Based on the principle of short-time uniform acceleration motion, the Lie algebra increment of the current frame corresponding to the motion posture of the current frame is predicted according to the equivalent acceleration. Based on the state transition equation constructed using the current frame Lie algebra increment and the left Jacobian matrix, the predicted motion attitude value for the current frame is obtained. The initial motion attitude of the current frame is obtained by performing extended Kalman filter iterative estimation based on the current frame motion attitude prediction value, observation equation and preset attitude increment damping constraint; wherein, the observation equation is obtained by fusing the point-to-plane geometric residual, the plane normal vector angle residual and the feature point reprojection residual.

7. The camera-based motion pose estimation method as described in claim 6, characterized in that, The step of calculating the equivalent acceleration of the camera based on the three historical motion poses includes: Based on the three historical motion poses, calculate the Lie group transform increment between adjacent frames; The Lie group transformation increment is converted into a Lie algebra increment through a logarithmic mapping. The equivalent acceleration of the camera is obtained by performing second-order difference calculation on the Lie algebra increment.

8. The camera-based motion pose estimation method as described in claim 7, characterized in that, If the motion state type of the current frame is the dynamic condition, the initial motion posture of the current frame image predicted by the pose prediction model is used as the initial value for optimization. Sliding window bundle adjustment optimization is performed on multiple consecutive frames including the current frame image to obtain the motion posture of the current frame image, including: Using multiple consecutive frames, including the current frame image, as a sliding window, an objective function is constructed that includes the point-to-plane geometric residual, the plane normal vector angle residual, and the feature point reprojection residual. Based on the scene texture and the number of planes, the residual weights corresponding to the point-to-plane geometric residual, the plane normal vector angle residual, and the feature point reprojection residual are adaptively assigned to obtain the optimization model to be solved. Based on the initial motion posture of the current frame, the variables to be optimized within the sliding window are initialized to obtain the optimized initial solution for the variables to be optimized; Based on the optimized initial solution, the variable values ​​after each iteration are obtained by using the Gauss-Newton method iterative calculation and the Schul elimination operation. When the preset iteration convergence condition is met, the iteration calculation is terminated, and the motion posture of the current frame is obtained.

9. The camera-based motion pose estimation method as described in claim 8, characterized in that, If the motion state type of the current frame is the motion stop condition, then based on all frame images and the motion poses of all frames corresponding to all frame images during the entire camera motion process, a global iterative optimization is performed to obtain the motion pose of the current frame, including: Based on the local frame stack length, all frame image sequences are divided into multiple local frame stacks, wherein the local frame stack length is obtained as a percentage of the total number of frames accumulated throughout the entire camera motion process; Based on the motion posture of each frame within each local frame stack, the objective function is used to perform local sliding window beam adjustment optimization to obtain the local optimized posture, and the proportion of non-zero elements and the average squared residual of each frame's Shure elimination matrix are updated. Based on the proportion of non-zero elements in the Shure elimination matrix corresponding to the motion posture of each frame in the local frame stack and the average squared residual, the global keyframe set in the local frame stack is determined. Based on the global keyframe set, the global optimal keyframe pose is obtained by solving the incremental global sliding window beam adjustment optimization model. The globally optimal keyframe pose is written back to the corresponding local frame stack. The objective function is reconstructed by distinguishing and setting keyframe weights and ordinary frame weights to obtain the updated objective function. Based on the updated objective function, the local sliding window beam adjustment optimization process is re-executed to obtain the corrected local optimized pose. Based on the globally optimal keyframe pose and the corrected locally optimized pose, the optimized motion pose is obtained, and the current frame motion pose is updated with the optimized motion pose.

10. A camera-based motion pose estimation device, characterized in that, include: The acquisition module is used to acquire the previous frame image and the current frame image captured by the camera, wherein the previous frame image and the current frame image respectively contain a pair of depth map and intensity map; The depth ratio determination module is used to calculate the proportion of depth motion points in the current frame image based on the effective depth point set of the depth map in the previous frame image and the effective depth point set of the depth map in the current frame image. The feature ratio determination module is used to calculate the feature motion point ratio in the current frame image based on the first feature point of the intensity map extracted from the previous frame image and the second feature point of the intensity map in the current frame image; wherein, the depth motion point ratio and the feature motion point ratio are used to determine the current frame motion state type of the camera; The estimation module is used to estimate the current frame motion pose of the camera based on the current frame motion state type of the camera.

Images