Real-time robust visual slam positioning method for dynamic occlusion environment

Abstract

The application discloses a kind of real-time robust visual SLAM positioning methods for dynamic occlusion environment, it is related to visual positioning technical field, comprising the following steps: the current frame of camera field of view is obtained to the visual image, and the current frame of visual image is preprocessed to obtain the ambient light source direction vector of current frame;Based on the visual image and ambient light source direction vector of current frame, extract the accompanying shadow profile mask of current frame.The application guarantees the smoothness of camera trajectory by adaptive soft weight reduction mechanism, discards the traditional 0 or 1 hard rejection strategy, so that the influence of error term on target cost function is gradually weakened when the system faces dynamic occlusion, so as to ensure that the three-dimensional space transformation matrix solved by iteration is continuous and smooth, effectively avoids the violent jitter of camera pose.

Description

Technical Field

[0001] This invention relates to the field of visual positioning technology, specifically to a real-time robust visual SLAM localization method for dynamic occlusion environments. Background Technology

[0002] In the field of modern industrial warehousing and logistics, autonomous navigation of Automated Guided Vehicles (AGVs) is a core element for achieving intelligent operations. With the popularization of visual synchronous positioning and mapping technologies, many AGVs rely on onboard cameras to extract features of the surrounding environment in real time to estimate their spatial position. However, real warehouse environments are usually equipped with strong lighting equipment and have a large number of mobile forklifts moving around frequently, which makes the camera's field of view extremely susceptible to complex dynamic interference.

[0003] Currently, when dealing with such dynamic environments, mainstream visual navigation solutions typically employ passive outlier filtering strategies. Specifically, during the image matching or location calculation stages, if the system detects that certain feature points have excessively large calculation errors due to occlusion by moving objects, it immediately classifies them as noise and completely removes them from the dataset.

[0004] For a visual positioning system to maintain stable and continuous trajectory output, the fundamental prerequisite is the ability to stably track a sufficient number of static background points over a continuous time series. However, in brightly lit warehouses where forklifts frequently converge, the rapid movement of large equipment often causes sudden, large-scale visual obstruction. Because current technology can only react with a lag to feature points that have already incurred errors, when such large-scale sudden obstruction occurs, the system instantly identifies a large number of anomalies, thus abruptly discarding a large number of feature points. This abrupt, one-size-fits-all rejection operation leads to a precipitous reduction in the constraints of the system's underlying mathematical solution matrix, making it impossible to smoothly transition the position calculation between time points. This results in drastic changes in the output trajectory, and may even cause the entire navigation system to fail. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a real-time robust visual SLAM localization method for dynamic occlusion environments.

[0006] To achieve the above objectives, the technical solution of the present invention is as follows:

[0007] A real-time robust visual SLAM localization method for dynamically occluded environments includes the following steps:

[0008] Obtain the visual image of the current frame within the camera's field of view, and preprocess the visual image of the current frame to obtain the ambient light direction vector of the current frame;

[0009] Based on the visual image of the current frame and the ambient light direction vector, extract the accompanying shadow contour mask of the current frame;

[0010] Obtain the accompanying shadow contour mask of the previous frame, and perform displacement calculation by combining it with the accompanying shadow contour mask of the current frame to obtain the shadow diffusion velocity vector.

[0011] Extrapolation calculations are performed based on the accompanying shadow contour mask and shadow spread velocity vector of the current frame to generate an occlusion spread region for predicting the future occlusion range.

[0012] Obtain the set of static feature points for SLAM calculation, and determine whether the coordinates of each feature point in the set of static feature points overlap with the occlusion diffusion area.

[0013] If so, the feature point weights corresponding to the feature points in the overlapping region are reduced based on the preset attenuation coefficient, and the robust pose data of the camera is calculated and output using the reduced feature point weights and the static feature point set data of the non-overlapping region.

[0014] If not, the robust pose data of the camera is calculated and output directly using the static feature point set data.

[0015] A real-time robust visual SLAM localization system for dynamically occluded environments includes:

[0016] The data acquisition module is used to acquire the visual image of the current frame in the camera's field of view, and to preprocess the visual image of the current frame to obtain the ambient light direction vector of the current frame.

[0017] The mask extraction module is used to extract the accompanying shadow contour mask of the current frame based on the visual image of the current frame and the ambient light source direction vector;

[0018] The velocity calculation module is used to obtain the accompanying shadow contour mask of the previous frame, and combine it with the accompanying shadow contour mask of the current frame to perform displacement calculation and obtain the shadow diffusion velocity vector.

[0019] The extrapolation prediction module is used to perform extrapolation calculations based on the accompanying shadow contour mask and shadow diffusion velocity vector of the current frame to generate an occlusion diffusion region for predicting the future occlusion range.

[0020] The spatial overlap judgment and pose output module is used to acquire static feature point set data for SLAM calculation, and to determine whether the coordinates of each feature point in the static feature point set spatially overlap with the occlusion diffusion area. If so, the feature point weights corresponding to the feature points in the overlapping area are reduced based on a preset attenuation coefficient, and the robust pose data of the camera is calculated and output using the reduced feature point weights and the static feature point set data of the non-overlapping area. If not, the robust pose data of the camera is directly calculated and output using the static feature point set data.

[0021] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0022] This invention breaks away from the traditional SLAM's delayed response mode of first occluding and then removing objects. By extracting the ambient light direction vector and the accompanying shadow contour mask of the current frame, and combining this with the shadow displacement of previous and subsequent frames to calculate the shadow diffusion velocity vector, the occlusion diffusion region can be generated during extrapolation calculations. This means the system can use shadows as early warning signals for physical entities, predicting areas that will be occluded by dynamic objects in the near future, effectively reducing the risk of tracking loss due to sudden large-area occlusion in the SLAM system. Furthermore, through an adaptive soft weighting mechanism, the smoothness of the camera trajectory is ensured, abandoning the traditional hard 0 or 1 elimination strategy. For feature points falling within the predicted occlusion diffusion area, the system calculates the remaining safe time data before facing entity occlusion and converts it into a dynamic attenuation coefficient. The closer the feature point is to the shadow intrusion and the higher the urgency, the lower its weight in the back-end optimization information matrix will be continuously and smoothly reduced. This time-based soft weighting strategy makes the impact of the error term on the target cost function gradually weaken when the system faces dynamic occlusion, thus ensuring that the 3D spatial transformation matrix obtained by iterative solution is continuous and smooth, effectively avoiding severe camera pose jitter. Attached Figure Description

[0023] The disclosure of this invention is illustrated with reference to the accompanying drawings. It should be understood that the drawings are for illustrative purposes only and are not intended to limit the scope of protection of this invention. In the drawings, the same reference numerals are used to refer to the same parts. Wherein:

[0024] Figure 1 This is a diagram illustrating the method steps of the present invention;

[0025] Figure 2 This is a flowchart of the present invention. Detailed Implementation

[0026] It is readily understood that, based on the technical solution of this invention, those skilled in the art can propose various interchangeable structural methods and implementations without altering the essential spirit of the invention. Therefore, the following detailed embodiments and accompanying drawings are merely illustrative examples of the technical solution of this invention and should not be considered as the entirety of the invention or as limitations or restrictions on the technical solution of this invention.

[0027] Application Overview:

[0028] In the field of autonomous driving and robot navigation in complex scenarios such as modern industrial warehousing and logistics, the continuity and smoothness of the output trajectory of the visual simultaneous localization and mapping (SLAM) system are regarded as key indicators for measuring navigation reliability. This high-precision pose estimation is essentially a continuous optimization process of multi-view geometric constraints at the underlying mathematical framework. That is, by continuously capturing static background feature points in the environment through the camera, the spatial geometric constraints are transferred to the back-end optimizer without loss by using the stable reprojection of the feature points in the time series, thereby fitting a running trajectory with specific smoothness characteristics in three-dimensional space.

[0029] However, existing technologies lack a forward-looking prediction mechanism for the movement trends of dynamic obstacles and the evolution of field-of-view occlusion, resulting in an inability to smoothly handle the precipitous drop in feature point weights under sudden large-area occlusion. Specifically, the hysteresis response manifests as the system being able to identify feature points that are occluded and cause large errors, but often only passively removing them after the observation data has been severely contaminated. On the other hand, the one-size-fits-all hard filtering manifests as the system directly reducing the weights of affected feature points to zero when faced with sudden occlusion, causing violent oscillations in the structure of the underlying optimization matrix. As a result, a smooth transition relationship cannot be established between the dynamic changes in the environment and the state estimation of the system, causing the system to abruptly strip away available constraints, thereby affecting the continuity of positioning solutions and the safety of navigation control.

[0030] For example, in real-time navigation of industrial warehouses with dense forklift traffic, existing visual positioning systems, when performing real-time path planning for AGVs, can only determine feature point failure after a surge in error using conventional robust kernel functions or outlier checks. However, they cannot use the accompanying shadows cast by moving forklifts under strong ambient light to predict their occlusion trajectory in advance. Furthermore, when a large forklift suddenly crosses the AGV's field of vision, the system only records the apparent sudden drop in the number of effective feature points, failing to detect the risk of distortion in the underlying information matrix caused by a sharp reduction in local constraints. Specifically, the system misjudges the sudden large-scale failure of feature points as a drastic spatial displacement of the AGV itself, or incorrectly classifies trajectory drift caused by insufficient constraints as random noise from hardware sensors. As a result, the AGV's underlying control system receives abruptly abnormal data and cannot form a smooth trajectory that conforms to the principles of vehicle kinematics.

[0031] If the above problems are not solved, the visual navigation system will continue to lose its ability to smoothly predict trajectories in highly dynamic environments. In particular, the unresolved hysteresis response will leave the system in a passive and vulnerable state when dynamic objects frequently pass by, causing the loss path of effective feature observations to deviate from the principle of continuous tracking, thereby weakening the robustness of pose estimation. At the same time, the uncorrected one-size-fits-all hard filtering problem will cause the mathematical constraints within the optimization framework to break, making it impossible for the camera pose calculation to present smooth transition kinematic characteristics, ultimately causing the output trajectory to lose its due continuity and stability. Thus, the drastic changes in state estimation will systematically hinder the AGV from achieving the core goal of safe and reliable navigation in complex dynamic warehouses.

[0032] like Figure 1-2 As shown, a real-time robust visual SLAM localization method for dynamic occlusion environments includes the following steps:

[0033] Step 1: Acquire the visual image of the current frame from the camera's field of view and preprocess it to obtain the ambient light source direction vector. This step is the extraction and benchmark establishment of the ambient light source direction vector: The system first acquires the current frame's color visual image captured in real time by the camera's field of view mounted on a mobile device (such as an AGV). To eliminate interference from complex colors and local metallic reflections in the industrial environment, the system converts the color image into a two-dimensional brightness distribution and extracts the extreme center where the brightness gradient converges and exceeds the exposure threshold through spatial smoothing and denoising. This extreme center represents the candidate point of the main light source on the two-dimensional image. Subsequently, the system performs inverse reprojection using the camera's intrinsic parameter model, mapping the two-dimensional light source coordinates to the three-dimensional physical coordinate system and normalizing the connection with the camera's optical center, thereby accurately extracting the ambient light source direction vector of the current frame. This vector provides a reliable three-dimensional spatial geometric benchmark for subsequent deduction of the physical causes of shadows.

[0034] Step 2: Based on the visual image of the current frame and the ambient light source direction vector, extract the accompanying shadow contour mask of the current frame. This step is a physically constrained accompanying shadow contour mask extraction: After obtaining the ambient light source direction vector in Step 1, the system performs structural edge detection in the current frame image to identify the contours of potential physical obstacles. To accurately distinguish between true dynamic shadows and the inherent dark background of the environment, the system uses the identified entity edges as a reference and strictly delineates the backlight search area along the backward extension line of the light source projection. Within this physically constrained area, the system compares the attenuation transmittance of local brightness and filters isolated noise points through connected component area. Finally, the shadow areas with entity companion relationships are binarized and packaged as a whole, and the output is the accompanying shadow contour mask of the current frame.

[0035] Step 3: Obtain the accompanying shadow contour mask of the previous frame and perform displacement calculation by combining it with the accompanying shadow contour mask of the current frame to obtain the shadow diffusion velocity vector. This step is the kinematic derivation of the shadow diffusion velocity vector: the system extracts the accompanying shadow contour mask saved in the previous frame and performs a temporal comparison with the mask of the current frame. To meet the real-time requirements of the SLAM system, the system abandons global pixel-level matching and instead calculates only the normal vectors of the mask edges, filtering out the set of diffusion leading pixels with an outward expansion trend. Using these leading pixels as a reference, the system quickly retrieves the corresponding historical positions of the previous frame within a local window along the opposite direction of the normal vector, and then calculates the two-dimensional pixel-level displacement difference. Combining the time interval between two frames and the normal direction constraint, the system generates a shadow diffusion velocity vector containing scalar magnitude and direction information, transforming the static light and shadow state into quantized kinematic data.

[0036] Step 4: Extrapolate the accompanying shadow contour mask and shadow diffusion velocity vector of the current frame to generate an occlusion diffusion region for predicting future occlusion range. This step is the geometric extrapolation of the predictive occlusion diffusion region: Based on the instantaneous motion state of the current frame, the system initiates a forward-looking prediction mechanism. The system introduces a preset future prediction time window, combines it with the shadow diffusion velocity vector, and calculates the future two-dimensional predicted pixel offset. The system performs translational projection on the outer contour of the current shadow mask according to this offset, and constructs a swept envelope polygon covering the current position to the future predicted position through geometric closure operations. After using Boolean clipping to filter out out-of-bounds data using the camera field of view boundary, the system generates a clear occlusion diffusion region. This region physically delineates the range of high-risk pixels that will be occluded by dynamic obstacles in the near future.

[0037] Step 5: Obtain the static feature point set data for SLAM calculation and determine whether the coordinates of each feature point in the static feature point set spatially overlap with the occlusion diffusion region. This step is a spatial risk screening of the SLAM feature point set: After completing the forward prediction of the dynamic environment, the system connects to the front-end module of conventional visual SLAM to obtain the static feature point set data for pose calculation. The system rounds and aligns the sub-pixel coordinates of the feature points and uses them as indices to project into the previously generated occlusion diffusion region mask matrix. Through this cross-comparison of coordinate indices, the system accurately determines which feature points fall into the high-risk overlapping region that is about to be occluded, and which feature points are still in the unaffected safe non-overlapping region, thus achieving physical isolation and risk classification of the feature point data.

[0038] Step Six: If yes, then the feature point weights corresponding to the feature points in the overlapping region are reduced based on the preset attenuation coefficient, and the robust pose data of the camera is calculated and output using the reduced feature point weights and the static feature point set data of the non-overlapping region; if no, the robust pose data of the camera is directly calculated and output using the static feature point set data. This step is based on robust pose calculation using adaptive attenuation and hybrid weighting: For the classification results in Step Five, the system executes a differentiated soft processing strategy:

[0039] If a feature point is determined to fall within an overlapping region, the system no longer performs the traditional direct discard operation, but instead triggers an adaptive weight reduction mechanism. The system combines a preset attenuation coefficient; the higher the urgency of a feature point's distance from occlusion, the smaller the attenuation coefficient and the lower the feature point's weight. This smoothly weakens the optimization weights of feature points within high-risk occlusion zones. Simultaneously, the initial weights of safe feature points in non-overlapping regions are fully preserved. Subsequently, the system re-integrates the weighted feature points with the safe feature points and inputs them together into the cost function of the backend optimizer for least-squares iterative solution until convergence, at which point robust camera pose data is output.

[0040] If the preliminary judgment result is negative, meaning there are no feature points in the current field of view facing occlusion risk, the system directly uses the initial weight data of the entire set of static feature points to perform conventional optimal state estimation, calculate and output the camera pose.

[0041] Through the coordinated operation of the above six steps, the environmental shadows were successfully used as early warning sentinels for dynamic entities, transforming passive anomaly removal into proactive risk prediction and soft weighting transition, preserving the underlying multi-view geometric constraints to the greatest extent, and ensuring the continuity and reliability of the visual navigation system in complex dynamic environments.

[0042] The core innovation of this invention lies in breaking away from the traditional SLAM's delayed response mode of occlusion followed by removal. By extracting the ambient light direction vector and accompanying shadow contour mask of the current frame, and combining this with the shadow displacement of previous and subsequent frames to calculate the shadow diffusion velocity vector, the occlusion diffusion region can be generated during extrapolation calculations. This means that the system can use shadows as early warning signals for physical entities, predicting in advance the areas that will be occluded by dynamic objects in the future, effectively reducing the risk of tracking loss due to sudden large-area occlusion in the SLAM system. Furthermore, through an adaptive soft weighting mechanism, the smoothness of the camera trajectory is ensured, abandoning the traditional 0 or 1 hard removal strategy. For feature points falling within the predicted occlusion diffusion area, the system calculates the remaining safe time data before facing entity occlusion and converts it into a dynamic attenuation coefficient. The closer the feature point is to the shadow intrusion and the higher the urgency, the lower its weight in the back-end optimization information matrix will be continuously and smoothly reduced. This time-based soft weighting strategy makes the impact of the error term on the target cost function gradually weaken when the system faces dynamic occlusion, thus ensuring that the 3D spatial transformation matrix obtained by iterative solution is continuous and smooth, effectively avoiding severe camera pose jitter.

[0043] In complex visual environments (such as industrial scenes with metallic reflections), traditional vision systems struggle to accurately and reliably extract the light source projection direction from 2D pixel images in the 3D physical world. Because localized material reflections can create false highlights, directly extracting the brightest point in the image as the light source direction leads to a shift in the calculated light source vector, resulting in significant geometric errors in subsequent shadow projection region predictions that rely on this vector. Therefore, this paper proposes: acquiring the visual image of the current frame within the camera's field of view and preprocessing it to obtain the ambient light source direction vector for the current frame, as detailed below:

[0044] The ambient light direction vector of the current frame is obtained by preprocessing the visual image of the current frame, specifically including:

[0045] Extract the luminance channel components of each pixel in the visual image of the current frame and generate a two-dimensional global luminance distribution matrix;

[0046] Perform convolution filtering on the global brightness distribution matrix to filter out local highlight noise data caused by local material reflections and generate a smooth brightness matrix;

[0047] Spatial gradient difference calculation is performed on the smoothed brightness matrix to extract the extreme point region where the brightness gradient vector points to converge and the brightness value is greater than the preset exposure threshold. The geometric center of the extreme point region is extracted as the two-dimensional candidate pixel coordinates of the main light source.

[0048] Obtain camera intrinsic parameter matrix data, and use the camera intrinsic parameter matrix data to perform inverse reprojection transformation on the two-dimensional main light source candidate pixel coordinates, mapping the two-dimensional main light source candidate pixel coordinates from the two-dimensional image coordinate system to the camera's three-dimensional physical coordinate system, generating three-dimensional light source spatial point coordinates.

[0049] An initial direction vector is constructed starting from the optical center coordinates of the camera and ending at the coordinates of the three-dimensional light source spatial points. The initial direction vector is then normalized to generate the ambient light source direction vector for the current frame.

[0050] Luminance channel component: refers to the grayscale value that reflects the intensity of light after the color image has been converted to a color space and the chromaticity information has been removed.

[0051] Localized highlight noise: In industrial settings with metallic or smooth surfaces, bright pixel clusters formed by localized material reflection caused by direct sunlight, which are not actually light sources.

[0052] Camera intrinsic parameter matrix: A matrix of parameters that characterize the internal geometry and optical features of the camera, including the camera focal length and optical center coordinates, used to establish the mapping relationship between the two-dimensional image plane and the light rays in three-dimensional space.

[0053] Inverse reprojection transformation: The calculation process of projecting the pixel coordinates on a two-dimensional image back into a three-dimensional physical coordinate system along the line of sight using the camera's imaging model.

[0054] Step 1: Extract the luminance channel components to generate a global luminance distribution matrix: Obtain the RGB color visual image of the current frame, traverse each pixel in the image, and sum the weighted values ​​of its red, green, and blue channels to calculate the luminance channel components of that pixel.

[0055] ;

[0056] in, and These represent the horizontal and vertical coordinates of a pixel in a two-dimensional image coordinate system, respectively. , and These represent the red, green, and blue channel values ​​of the pixel, respectively. This represents the calculated luminance channel component; by simulating the sensitivity of the human visual system to different wavelengths of light, the three-channel color information is reduced and compressed into single-channel illumination intensity information, eliminating interference from the object's own color. After traversing all pixels, it is determined by all... The dataset generates a two-dimensional global brightness distribution matrix, denoted as... .

[0057] Step 2: Convolution filtering operation to generate a smooth brightness matrix: In order to eliminate the specular noise caused by local material reflections, the global brightness distribution matrix is ​​processed. Perform a two-dimensional Gaussian convolution filter operation:

[0058] ;

[0059] in, This indicates that the smoothed brightness matrix generated after filtering is in The value at the coordinates. This represents the preset filter kernel radius, which is usually an integer between 2 and 4. This represents the weight value of the two-dimensional Gaussian filter kernel function at this coordinate, and its calculation depends on the preset standard deviation parameter. , The value range is typically set from 1.0 to 3.0, and the value is adjusted positively according to the intensity of reflection in the working environment. Gaussian filtering can perform distance-weighted averaging of the brightness values ​​in the neighborhood of the center pixel, thereby effectively smoothing isolated abrupt highlight pixels while preserving the large-area brightness gradient trend that reflects the actual direction of the light source.

[0060] Step 3: Extract the coordinates of candidate pixels for the two-dimensional main light source: Perform spatial difference on the smoothed brightness matrix, calculate the brightness gradient in the horizontal and vertical directions, and find regions where the brightness converges (i.e., dark around the edges and bright in the center). First, determine whether the pixels meet the preset exposure threshold conditions:

[0061] ;

[0062] in, This indicates the preset exposure threshold. For 8-bit grayscale images, its value is typically set between 200 and 240. Filtering out values ​​greater than... The connected pixel group is taken as the region of extreme points, denoted as set. .

[0063] Subsequently, the gray-weighted geometric center of the extreme point region is calculated:

[0064] ; ;

[0065] in, and These represent the horizontal and vertical floating-point coordinates of the calculated 2D candidate pixel coordinates of the main light source, respectively. Since real strong light sources often cover multiple pixels, simply taking the maximum value is easily affected by residual noise. By using the brightness value as a weight to calculate the centroid, sub-pixel-level accurate positioning can be achieved, improving the stability of coordinate extraction.

[0066] Step 4: Inverse reprojection to generate 3D light source spatial point coordinates: Obtain the camera intrinsic parameter matrix, and transform the obtained 2D main light source candidate pixel coordinates from the 2D image coordinate system to the 3D physical coordinate system. The inverse reprojection formula is as follows:

[0067] ;

[0068] in, , , This represents the X, Y, and Z axis values ​​of the generated 3D light source spatial point coordinates in the camera's physical coordinate system. This represents the inverse of the camera intrinsic parameter matrix. This represents the preset reference depth scale. Since only the direction vector needs to be solved, the absolute value of the depth does not affect the orientation of the vector. The value is assigned to a constant 1.0. By establishing a physical inverse mapping between two-dimensional pixels and three-dimensional light rays, the coupling between the light source position and internal parameters such as image resolution and camera focal length is decoupled, unifying them into a three-dimensional physical measurement space.

[0069] Step 5: Normalize the modulus to generate the ambient light direction vector: Using the camera's optical center as the origin of the 3D coordinate system, i.e., the coordinates are... An initial direction vector is constructed with the coordinates of the three-dimensional light source point as the endpoint, denoted as . : ;

[0070] To facilitate subsequent vector-based geometric projection calculations and eliminate the influence of absolute coordinate values, the magnitude of the initial direction vector is normalized:

[0071] ;

[0072] in, A three-dimensional Euclidean scalar representing the length of the initial direction vector; This represents the ambient light direction vector of the current frame after modulus normalization. By converting a coordinate vector with depth assumptions into a unit vector, it outputs a three-dimensional vector with a strict length of 1 that only represents the spatial light projection direction, providing standardized mathematical input for subsequent calculations of shadow projection rules.

[0073] Example 1: Assume that after the image region being processed undergoes transformation to generate a global luminance distribution matrix by extracting luminance channel components and smoothing filtering to generate a smooth luminance matrix by convolution filtering, a local luminance value greater than a preset exposure threshold is generated. (Value 240) Extreme point region .

[0074] This region contains 4 pixels, and its smooth brightness matrix The numerical distribution table is as follows:

[0075]

[0076] Extracting candidate pixel coordinates of the main light source:

[0077] Calculate the weighted sum of gray levels in the region of extreme points: ;

[0078] Calculate horizontal floating-point coordinates :

[0079] ;

[0080] Calculate vertical floating-point coordinates :

[0081] ;

[0082] Therefore, the coordinates of the candidate pixels for the two-dimensional main light source are: .

[0083] Inverse reprojection: Assuming the obtained camera intrinsic parameter inverse matrix This is a simplified diagonal array, and the known focal length-related parameters result in a pixel transformation ratio of 0.002 and an optical center offset of 0.5. A preset reference depth scale is used. The value is assigned to 1.0.

[0084] Perform the inverse reprojection transformation operation:

[0085] ;

[0086] ;

[0087] ;

[0088] The coordinates of the three-dimensional light source point are obtained as follows: .

[0089] Modulus normalization:

[0090] Initial direction vector That is .

[0091] Calculate its three-dimensional Euclidean length scalar: ;

[0092] Calculate the direction vector of ambient light source Each component:

[0093] X-axis component: ;

[0094] Y-axis component: ;

[0095] Z-axis component: ;

[0096] The final output is the ambient light direction vector of the current frame. .

[0097] In the aforementioned technique, the color image is first converted into a global brightness distribution matrix, and local abrupt specular noise is removed through spatial convolution filtering. Next, a brightness threshold is set, and spatial gradient difference calculation is used to find the geometric center of the brightness convergence region as the two-dimensional coordinates of the main light source. Finally, an inverse reprojection transformation is performed using the camera's intrinsic parameter matrix model to transform the denoised two-dimensional pixel coordinates into a three-dimensional physical coordinate system, and vector normalization is performed by connecting the coordinates to the camera's optical center. This effectively eliminates specular interference caused by local material reflections, improving the accuracy of two-dimensional light source coordinate positioning. Simultaneously, through the inverse projection and normalization calculation of the camera's intrinsic parameters, the pixel features of the plane are converted into physically meaningful three-dimensional direction vectors. This ambient light source direction vector provides a reliable three-dimensional spatial geometric reference for subsequent steps, including calculating the backlight area along the light rays based on obstacle contours and constructing the occlusion diffusion geometric envelope.

[0098] Existing image shadow detection schemes often rely on globally fixed brightness thresholds for segmentation. This approach easily misclassifies objects that are inherently dark as shadows and cannot distinguish which shadows are generated by moving obstacles. Consequently, the extracted shadow data is filled with irrelevant background noise, failing to provide reliable dynamic object motion warning information for visual positioning systems. Therefore, this paper proposes: based on the visual image of the current frame and the ambient light source direction vector, extracting the accompanying shadow contour mask of the current frame, specifically including:

[0099] Perform pixel-level edge detection calculations on the visual image of the current frame, extract the contour boundaries with high gradients in the image, and generate a set of structural edge pixels;

[0100] Using the set of pixels at the structural edge as the reference position, and following the direction of the reverse extension of the ambient light source direction vector, a ray projection dilatation calculation is performed in the two-dimensional image coordinate system to delineate the backlight search area.

[0101] Within the pixel range covered by the backlight search area, the current brightness component data of each pixel is extracted, and the difference is calculated with the local background average brightness data of the adjacent non-backlight side to filter out candidate dark pixels whose brightness attenuation is within the preset shadow transmittance range.

[0102] Perform connected component calculations on candidate dark pixels, filter out isolated pixel blocks with an area smaller than a preset noise threshold, and obtain continuous shadow connected component data.

[0103] The shadow connected component data is binarized and labeled in the original image coordinate system. The pixel regions labeled as target states are packaged as a whole to generate and output the accompanying shadow contour mask of the current frame.

[0104] Accompanying shadow contour mask: A binary image matrix representing the continuous dark areas projected onto the ground on the physically shaded side after an object blocks ambient light. This mask distinguishes between shadowed areas (target state) and unshadowed areas (background state).

[0105] Structural edge pixel set: A two-dimensional pixel coordinate sequence representing the physical outer boundary of a solid obstacle where local brightness values ​​in the image undergo drastic spatial changes.

[0106] Backlight search area: Starting from the edge pixels of the structure, a search range is defined on the two-dimensional image plane that extends strictly in the opposite direction of the ambient light. This area conforms to the physical and optical laws of shadow generation.

[0107] Shadow transmittance: The ratio (or relative difference multiple) between the current observed brightness value of a pixel and the average brightness value of the local background in its adjacent non-shaded areas. This metric reflects the degree of energy attenuation when light is blocked by an obstacle.

[0108] Connected component: In a two-dimensional image matrix, a set of pixel coordinates that have the same data attributes (such as all being candidate dark pixels) and are spatially adjacent to each other (usually 4-neighborhood or 8-neighborhood).

[0109] Step 1: Pixel-level edge detection calculation: The system performs spatial first-order difference calculation on the brightness distribution of the current frame's visual image to extract high-gradient contour boundaries in the image. The formula for calculating the gradient magnitude of a pixel is as follows:

[0110] ;

[0111] in, The coordinates in the two-dimensional image coordinate system are The pixel gradient magnitude of the pixel; This represents the brightness difference value of the pixel in the horizontal direction; This represents the brightness difference value of the pixel in the vertical direction.

[0112] After calculation, the gradient magnitude is compared with the preset edge detection threshold, and the filtering conditions are as follows:

[0113] ;

[0114] in, This represents the preset edge detection threshold. For 8-bit grayscale images, its value is typically set between 40 and 80. Since real-world obstacles usually have a significant contrast with their surrounding background, filtering can remove smooth background areas and retain only the physical contour boundaries that produce abrupt changes. These coordinates that meet the threshold condition integrate the set of structural edge pixels.

[0115] Step 2: Ray casting dilatation to define the backlight search area: Obtain the 3D ambient light source direction vector, extract its horizontal and vertical components to construct the projection vector of the 2D plane, and normalize it:

[0116] ;

[0117] in, and These are the component values ​​of the three-dimensional ambient light source direction vector in the X and Y axes, respectively; This represents the unit vector of light source projection onto a two-dimensional plane.

[0118] Then, starting from the coordinates in the set of pixels at the edge of the structure, a ray projection is performed along the backward extension of that vector: ;

[0119] in, Represents a starting reference coordinate in the set of pixels at the edge of the structure; The step size distance scalar represents the ray projection, and its value range is set to 1 to... (Preset maximum projection length) The value is typically between 30 and 60 pixels. This represents the two-dimensional coordinates of the backlight search point calculated after extending in the opposite direction. Since shadows can only appear on the backlight side of an entity in physics, the search boundary is established by projecting a reverse ray, directly excluding all pixels on the light-facing side of the image. This greatly reduces the search space for subsequent calculations and avoids false positives.

[0120] Step 3: Extracting Luminance Components and Filtering Candidate Dark Pixels: Within the defined backlight search area, the system extracts the current luminance component data of each pixel and calculates the average background luminance of adjacent non-backlight pixels. The formula for calculating shadow transmittance is as follows:

[0121] ;

[0122] in, Indicates the coordinates that belong to the backlight search area; This indicates the actual extracted current luminance component data at this coordinate point; This represents the local average background brightness data of the safe background area adjacent to the edge of the entity and located on the non-backlit side; This represents the calculated shadow transmittance.

[0123] Subsequently, the transmittance was compared with a preset range:

[0124] ;

[0125] in, This indicates the lower limit of the preset shadow transmittance range (usually a value of 0.3). This represents the upper limit of the preset shadow transmittance range (typically 0.7). Shadows are essentially proportional attenuations of light caused by occlusion, not absolute black. Adaptive filtering using relative attenuation ratios allows the system to operate independently of globally fixed grayscale thresholds, ensuring accurate shadow identification on both light and dark surfaces.

[0126] Step 4: Connected Component Calculation and Isolated Pixel Block Removal: Spatial aggregation is performed on candidate dark pixels that meet the above transmittance conditions to extract independent connected component blocks, and the total number of pixels in their area is calculated.

[0127] ;

[0128] in, This represents the first generation generated after connectivity analysis. Each independent connected component; Represents the pixel coordinates belonging to this connected component; This represents the total number of pixels in the area of ​​the connected region.

[0129] Perform noise filtering: ;

[0130] in, This represents the preset noise threshold, typically set between 100 and 200 pixels. Due to random thermal noise inherent in the image sensor itself, or the presence of tiny dark stains on the ground, area filtering eliminates these unstructured, isolated tiny pixel blocks, ensuring that the retained continuous shadow connected component data corresponds to dynamic obstacles with a certain macroscopic physical volume.

[0131] Step 5: Binarization and transformation to generate accompanying shadow contour mask: Project the retained effective connected component data back into the original 2D image coordinate system and perform Boolean assignment:

[0132] ;

[0133] in, This represents the set of valid continuous shaded connected components retained after area threshold filtering. Indicates coordinates The output mask state values ​​are 1 for the target state (i.e., belonging to the accompanying shadow) and 0 for the background state. By unifying the data format through Boolean assignment, the multi-channel, multi-grayscale visual image is transformed into a logically simplified binary matrix, providing a standardized data format for subsequent mask displacement calculations between different frames.

[0134] Example 2: Continuing from the previous example, the ambient light direction vector of the current frame is calculated as follows: .

[0135] Ray projection dilation delineates the region: extracting the horizontal component of the ambient light source direction vector. and vertical components .

[0136] Calculate its modulus in a two-dimensional plane: .

[0137] Generate a two-dimensional plane projected unit vector:

[0138] .

[0139] Assuming pixel-level edge detection is performed, a starting reference coordinate is extracted from the set of structural edge pixels. .

[0140] Perform ray projection calculations along the backward extension line, assuming the projection step size is a scalar distance. :

[0141] .

[0142] The coordinates It is then included in the backlight search area.

[0143] Sub-calculation 2: Brightness Difference Extraction and Filtering: The system extracts relevant brightness data from the retrieved coordinate points. The relationship mapping table is as follows:

[0144]

[0145] Perform transmittance calculation and screening:

[0146] For coordinates Shadow transmittance Numerical value In Within the range, pixels are identified as candidate dark pixels.

[0147] For coordinates Shadow transmittance Numerical value In Within the range, pixels are identified as candidate dark pixels.

[0148] For coordinates Shadow transmittance Numerical value Greater than the upper limit Pixels that are not in the dark area are identified and removed.

[0149] Connected component filtering and binarization labeling: Assuming that the coordinates have been calculated through traversal... and Connected components It contains 150 adjacent pixels that meet the criteria.

[0150] Calculate the total number of pixels in the area scalar: .

[0151] Compare this value with the preset noise threshold Comparison: If the condition is met, the connected components are preserved. .

[0152] Finally, perform binarization:

[0153] because The system assigns it the target state: .

[0154] Outputting this matrix generates the accompanying shadow contour mask for the current frame.

[0155] The aforementioned technique incorporates a three-dimensional physical prior constraint: the direction of the light source. First, the edges of the entity structure are extracted by calculating the pixel spatial gradient. Then, a specific backlight search area is defined along the opposite direction of the light source projection, narrowing the detection range. Within the search area, transmittance is calculated using the ratio of the current brightness to the average brightness of the local background, thereby determining the pixel attenuation level. Finally, isolated noise points are eliminated by calculating the area of ​​connected components, and a binary mask is output. This effectively eliminates interference from misclassifying dark backgrounds as shadows. Through the joint constraint of the light source direction vector and entity edges, the system searches for shadows only on the physical backlight side of the obstacle, ensuring a close physical relationship between the extracted shadows and the entity obstacle. Simultaneously, based on local transmittance filtering and connected component filtering, the accuracy of shadow boundary extraction is improved, providing a clear and noise-free data foundation for subsequent calculations of shadow diffusion speed and prediction of entity occlusion range.

[0156] In traditional visual processing schemes, calculating the motion state of large dark areas in an image typically employs global dense optical flow (GDI) or feature point matching. However, the interior of accompanying shadows usually exhibits uniform low brightness and lacks obvious texture features, leading to the failure of feature point extraction. Furthermore, GDI is prone to producing holes and aperture effects when processing textureless regions, and calculating the displacement of a large number of pixels is too time-consuming, failing to meet the high-frequency, real-time computation requirements of SLAM systems. Therefore, this paper proposes: obtaining the accompanying shadow contour mask from the previous frame, and combining it with the accompanying shadow contour mask from the current frame to perform displacement calculation, obtaining the shadow diffusion velocity vector, specifically including:

[0157] Extract the first edge pixel coordinates of the accompanying shadow contour mask of the previous frame and the second edge pixel coordinates of the accompanying shadow contour mask of the current frame respectively.

[0158] Calculate the normal vector of the second edge pixel coordinates in the two-dimensional image coordinate system, filter out local edge pixels with outward expansion trend based on the normal vector, and cluster them to generate a set of diffusion front pixels;

[0159] Based on the set of pixels at the diffusion front, a nearest neighbor search is performed in the coordinates of the first edge pixels along the opposite direction of the normal vector to obtain the corresponding historical position coordinates.

[0160] Calculate the spatial Euclidean distance between the pixel set at the diffusion front and the historical position coordinates to generate two-dimensional pixel displacement difference data;

[0161] The time interval between the previous frame and the current frame is obtained. The ratio of the two-dimensional pixel displacement difference data to the time interval is calculated to obtain the rate scalar. Combined with the normal vector as the direction constraint, the shadow diffusion velocity vector is generated and output.

[0162] Edge pixel coordinates: In the binary mask matrix, the set of pixel coordinates located at the boundary between the target state (pixel value 1) and the background state (pixel value 0). The first edge pixel coordinates correspond to the historical frame, and the second edge pixel coordinates correspond to the current frame.

[0163] Normal vector: A two-dimensional geometric vector perpendicular to the tangent direction of the edge contour. In this embodiment, it specifically refers to the unit outward normal vector pointing from the inside of the mask (shaded area) to the outside of the mask (unshaded area).

[0164] Diffusion leading edge pixel set: In the edge pixels of the current frame, it is a set of local pixels that move outward (i.e. cover the non-shadow area of ​​the previous frame) and are spatially continuous.

[0165] Step 1: Extract edge pixel coordinates from previous and current frames. Read the accompanying shadow contour mask matrix from the previous frame and the accompanying shadow contour mask matrix from the current frame. Extract edges using local neighborhood pixel state comparison.

[0166] ;

[0167] ;

[0168] in, and These represent the mask matrices of the previous frame and the current frame, respectively. Representing coordinates The set of eight neighboring pixel coordinates; This represents the set of coordinates of the first edge pixels generated. This represents the set of coordinates of the generated second edge pixels. The boundary is determined using statistical logic. The effect is to filter out large areas of uniform pixels within the mask, retaining only contour information, effectively reducing the computational dimensionality of subsequent matching.

[0169] Part Two: Calculating Normal Vectors and Extracting the Diffusion Front Pixel Set: For For each pixel in the array, calculate its local gradient unit normal vector. Use the difference operator:

[0170] ;

[0171] in, and This indicates that the coordinates are obtained through operators such as Sobel. The horizontal and vertical difference values ​​are calculated at this point. This represents the calculated outward unit normal vector.

[0172] Filter out local edge pixels that show an outward expansion trend:

[0173] ;

[0174] in, This represents the expanded set of pixels that are in shadow in the current frame but were background in the previous frame; for Pixels within a given range are clustered based on connectivity, and pixels with a spatial distance less than a threshold are grouped into multiple independent sets of diffusion front pixels, denoted as . .

[0175] Step 3: Using the set of pixels at the diffusion front as a reference, perform a nearest neighbor search along the opposite direction of the normal vector in the coordinates of the first edge pixels to obtain the corresponding historical position coordinates. This specifically includes:

[0176] For any target pixel in the diffusion front pixel set, extract the inverse unit vector of the corresponding normal vector, and combine it with the preset maximum occlusion movement speed parameter to generate a strip-shaped local search window extending along the inverse unit vector.

[0177] The spatial intersection operation is performed between the local search window and the first edge pixel coordinates of the accompanying shadow contour mask of the previous frame. All pixel coordinates that fall within the local search window are selected as historical candidate pixel set data.

[0178] Iterate through the historical candidate pixel set data to calculate the coordinates of each candidate pixel and the pixel-level Euclidean distance between them and the target pixel, generating a one-dimensional distance scalar sequence.

[0179] The absolute minimum value is retrieved from the distance scalar sequence, and the candidate pixel coordinates corresponding to the absolute minimum value are used as the unique historical position coordinates of the target pixel.

[0180] Strip-shaped local search window: A two-dimensional rectangular search area with a limited length and width, starting from the current target pixel and extending in the opposite direction of shadow diffusion (in the opposite direction of the normal vector).

[0181] Distance scalar sequence: a set consisting of multiple real numbers (one-dimensional undirected data) that reflect spatial physical intervals.

[0182] Absolute minimum: The smallest real number in a given sequence of values.

[0183] Maximum occlusion movement speed parameter: Based on the physical movement limits of dynamic obstacles (such as forklifts) in the scene and the camera frame rate, the maximum pixel distance threshold that the shadow may move between two adjacent frames is pre-calculated.

[0184] Step 1: Generate a strip-shaped local search window: From Extract any target pixel, denoted as . Extract the reverse unit vector of its corresponding normal vector:

[0185] ;

[0186] Combined with preset maximum occlusion movement speed parameters (Its value range is usually set to 10 to 50 pixels per frame depending on the frame rate), and the preset window width. (Typically set to 3 to 5 pixels), define the spatial constraints of the local search window:

[0187] For any two-dimensional coordinates It must simultaneously satisfy both the longitudinal length constraint and the lateral width constraint:

[0188] ;

[0189] ;

[0190] In this formula, Represents the dot product of vectors. This represents the cross product of two-dimensional vectors (calculating the perpendicular distance from a point to a line). This represents the coordinates of the target pixel. The two-dimensional spatial region satisfying the above two inequalities constitutes a strip-shaped local search window extending along the reverse unit vector, denoted as the region. .

[0191] Step 2: Spatial intersection to generate historical candidate pixel set data: Perform an intersection operation between the generated local search window region and the first edge pixel coordinates of the previous frame.

[0192] ;

[0193] in, This represents a strip-shaped local search window area; This represents the set of first edge pixel coordinates of the previous frame; This represents the set of historical candidate pixels that fall within this window. By limiting the search range under the premise of known motion direction, interference from full-image search is eliminated, and the number of feature matching candidate points is compressed to single digits.

[0194] Step 3: Generate a distance scalar sequence: Traverse the coordinates of each candidate pixel in the historical candidate pixel set data and calculate the pixel-level Euclidean distance between it and the target pixel:

[0195] ;

[0196] in, Indicates the coordinates of the target pixel; express The Middle The coordinates of the candidate pixels; Represents the calculated first... The pixel-level Euclidean distance values. All calculated values. Arrange them in traversal order to generate a one-dimensional distance scalar sequence. .

[0197] Step 4: Retrieve historical location coordinates: Perform numerical comparison operations in the distance scalar sequence to find the absolute minimum: ;

[0198] The search produces this Corresponding index And extract the candidate pixel coordinates under that index:

[0199] ;

[0200] in, Indicates the absolute minimum value; This represents the unique historical location coordinates of the retrieved target pixel. By utilizing the principle of local continuity of optical flow, the nearest physical counterpart within the constrained region is located.

[0201] Step 4: Generate 2D pixel displacement difference data: Since the distance has already been calculated, directly extract the absolute minimum value as the spatial displacement between the two points: ;

[0202] in, This represents the generated two-dimensional pixel displacement difference data. It is a non-directional real scalar that represents the physical pixel length that the shadow front edge moves during this frame.

[0203] Step 5: Generate shadow diffusion velocity vector: Obtain the system timestamp and calculate the time interval between the previous frame and the current frame. ;

[0204] Computational rate scalar: ;

[0205] Using the outward normal vector at the target pixel as a directional constraint, the following vector is generated:

[0206] ;

[0207] in, and These represent the timestamps of the current frame and the previous frame, respectively. Indicates the numerical value of the time interval; Represents the calculated rate scalar; This represents the final generated and output shadow diffusion velocity vector. By introducing a time dimension, the static image displacement is transformed into dynamic velocity parameters under a unified time metric, facilitating motion prediction across multiple frames.

[0208] Example 3: Continuing from the previous example, the edge of the shadow in the current frame is analyzed and located at coordinates... There is a diffusion front at this point, with the previous frame timestamp being 1.0 seconds and the current frame timestamp being 1.1 seconds.

[0209] Obtaining normals and generating the search window: Given the current target pixel. .

[0210] After difference calculation, its outward unit normal vector is .

[0211] Extracting the reverse unit vector: .

[0212] Preset maximum occlusion movement speed parameter pixels, window width Pixel.

[0213] Find the intersection of the historical edge coordinate spaces of the previous frame:

[0214] The set of first edge pixel coordinates of the previous frame It contains multiple coordinate points. (This is processed through a strip-shaped search window.) Dot product and cross product constraints are used to filter historical candidate pixel sets that fall within the window. Includes the following 3 points:

[0215] Candidate point 1: ;

[0216] Candidate point 2: ;

[0217] Candidate point 3: ;

[0218] Euclidean distance calculation and minimum value retrieval: Traverse the candidate point set and calculate the distance scalar:

[0219] ;

[0220] ;

[0221] ;

[0222] Generate distance scalar sequence .

[0223] The smallest absolute minimum value is retrieved. .

[0224] The index corresponding to this minimum value is 1, which means the corresponding historical position coordinates are... .

[0225] Displacement and velocity vector generation: Extracting two-dimensional pixel displacement difference data: Pixel.

[0226] Calculation time interval: Second.

[0227] Computational rate scalar: Pixels per second.

[0228] The final shadow diffusion velocity vector is generated by combining the normal vector direction:

[0229] .

[0230] The system outputs this velocity vector. Used for subsequent calculations.

[0231] In the aforementioned technique, the processing of pixels within the shadow is avoided, and instead, the edge contours of the binarized mask are extracted. The set of pixels at the front of diffusion where actual displacement occurs is precisely selected by calculating the mask normal vector. When matching positions between consecutive frames, the global search in the two-dimensional plane is abandoned; instead, a local search window constrained by the reverse normal is constructed, and the historical position coordinates are locked by calculating the minimum Euclidean distance in a one-dimensional direction. Finally, a vector velocity is generated by combining the time interval and the normal direction. By reducing the complexity of the global pixel matching process in the two-dimensional image to a one-dimensional search targeting only the edge contours, the matching failure problem caused by the lack of texture features within the shadow is avoided. This significantly reduces the search space and computational overhead of the algorithm, ensuring the efficiency of displacement calculation. The final generated shadow diffusion velocity vector contains precise scalar magnitude and directional constraints, providing a reliable kinematic basis for subsequent calculations of the occlusion evolution of physical obstacles on the camera's field of view.

[0232] Existing visual SLAM systems, when dealing with dynamic object occlusion, can only remove feature points based on existing occlusion results in the current frame. This approach cannot predict the arrival position of obstacles in the next moment or in the next few frames, preventing the system from intervening in advance for feature points that will soon be affected. This makes the information matrix in the backend optimization prone to sudden structural breaks. Therefore, this paper proposes: extrapolating the accompanying shadow contour mask and shadow diffusion velocity vector of the current frame to generate an occlusion diffusion region for predicting the future occlusion range, specifically including:

[0233] Obtain the preset future prediction time window, and multiply the future prediction time window with the shadow diffusion velocity vector to generate the predicted pixel offset data in the two-dimensional image coordinate system;

[0234] Extract the outer contour pixel coordinates of the accompanying shadow contour mask of the current frame, and perform translation projection calculation on the outer contour pixel coordinates according to the two-dimensional direction and displacement length indicated by the predicted pixel offset data to generate the predicted edge boundary data for future time.

[0235] By performing geometric vertex connection and closure calculations on the outer contour pixel coordinates and predicted edge boundary data, the translational sweep area covering the accompanying shadow from the current time to the future prediction time is constructed, and the initial diffusion envelope polygon data is generated.

[0236] Obtain the two-dimensional field of view boundary coordinate data of the camera frame, and determine whether there are any out-of-bounds pixels in the initial diffusion envelope polygon data that exceed the two-dimensional field of view boundary coordinate data;

[0237] If it exists, the initial diffusion envelope polygon data is processed by Boolean intersection clipping using the two-dimensional field of view boundary coordinate data to filter out outbound pixels, and the connected pixel range retained within the frame is marked and output as the occlusion diffusion area.

[0238] Predicted pixel offset data: The physical pixel distance and direction vector that the shadow area is expected to move on the two-dimensional image plane within the set prediction time window.

[0239] Translation sweep area: a two-dimensional geometric concept, referring to the union of all pixel areas covered by the outer contour of the accompanying shadow as it is translated from its current starting position along the predicted offset direction to its future predicted position.

[0240] Initial diffusion envelope polygon data: a closed polygon geometric dataset consisting of the current contour, the future predicted contour, and the line segments connecting the edges of their motion trajectories.

[0241] Two-dimensional field of view boundary coordinate data: The physical boundary range of the effective pixel matrix that the camera image sensor can output, which is usually determined by the resolution (width and height) of the image.

[0242] Boolean intersection cropping: a spatial geometric operation that finds the intersection of two two-dimensional pixel sets and retains only the pixel coordinates that exist in both sets simultaneously. In this case, it is used to remove invalid coordinates that exceed the physical boundaries of the image.

[0243] Step 1: Generate predicted pixel offset data: The system obtains a preset future prediction time window and performs a scalar-vector multiplication operation on it and the generated shadow diffusion velocity vector.

[0244] ;

[0245] in, This represents the preset future prediction time window, with the unit being seconds. The value range is usually set from 0.3 to 1.5 depending on the system's frame rate and safe braking distance. Represents the shadow diffusion velocity vector; This represents the calculated predicted pixel offset data vector. Using the fundamental principle of uniform extrapolation in kinematics, the abstract velocity dimension is transformed into an intuitive two-dimensional spatial displacement vector in the image coordinate system, providing a quantitative standard for subsequent geometric translation.

[0246] Step 2: Generate predicted edge boundary data: The system iterates through and extracts the outermost contour pixel coordinates of the accompanying shadow contour mask in the current frame, and performs translation and projection calculations on each coordinate point using the calculated offset vector: ;

[0247] in, Indicates the first [value] on the outer contour of the accompanying shadow in the current frame. Two-dimensional pixel coordinates; This is the data vector for predicting pixel offsets; This represents the predicted two-dimensional pixel coordinates of the point at a future time, obtained after translation calculation. All The resulting set constitutes the predicted edge boundary data for future moments. By assuming that the obstacle remains in a translational state for a short period, the specific image location of the dynamic obstacle at the predicted future moment is calculated.

[0248] Step 3: Constructing the initial diffusion envelope polygon data: To cover the influence range of the shadow throughout the translation process, the system geometrically connects the outer contour pixel coordinate set with the predicted edge boundary data set. The set union calculation formula for continuous translation scans is used:

[0249] ;

[0250] in, This represents the generated initial diffusion envelope polygon data, i.e., the translation sweep area; This represents the set number of interpolation sampling steps constant, used to discretize continuous motion; This represents the current interpolation step number variable. Since fast-moving objects occlude all space between their starting and ending positions, a seamless closed polygonal region is generated to ensure that any feature points traversed by the dynamic object during the prediction time period are included.

[0251] Step 4: Boundary Check and Field of View Boundary Acquisition: The system reads the camera's hardware resolution parameters and obtains the 2D field of view boundary coordinate data. The field of view boundary set is defined as follows:

[0252] ;

[0253] in, This represents a constant indicating the horizontal pixel width of the image output by the camera. This represents a constant vertical pixel height. This represents the set of valid two-dimensional field-of-view boundary coordinates for the camera; the system judgment set. Does it exist that do not satisfy? or Out-of-bounds pixels.

[0254] Step 5: Boolean intersection cropping to generate occlusion diffusion regions: If there are pixels that exceed the boundary, the system performs Boolean intersection cropping processing: ;

[0255] in, The Boolean intersection operator for sets; This represents the range of connected pixels remaining within the image frame after filtering out out-of-bounds pixels. This set is then binarized and output as the final occlusion diffusion region. Since pure mathematical extrapolation calculations do not possess the concept of physical sensor boundaries, invalid memory address coordinates can be removed, preventing memory overflow errors in subsequent steps when performing array or matrix index matching, while also reducing the amount of data.

[0256] Example 4: Continuing from the previous example:

[0257] The calculated shadow diffusion velocity vector is: (Unit: pixels / second). The two-dimensional field-of-view boundary constant of the camera frame is: , .

[0258] Generate predicted pixel offset data: Preset future prediction time window Seconds. Perform product operation:

[0259] ;

[0260] To accommodate pixel coordinates, rounding is performed: Pixel.

[0261] Generate predicted edge boundary data: Assume the two extreme pixel coordinates of the outer contour of the accompanying shadow in the current frame are: point ,point .

[0262] Perform translation and projection calculations on them respectively:

[0263] ;

[0264] .

[0265] Polygon Construction and Boundary Detection: The system uses outer contour points and predicted points to generate initial diffusion envelope polygon data. .

[0266] Perform boundary checks on the coordinates of points within the polygon. For the generated predicted points... :

[0267] horizontal coordinates satisfy .

[0268] Vertical coordinates Not satisfied (Because the maximum effective coordinate is the height minus one).

[0269] Therefore, it can be determined that there are out-of-bounds pixels in the polygon data that exceed the boundaries of the two-dimensional field of view.

[0270] Boolean intersection clipping: This process clips the initial diffusion envelope polygon data. With 2D field of view boundary coordinate data (x-axis 0-1279, y-axis 0-719) Perform the intersection operation.

[0271] For point Because its ordinate is not Within the range, points are determined to be non-intersecting and filtered out. Their adjacent points are as follows: Then it will be retained.

[0272] After processing, the system marks the closed connected pixel range within the image frame as the occlusion diffusion area. This is used for the next step of feature point cross-comparison.

[0273] The aforementioned technique incorporates a time-based geometric extrapolation mechanism. First, the displacement vector is obtained by multiplying the velocity vector by the prediction time. Then, a translation operation is performed on the current outer contour of the shadow to obtain the future predicted boundary. Next, a swept polygon containing the motion process is constructed through geometric closure operations. Finally, intersection clipping is performed in conjunction with the camera's field of view boundary to filter out meaningless out-of-bounds pixels, establishing the final occlusion diffusion range. This transforms the instantaneous motion state of dynamic obstacles into a two-dimensional pixel geometric region with a clear spatial boundary. By constructing the swept envelope polygon, the system obtains a deterministic spatial range of occlusion risk within a future timeframe. This provides a mathematical and geometric basis for the system to subsequently identify high-risk feature points in advance and initiate an adaptive weighting mechanism, effectively avoiding pose calculation delays caused by sudden occlusion.

[0274] Conventional visual SLAM front-ends lack semantic understanding of the physical environment of the image when extracting feature points, simply searching for pixels with high gradients throughout the entire image. When moving occlusions exist in the environment, the system will also send pixels falling on the surface of these moving objects or the areas they are about to cover as valid feature points to the back-end optimizer. These feature points with incorrect motion constraints will seriously interfere with the pose calculation process of the SLAM system, leading to increased tracking errors. To address this, we propose: acquiring static feature point set data for SLAM calculation, and determining whether the coordinates of each feature point in the static feature point set spatially overlap with the occlusion diffusion area, as follows:

[0275] Static feature point set data: Visual Odometry, the front-end of Visual SLAM, extracts a sequence of two-dimensional pixel coordinates from the current frame image through feature extraction (such as the FAST algorithm or the Shi-Tomasi corner detection algorithm). These points have significant grayscale changes and are expected to remain stationary in physical space. They are used to build spatial geometric constraints in the back-end optimizer.

[0276] Spatial overlap: In a two-dimensional image coordinate system, the geometric inclusion relationship in which the physical pixel coordinates of a feature point completely fall within the occlusion diffusion region (target state pixel region) defined by the binary mask matrix.

[0277] Step 1: Extracting Static Feature Point Set Data: The front-end module of the visual SLAM system performs corner detection on the current frame image, extracting the coordinates of pixels with high information entropy. All feature points are then aggregated to construct a static feature point set data. The formula is as follows:

[0278] ;

[0279] in, This represents the set of static feature points extracted from the current frame. This represents the total number of feature points extracted in the current frame. It is usually set to a positive integer between 800 and 2000, depending on the camera's resolution and texture richness. Indicates the first element in the set. The two-dimensional sub-pixel level coordinates of a feature point are denoted as follows: ,in The value range is from 1 to By transforming a continuous array of pixels in an image into a discrete sequence of feature coordinate points, fundamental and traceable geometric observation data is provided for solving the pose of the system.

[0280] Step 2: Pixel-level discretization and alignment of feature point coordinates: To improve accuracy, the front-end feature extraction algorithm outputs feature point coordinates... Typically, these are floating-point numbers with decimals (i.e., sub-pixel coordinates), while the occlusion diffusion region is constructed based on a binary mask matrix of integer pixels. Therefore, it is necessary to perform rounding operations on the feature point coordinates. The calculation formula is as follows:

[0281] ; ;

[0282] in, This represents the rounding function; This represents the integer x-coordinate obtained after the rounding operation; This represents the integer ordinate obtained after rounding. The index of the discrete matrix must be a positive integer. By eliminating the scale misalignment between sub-pixel coordinates and the integer pixel mask matrix, feature point coordinates can be directly used as index addresses to access the memory matrix.

[0283] Step 3: Traversal and query of spatially overlapping states: Obtain the previously generated occlusion diffusion region, which is stored in system memory as a binary mask matrix, denoted as... The system uses the coordinates of the discretized and aligned feature points as a matrix index to query the corresponding state identifier. The query formula is as follows:

[0284] ;

[0285] in, Represents the mask matrix on the integer x-axis. with integer ordinate The values ​​stored at the intersection; This indicates the result of the query. The spatial overlap status identifier of each feature point; the computational complexity of the query operation based on the two-dimensional array memory address mapping is O(n). This effectively avoids complex point-polygon containment relationships (such as ray casting) calculations, enabling rapid determination of whether a feature point falls into an occluded area with extremely low computational overhead.

[0286] Step 4: Logical classification output of the static feature point set: Based on the state identifier results returned by the traversal query, the system divides the original static feature point set into two subsets. The formula for including the feature points in the overlapping region is: ;

[0287] in, This represents a set of feature points with a state identifier of 1, i.e., high-risk feature point data that spatially overlaps with the occlusion diffusion area.

[0288] The formula for including feature points in the set of non-overlapping regions is: ;

[0289] in, The system outputs two subsets of data: one containing feature points with a state flag of 0 (i.e., safe feature point data that do not overlap spatially) and the other containing feature points with a state flag of 0. Since the pose calculation module needs to perform differentiated processing on data with different risk levels, it isolates data affected by dynamic obstacles from purely static background data at the physical level, providing a clear data interface for subsequent weight allocation.

[0290] Example 5: Continuing from the previous example: The translation of dynamic obstacles generates an occlusion diffusion area. The system converts it into a binary mask matrix. Given the predicted coordinates calculated earlier. The connected pixels extending from it are all located within this mask, and the value of the corresponding position in its matrix is ​​1; while the matrix value of the static shelf area in the distance is 0.

[0291] Extraction and Discretization: Suppose that the visual SLAM front-end module extracts the following two specific static feature points in the current frame image:

[0292] Feature points Its subpixel coordinates are ;

[0293] Feature points Its subpixel coordinates are .

[0294] Perform rounding and alignment operations on feature points:

[0295] ;

[0296] ;

[0297] The discretized coordinates are .

[0298] ;

[0299] ;

[0300] The discretized coordinates are .

[0301] Spatial overlap status query and classification:

[0302] The system uses discretized coordinates to perform index lookups in the binary mask matrix M_{occ}.

[0303] Given coordinates If the matrix is ​​located within the predicted occlusion diffusion area, the matrix returns a value of 1.

[0304] ;

[0305] Given coordinates Located in a static background area far from the motion trajectory, the matrix returns a value of 0.

[0306] ;

[0307] Classification result relationship mapping:

[0308] Based on the retrieved status identifiers, the system assigns the feature points to their corresponding sets. The relationship mapping table is as follows:

[0309]

[0310] After execution, the system output includes Set of feature points in overlapping regions and including Set of feature points in non-overlapping regions Then, proceed to the next step of the weight reduction process.

[0311] In the aforementioned technique, the sub-pixel-level feature point coordinates extracted by the SLAM system are discretized by rounding after obtaining the predicted occlusion diffusion mask matrix obtained from the previous calculation. Then, using coordinate indexing, the state value corresponding to each feature point is queried in the mask matrix one by one. Using mathematical intersection operations, the feature points are strictly divided into two independent sets: one in the occlusion risk zone and the other in the safe zone. This establishes a filtering mechanism based on spatial geometric position before the pose calculation officially begins. It eliminates the need for complex deep learning recognition models; it accurately determines the physical occlusion risk faced by each feature point with pixel-level precision using only low-computing two-dimensional matrix indexing queries. This classification mechanism physically isolates the previously mixed reliable data from the high-risk data, providing a clear data classification foundation for subsequent implementation of targeted adaptive weighting strategies.

[0312] In traditional visual SLAM, feature point states are typically defined as binary (usable or unusable). When a dynamic object intrudes into the field of view, existing systems often directly remove occluded feature points, causing a sudden reduction in the dimension of the information matrix used to solve for pose in the backend optimizer. This rigid feature point stripping mechanism can lead to abrupt loss of geometric constraints, causing discontinuous jumps in the solved camera trajectory, and even causing the optimization algorithm to diverge and the system's localization to collapse. Therefore, this paper proposes the following: If the occlusion occurs, the feature point weights corresponding to the feature points in the overlapping region are reduced based on a preset attenuation coefficient, and the robust pose data of the camera is calculated and output using the reduced feature point weights and the static feature point set data of the non-overlapping region; otherwise, the robust pose data of the camera is directly calculated and output using the static feature point set data, as detailed below:

[0313] Using the reduced-weight feature point weights and the static feature point set data of non-overlapping regions, robust pose data of the camera is calculated and output, specifically including:

[0314] Obtain the camera intrinsic parameter matrix data and the prior pose data output from the previous frame;

[0315] The static feature point set data of non-overlapping regions is concatenated and fused with the reduced-weight feature points to construct the global feature dataset to be optimized for the current frame;

[0316] Using camera intrinsic parameter matrix data and prior pose data, the theoretical projection coordinates of each feature point in the global feature dataset to be optimized on the two-dimensional image plane are calculated, and the deviation between the theoretical projection coordinates and the actual extracted coordinates is calculated to generate reprojection error data corresponding to each feature point.

[0317] The weights of each item in the global feature dataset to be optimized are converted into the diagonal elements of the information matrix in the back-end optimizer of the SLAM system, and the information matrix is ​​used to perform a product operation on the corresponding reprojection error data to construct a weighted objective cost function.

[0318] The weighted objective cost function is solved by least squares iteration until the weighted sum of squares of each reprojection error data converges to a minimum value. The 3D spatial transformation matrix in the converged state is then extracted and output as the robust pose data of the camera.

[0319] Prior pose data: The position and rotational orientation of the camera in three-dimensional space calculated by the system based on the previous frame image are used as the initial iterative reference values ​​for nonlinear optimization in this frame.

[0320] Theoretical projection coordinates: The two-dimensional image plane pixel coordinates calculated from known 3D spatial map points using the camera's intrinsic parameter model and assumed camera pose, through perspective projection geometry.

[0321] Reprojection error data: The spatial distance difference between the actual extracted observation coordinates of feature points on the 2D image and the theoretical projection coordinates of their corresponding 3D map points calculated using the current pose.

[0322] Information matrix: In state estimation, this matrix represents the confidence level of each observation. In this step, since it is assumed that the observations of each feature point are independent, this matrix is ​​a diagonal matrix, and its diagonal elements are the weight coefficients of each feature point.

[0323] Weighted objective cost function: A mathematical function consisting of the sum of the squared weights of the reprojection errors of all feature points according to their corresponding information matrix weights. It is the objective function that the optimizer needs to find the minimum value of.

[0324] Least squares iterative solution operation: a nonlinear optimization algorithm (such as the Gauss-Newton method) that continuously calculates the local gradient (Jacobi matrix) of the cost function and updates the independent variable (camera pose) so that the sum of squares of all weighted errors gradually decreases and converges to a minimum value.

[0325] Step 1: Branch weight reduction processing of feature point weights: The system determines whether the set of overlapping feature points output from the previous step is empty. If overlapping feature points exist, the system obtains the preset attenuation coefficient and performs a weight reduction operation on the initial weights of these high-risk feature points:

[0326] ;

[0327] in, Indicates the first The initial weight values ​​of feature points in overlapping regions in the back end of the SLAM system (usually set to a constant of 1.0). This represents the preset attenuation coefficient, which is usually set to a decimal between 0.01 and 0.5. This represents the updated weight value of the corresponding feature point after the weight reduction process. Since feature points falling into occluded areas are very likely to introduce incorrect motion constraints, their influence in the system is smoothly reduced through multiplicative scaling, rather than being directly removed, thus avoiding a precipitous drop in the amount of available data.

[0328] Step 2: Construct a global feature dataset to be optimized by splicing and merging: If there is a set of static feature points in non-overlapping regions, the system keeps their initial weights unchanged.

[0329] ;

[0330] in, and They represent the first Initial weights and updated weights for security feature points in non-overlapping regions.

[0331] Subsequently, the system concatenates the downweighted feature points with the safe feature points that retain their initial weights to construct a global feature dataset to be optimized for the current frame, with the set size denoted as . If it is determined that no feature point overlaps with the occluded area, then all feature points are processed. Then proceed directly to the subsequent calculations.

[0332] Step 3: Calculate theoretical projection coordinates: Obtain the camera intrinsic parameter matrix data and the prior pose data output from the previous frame. For each item in the global feature dataset to be optimized, transform the corresponding 3D map point from the world coordinate system to the camera coordinate system using the prior pose:

[0333] ;

[0334] in, Indicates the first The coordinate vector of a 3D spatial map point corresponding to a feature point; and These represent the three-dimensional rotation matrix and translation vector in the prior pose data, respectively. This represents the three-dimensional coordinates in the transformed camera coordinate system.

[0335] Subsequently, the theoretical projection coordinates are calculated using the camera intrinsic parameter matrix:

[0336] ; ;

[0337] in, and This represents the horizontal and vertical focal length parameters in the camera intrinsic parameter matrix; and This represents the optical center offset parameter in the camera intrinsic parameter matrix; and These represent the x and y coordinates of the calculated theoretical projection coordinates. By establishing a geometric mapping between the pinhole camera and the two-dimensional imaging plane, a theoretical reference quantity is provided for the calculation error.

[0338] Step 4: Generate reprojection error data: Calculate the deviation between the actual extracted coordinates and the theoretical projected coordinates.

[0339] ;

[0340] In this formula, and This represents the sub-pixel-level two-dimensional observation coordinates actually extracted by the front-end visual odometry; Indicates the generated first The reprojection error data vector corresponding to each feature point. By quantifying the difference between the current estimated pose and the actual observation, the geometric constraints are transformed into residual data that can be directly used in numerical optimization.

[0341] Step 5: Construct the weighted objective cost function: Convert the update weights of each item in the global feature dataset to be optimized into the diagonal elements of the information matrix.

[0342] ;

[0343] in, Indicates the first The two-dimensional information matrix corresponding to the feature points.

[0344] Construct a weighted objective cost function using the information matrix:

[0345] ;

[0346] in, Represents the variables of the three-dimensional spatial transformation matrix to be solved, which includes rotation and translation; This represents the constructed weighted objective cost function, which is the weighted sum of squares of the reprojection error data. Using the maximum likelihood estimation principle from probability theory, the high-confidence (safe) error terms dominate the function, while the low-confidence (occluded) error terms are suppressed.

[0347] Step 6: Perform least squares iteration and output: Perform Gauss-Newton iteration on the cost function. Calculate the Jacobian matrix of the error with respect to pose perturbation. And calculate the incremental equation:

[0348] ;

[0349] ;

[0350] in, This represents the incremental change in pose during iterative updates. After multiple iterations, until... Approaching zero or The system converges to a minimum. The system extracts the values ​​in the convergent state. It is output as robust pose data of the camera.

[0351] Example 6: Continuing from the previous example: In the previous step, feature points were obtained. For coincident feature points, their actual observed coordinates Feature points For non-overlapping feature points, their actual observed coordinates .

[0352] Assuming the initial weights of all cameras are 1.0, and the preset attenuation coefficient is... .

[0353] Assuming camera intrinsic focal length , , light center , To simplify the explanation, assume the prior pose of the previous frame. It is an identity matrix, and it is known and The corresponding 3D map point coordinates are as follows: and .

[0354] Weighting:

[0355] because If the device falls into an obstructed area, its priority will be reduced: .

[0356] because Located in a safe zone, no weight reduction: .

[0357] Calculate the theoretical projected coordinates:

[0358] because The camera coordinate system remains unchanged after the transformation, as it is an identity matrix.

[0359] for :

[0360] ;

[0361] ;

[0362] for :

[0363] ;

[0364] .

[0365] Generate reprojection error:

[0366] for :

[0367] ;

[0368] for :

[0369] .

[0370] Cost function data mapping and calculation: The constructed data mapping table is as follows:

[0371]

[0372] Calculate the weighted objective cost function value under the initial state:

[0373] ;

[0374] The system then uses this cost function as a basis to perform subsequent nonlinear iterative deductions, continuously adjusting the pose matrix until the value of that generation approaches its minimum, outputting the final robust pose data. It can be seen that... Because it is in a high-risk area, its error of 0.7949 is suppressed by the attenuation coefficient and does not have a detrimental dominant impact on the final cost.

[0375] When faced with dynamic occlusion, traditional visual SLAM algorithms typically hard-remove feature points in a binary manner (weights of 1 or 0) after the observation error becomes too large. For a region about to be occluded, the time points at which all feature points are deemed invalid are often highly concentrated, causing a sudden and precipitous drop in the number of constraints in the backend information matrix, leading to severe jitter and unevenness in the calculated camera trajectory. To address this, we propose that the attenuation coefficient be dynamically generated based on the shadow diffusion velocity vector and the accompanying shadow contour mask of the current frame, specifically including:

[0376] Extract the scalar magnitude value from the shadow spread velocity vector to generate the shadow intrusion rate under the current occlusion;

[0377] The feature points of the overlapping region are used as target feature points, and the shortest pixel distance between the feature point coordinates of the target feature points and the edge of the accompanying shadow contour mask of the current frame is calculated.

[0378] Calculate the ratio between the shortest pixel distance and the shadow intrusion rate data, generate the remaining safe time data of the target feature point before facing entity occlusion, and convert the remaining safe time data into a decay coefficient;

[0379] Obtain the initial feature point weights of the target feature points in the backend of the SLAM system, and use the attenuation coefficient to correct the initial feature point weights, outputting the reduced-weight feature point weights.

[0380] The remaining safety time data is converted into a decay coefficient, specifically including:

[0381] Obtain the preset critical warning time threshold and attenuation sensitivity parameters;

[0382] Determine whether the remaining safety time data is greater than the critical warning time threshold;

[0383] If so, the attenuation coefficient will be directly assigned to the initial constant data with no attenuation.

[0384] If not, the remaining safe time data is reciprocalized to obtain the occlusion urgency index data in relation to time;

[0385] Based on the occlusion urgency index data and attenuation sensitivity parameters, a penalty index factor is generated. Attenuation calculation is performed with the natural constant as the base and the negative number of the penalty index factor as the exponent to generate the attenuation coefficient.

[0386] Shadow intrusion rate: The absolute physical speed at which a shadow edge expands on a two-dimensional image plane. It is a scalar value (usually in pixels per second) that does not include directional attributes.

[0387] Shortest pixel distance: The smallest Euclidean distance on the image plane that connects the coordinates of a specific target feature point to the two-dimensional boundary of the accompanying shadow contour mask in the current frame via a straight line.

[0388] Remaining safe time data: Based on the current shadow expansion rate and spatial distance, the remaining physical quantity of time before the predicted dynamic shadow edge completely covers and causes the feature point to fail.

[0389] Critical warning time threshold: A time constant boundary preset by the system. When the remaining safe time of a feature point is lower than this value, the system determines that it is in a high-risk state of being about to be occluded and must initiate a weight reduction procedure.

[0390] The urgency index for occlusion is a mathematical scalar that is inversely proportional to the remaining safe time. The shorter the remaining time, the higher the value of the index, which indicates the severity of the occlusion threat.

[0391] Attenuation sensitivity parameter: A preset proportional coefficient used to adjust the intensity of the system's response to occlusion threats, which determines the steepness of the weight decay over time.

[0392] Step 1: Generating Shadow Intrusion Rate: The system extracts the shadow diffusion velocity vector calculated in the previous step, performs a modulo operation on it, and extracts the scalar magnitude. The calculation formula is as follows:

[0393] ;

[0394] in, and These represent the components of the shadow diffusion velocity vector in the horizontal and vertical directions, respectively; This represents the calculated shadow intrusion rate data. By converting distance to time, we only need to focus on the magnitude of the velocity, not its direction. This reduces the dimensionality of the directional vector into a unified scalar rate, simplifying subsequent kinematic time estimation.

[0395] Step 2: Calculate the shortest pixel distance: For the set of feature points in the overlapping region, select one of these feature points as the target feature point. Traverse the set of edge pixel coordinates of the accompanying shadow contour mask in the current frame, and find the edge point closest to the target feature point. The calculation formula is as follows:

[0396] ;

[0397] in, , This represents the coordinates of the target feature point on the two-dimensional image plane; This represents the set of all pixel coordinates of the edge of the accompanying shadow contour mask in the current frame; , This represents the coordinates of a specific edge pixel in the set. This indicates the operation of finding the minimum value; This represents the calculated shortest pixel distance. By finding the shortest path for shadow propagation, the geometrical spatial clearance distance between a target feature point and its nearest occlusion threat is accurately quantified.

[0398] Step 3: Generate remaining safety time data: Using basic kinematic formulas, divide the spatial distance by the intrusion rate. The calculation formula is as follows:

[0399] ;

[0400] in, This represents the shortest pixel distance obtained in the previous step; Indicates the rate of shadow intrusion; This represents the remaining safe time (in seconds) for the generated target feature points before they face entity occlusion. By uniformly transforming spatial geometric metrics into a time dimension, it eliminates evaluation differences caused by different feature point positions or shadow velocities, providing a standardized risk assessment metric.

[0401] Step 4: Threshold Determination and Initial Assignment: The system obtains the preset critical warning time threshold (denoted as...). Its value range is typically set from 0.3 to 1.0 seconds based on the camera frame rate and the system's tolerance for latency, as well as the attenuation sensitivity parameter (denoted as...). Its value typically ranges from 0.1 to 2.0.

[0402] judge Whether it is valid or not.

[0403] If so (i.e., sufficient remaining safety time), then the attenuation coefficient is directly assigned to the initial constant data with no attenuation. The calculation formula is as follows:

[0404] ;

[0405] in, This represents the preset initial constant data with no decay, which is usually set to 1.0; This represents the final attenuation coefficient.

[0406] Step 5: Generate occlusion urgency index data: If the judgment result is negative (i.e. If the feature point faces an immediate threat of occlusion, then the reciprocal of the remaining safe time data is calculated. The formula is as follows:

[0407] ;

[0408] in, This indicates the remaining safe time data; This represents the generated one-dimensional occlusion urgency index data. By establishing a positive correlation between time and risk, the urgency index increases non-linearly and sharply as the safe time decreases, amplifying the threat weight of near-occlusion situations.

[0409] Step 6: Perform attenuation calculation to generate the attenuation coefficient: Multiply the occlusion urgency index data by the attenuation sensitivity parameter to generate a penalty exponential factor, and then construct an exponential attenuation function with the natural constant as the base. The calculation formula is as follows:

[0410] ;

[0411] ;

[0412] in, This indicates the urgency of the obstruction index; This indicates the preset attenuation sensitivity parameter; This represents the calculated penalty index factor; Represents the natural constant; This represents the attenuation coefficient generated exponentially (with a value between 0 and 1). By simulating the damped smooth attenuation process in the physical system, the decrease curve of the attenuation coefficient is made smooth and continuous, avoiding the derivative discontinuity problem that may be caused by linear attenuation, and ensuring the stability of the back-end optimization mathematical solution.

[0413] Step 7: Output the corrected feature point weights: Obtain the initial feature point weights of the target feature point in the SLAM system backend and perform product correction. The calculation formula is as follows:

[0414] ;

[0415] in, This indicates the initial feature point weights of the target feature point before entering the optimization process; This represents the generated attenuation coefficient; This represents the feature point weights output after the weight reduction process.

[0416] The system calculates the unique remaining safe time for each feature point by dividing the shortest distance by the rate. Using a critical time threshold as a trigger, it introduces a continuous exponential decay function based on the natural logarithm base, transforming the reciprocalized time urgency into a continuously changing decay coefficient. This corrects the final weight of the feature point, refining spatial occlusion judgment to a temporal dimension for early warning. A refined soft-downweighting mechanism is implemented. The system assigns an independent weight decay lifecycle to each feature point; feature points closer to the occluder experience faster weight stripping, while maintaining a mathematically continuous transition. This effectively buffers the structural impact of dynamic object movement on the underlying optimization matrix, ensuring an extremely smooth output trajectory for the vision system.

[0417] Example 7: Continuing from the previous example: In the steps described above, the shadow diffusion velocity vector calculated by the system is... And the current feature point is determined. (coordinates are) , The points fall into the overlapping region and are used as target feature points.

[0418] The rate of shadow intrusion is generated: the horizontal and vertical components of the velocity vector are both 39.9455.

[0419] Calculate the scalar modulus: Pixels per second.

[0420] The shadow intrusion rate under the current occlusion is 56.5 pixels / second.

[0421] Calculate the shortest pixel distance: The system traverses the mask edges of the current frame, assuming the distance coordinates are found. The nearest associated shadow outline edge pixel coordinates are Perform distance calculation:

[0422] Pixel.

[0423] Generate remaining safety time data: calculated based on the ratio of distance to speed.

[0424] Second.

[0425] Threshold determination and attenuation calculation:

[0426] The system obtains preset parameters: critical warning time threshold. Seconds, decay sensitivity parameter Initial constants .

[0427] Judgment logic: because If the condition is met, the feature point is in a dangerous period, triggering the reciprocal decay calculation process.

[0428] Calculate the urgency index data for occlusion: .

[0429] Calculate the penalty index factor: .

[0430] With natural constant Perform attenuation calculation at the bottom:

[0431] .

[0432] Weighted output: target feature points The state relationship mapping table is as follows:

[0433]

[0434] Finally, the system outputs a feature point weight of 0.553 after weight reduction, and passes this value to the backend optimizer to replace the traditional hard removal (resetting the weight to 0), thus achieving a smooth data transition.

[0435] The aforementioned technology incorporates a soft weighting mechanism based on safe region partitioning. Instead of directly discarding high-risk feature points, the system divides feature points into two groups based on conditional judgment. Feature points in safe regions maintain their full weight to ensure the basic geometric constraints are met; for feature points overlapping with predicted occlusion areas, their weights are reduced linearly by introducing a decay coefficient. Finally, feature points with different weight gradients are integrated into a nonlinear least-squares optimization cost function for joint solution. This effectively avoids structural oscillations in the backend information matrix caused by the sudden disappearance of feature points. By retaining high-risk feature points but reducing their contribution to the error function, the system suppresses potentially erroneous observations while maintaining a smooth transition of spatial geometric constraints. This allows the nonlinear optimizer to output continuous and stable 3D spatial pose data even when the local view is obstructed by dynamic objects, improving the reliability of real-time navigation for mobile devices in complex dynamic scenes.

[0436] A real-time robust visual SLAM localization system for dynamically occluded environments includes:

[0437] The data acquisition module is used to acquire the visual image of the current frame in the camera's field of view, and to preprocess the visual image of the current frame to obtain the ambient light direction vector of the current frame.

[0438] The mask extraction module is used to extract the accompanying shadow contour mask of the current frame based on the visual image of the current frame and the ambient light source direction vector;

[0439] The velocity calculation module is used to obtain the accompanying shadow contour mask of the previous frame, and combine it with the accompanying shadow contour mask of the current frame to perform displacement calculation and obtain the shadow diffusion velocity vector.

[0440] The extrapolation prediction module is used to perform extrapolation calculations based on the accompanying shadow contour mask and shadow diffusion velocity vector of the current frame to generate an occlusion diffusion region for predicting the future occlusion range.

[0441] The spatial overlap judgment and pose output module is used to acquire static feature point set data for SLAM calculation, and to determine whether the coordinates of each feature point in the static feature point set spatially overlap with the occlusion diffusion area. If so, the feature point weights corresponding to the feature points in the overlapping area are reduced based on a preset attenuation coefficient, and the robust pose data of the camera is calculated and output using the reduced feature point weights and the static feature point set data of the non-overlapping area. If not, the robust pose data of the camera is directly calculated and output using the static feature point set data.

[0442] The technical scope of this invention is not limited to the content described above. Those skilled in the art can make various modifications and variations to the above embodiments without departing from the technical concept of this invention, and all such modifications and variations should fall within the protection scope of this invention.

Claims

1. A real-time robust visual SLAM localization method for dynamic occlusion environments, characterized in that, Includes the following steps: Obtain the visual image of the current frame within the camera's field of view, and preprocess the visual image of the current frame to obtain the ambient light direction vector of the current frame; Based on the visual image of the current frame and the ambient light direction vector, extract the accompanying shadow contour mask of the current frame; Obtain the accompanying shadow contour mask of the previous frame, and perform displacement calculation by combining it with the accompanying shadow contour mask of the current frame to obtain the shadow diffusion velocity vector. Extrapolation calculations are performed based on the accompanying shadow contour mask and shadow spread velocity vector of the current frame to generate an occlusion spread region for predicting the future occlusion range. Obtain the set of static feature points for SLAM calculation, and determine whether the coordinates of each feature point in the set of static feature points overlap with the occlusion diffusion area. If so, the feature point weights corresponding to the feature points in the overlapping region are reduced based on the preset attenuation coefficient, and the robust pose data of the camera is calculated and output using the reduced feature point weights and the static feature point set data of the non-overlapping region. If not, the robust pose data of the camera is calculated and output directly using the static feature point set data.

2. The real-time robust visual SLAM localization method for dynamic occlusion environments according to claim 1, characterized in that: The attenuation coefficient is dynamically generated based on the shadow diffusion velocity vector and the accompanying shadow contour mask of the current frame, specifically including: Extract the scalar magnitude value from the shadow spread velocity vector to generate the shadow intrusion rate under the current occlusion; The feature points of the overlapping region are used as target feature points, and the shortest pixel distance between the feature point coordinates of the target feature points and the edge of the accompanying shadow contour mask of the current frame is calculated. Calculate the ratio between the shortest pixel distance and the shadow intrusion rate data, generate the remaining safe time data of the target feature point before facing entity occlusion, and convert the remaining safe time data into a decay coefficient; Obtain the initial feature point weights of the target feature points in the backend of the SLAM system, and use the attenuation coefficient to correct the initial feature point weights, outputting the reduced-weight feature point weights.

3. The real-time robust visual SLAM localization method for dynamic occlusion environments according to claim 2, characterized in that: The remaining safety time data is converted into a decay coefficient, specifically including: Obtain the preset critical warning time threshold and attenuation sensitivity parameters; Determine whether the remaining safety time data is greater than the critical warning time threshold; If so, the attenuation coefficient will be directly assigned to the initial constant data with no attenuation. If not, the remaining safe time data is divided by its reciprocal to obtain the occlusion urgency index data that is inversely proportional to time. Based on the occlusion urgency index data and attenuation sensitivity parameters, a penalty index factor is generated. Attenuation calculation is performed with the natural constant as the base and the negative number of the penalty index factor as the exponent to generate the attenuation coefficient.

4. The real-time robust visual SLAM localization method for dynamic occlusion environments according to claim 1, characterized in that: Using the reduced-weight feature point weights and the static feature point set data of non-overlapping regions, robust pose data of the camera is calculated and output, specifically including: Obtain the camera intrinsic parameter matrix data and the prior pose data output from the previous frame; The static feature point set data of non-overlapping regions is concatenated and fused with the reduced-weight feature points to construct the global feature dataset to be optimized for the current frame; Using camera intrinsic parameter matrix data and prior pose data, the theoretical projection coordinates of each feature point in the global feature dataset to be optimized on the two-dimensional image plane are calculated, and the deviation between the theoretical projection coordinates and the actual extracted coordinates is calculated to generate reprojection error data corresponding to each feature point. The weights of each item in the global feature dataset to be optimized are converted into the diagonal elements of the information matrix in the back-end optimizer of the SLAM system, and the information matrix is ​​used to perform a product operation on the corresponding reprojection error data to construct a weighted objective cost function. The weighted objective cost function is solved by least squares iteration until the weighted sum of squares of each reprojection error data converges to a minimum value. The 3D spatial transformation matrix in the converged state is then extracted and output as the robust pose data of the camera.

5. The real-time robust visual SLAM localization method for dynamic occlusion environments according to claim 1, characterized in that: The ambient light direction vector of the current frame is obtained by preprocessing the visual image of the current frame, specifically including: Extract the luminance channel components of each pixel in the visual image of the current frame and generate a two-dimensional global luminance distribution matrix; Perform convolution filtering on the global brightness distribution matrix to filter out local highlight noise data caused by local material reflections and generate a smooth brightness matrix; Spatial gradient difference calculation is performed on the smoothed brightness matrix to extract the extreme point region where the brightness gradient vector points to converge and the brightness value is greater than the preset exposure threshold. The geometric center of the extreme point region is extracted as the two-dimensional candidate pixel coordinates of the main light source. Obtain camera intrinsic parameter matrix data, and use the camera intrinsic parameter matrix data to perform inverse reprojection transformation on the two-dimensional main light source candidate pixel coordinates, mapping the two-dimensional main light source candidate pixel coordinates from the two-dimensional image coordinate system to the camera's three-dimensional physical coordinate system, generating three-dimensional light source spatial point coordinates. An initial direction vector is constructed starting from the optical center coordinates of the camera and ending at the coordinates of the three-dimensional light source spatial points. The initial direction vector is then normalized to generate the ambient light source direction vector for the current frame.

6. The real-time robust visual SLAM localization method for dynamic occlusion environments according to claim 1, characterized in that: Based on the visual image of the current frame and the ambient light source direction vector, extract the accompanying shadow contour mask of the current frame, specifically including: Perform pixel-level edge detection calculations on the visual image of the current frame, extract the contour boundaries with high gradients in the image, and generate a set of structural edge pixels; Using the set of pixels at the structural edge as the reference position, and following the direction of the reverse extension of the ambient light source direction vector, a ray projection dilatation calculation is performed in the two-dimensional image coordinate system to delineate the backlight search area. Within the pixel range covered by the backlight search area, the current brightness component data of each pixel is extracted, and the difference is calculated with the local background average brightness data of the adjacent non-backlight side to filter out candidate dark pixels whose brightness attenuation is within the preset shadow transmittance range. Perform connected component calculations on candidate dark pixels, filter out isolated pixel blocks with an area smaller than a preset noise threshold, and obtain continuous shadow connected component data. The shadow connected component data is binarized and labeled in the original image coordinate system. The pixel regions labeled as target states are packaged as a whole to generate and output the accompanying shadow contour mask of the current frame.

7. The real-time robust visual SLAM localization method for dynamic occlusion environments according to claim 1, characterized in that: Obtain the accompanying shadow contour mask of the previous frame, and combine it with the accompanying shadow contour mask of the current frame to perform displacement calculation to obtain the shadow diffusion velocity vector, specifically including: Extract the first edge pixel coordinates of the accompanying shadow contour mask of the previous frame and the second edge pixel coordinates of the accompanying shadow contour mask of the current frame respectively. Calculate the normal vector of the second edge pixel coordinates in the two-dimensional image coordinate system, filter out local edge pixels with outward expansion trend based on the normal vector, and cluster them to generate a set of diffusion front pixels; Based on the set of pixels at the diffusion front, a nearest neighbor search is performed in the coordinates of the first edge pixels along the opposite direction of the normal vector to obtain the corresponding historical position coordinates. Calculate the spatial Euclidean distance between the pixel set at the diffusion front and the historical position coordinates to generate two-dimensional pixel displacement difference data; The time interval between the previous frame and the current frame is obtained. The ratio of the two-dimensional pixel displacement difference data to the time interval is calculated to obtain the rate scalar. Combined with the normal vector as the direction constraint, the shadow diffusion velocity vector is generated and output.

8. The real-time robust visual SLAM localization method for dynamic occlusion environments according to claim 7, characterized in that: Based on the set of pixels at the diffusion front, a nearest neighbor search is performed in the coordinates of the first edge pixels along the opposite direction of the normal vector to obtain the corresponding historical position coordinates. Specifically, this includes: For any target pixel in the diffusion front pixel set, extract the inverse unit vector of the corresponding normal vector, and combine it with the preset maximum occlusion movement speed parameter to generate a strip-shaped local search window extending along the inverse unit vector. The spatial intersection operation is performed between the local search window and the first edge pixel coordinates of the accompanying shadow contour mask of the previous frame. All pixel coordinates that fall within the local search window are selected as historical candidate pixel set data. Iterate through the historical candidate pixel set data to calculate the coordinates of each candidate pixel and the pixel-level Euclidean distance between them and the target pixel, generating a one-dimensional distance scalar sequence. The absolute minimum value is retrieved from the distance scalar sequence, and the candidate pixel coordinates corresponding to the absolute minimum value are used as the unique historical position coordinates of the target pixel.

9. The real-time robust visual SLAM localization method for dynamic occlusion environments according to claim 1, characterized in that: Extrapolation is performed based on the accompanying shadow contour mask and shadow spread velocity vector of the current frame to generate an occlusion spread region for predicting the future occlusion range, specifically including: Obtain the preset future prediction time window, and multiply the future prediction time window with the shadow diffusion velocity vector to generate the predicted pixel offset data in the two-dimensional image coordinate system; Extract the outer contour pixel coordinates of the accompanying shadow contour mask of the current frame, and perform translation projection calculation on the outer contour pixel coordinates according to the two-dimensional direction and displacement length indicated by the predicted pixel offset data to generate the predicted edge boundary data for future time. By performing geometric vertex connection and closure calculations on the outer contour pixel coordinates and predicted edge boundary data, the translational sweep area covering the accompanying shadow from the current time to the future prediction time is constructed, and the initial diffusion envelope polygon data is generated. Obtain the two-dimensional field of view boundary coordinate data of the camera frame, and determine whether there are any out-of-bounds pixels in the initial diffusion envelope polygon data that exceed the two-dimensional field of view boundary coordinate data; If it exists, the initial diffusion envelope polygon data is processed by Boolean intersection clipping using the two-dimensional field of view boundary coordinate data to filter out outbound pixels, and the connected pixel range retained within the frame is marked and output as the occlusion diffusion area.

10. A real-time robust visual SLAM localization system for dynamic occlusion environments, characterized in that, include: The data acquisition module is used to acquire the visual image of the current frame in the camera's field of view, and to preprocess the visual image of the current frame to obtain the ambient light direction vector of the current frame. The mask extraction module is used to extract the accompanying shadow contour mask of the current frame based on the visual image of the current frame and the ambient light source direction vector; The velocity calculation module is used to obtain the accompanying shadow contour mask of the previous frame, and combine it with the accompanying shadow contour mask of the current frame to perform displacement calculation and obtain the shadow diffusion velocity vector. The extrapolation prediction module is used to perform extrapolation calculations based on the accompanying shadow contour mask and shadow diffusion velocity vector of the current frame to generate an occlusion diffusion region for predicting the future occlusion range. The spatial overlap judgment and pose output module is used to acquire static feature point set data for SLAM calculation, and to determine whether the coordinates of each feature point in the static feature point set spatially overlap with the occlusion diffusion area. If so, the feature point weights corresponding to the feature points in the overlapping area are reduced based on a preset attenuation coefficient, and the robust pose data of the camera is calculated and output using the reduced feature point weights and the static feature point set data of the non-overlapping area. If not, the robust pose data of the camera is directly calculated and output using the static feature point set data.

Images