A mapping method based on joint optimization of point, line and surface features and dynamic segmentation
By jointly optimizing the graph building method of point, line and surface features and dynamic segmentation, the problem of insufficient robustness of dynamic SLAM in the existing technology is solved, more accurate identification of dynamic objects and full utilization of environmental information is achieved, and the system is improved Real-time and adaptability.
Patent Information
- Application Number
- CN202410105723.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-01-25
- Publication Date
- 2024-11-19
- Estimated Expiration
- 2044-01-25
AI Technical Summary
When handling dynamic objects, the existing dynamic SLAM technology is not robust enough to determine the geometric discrimination of multi-views, which is prone to failure in positioning. The deep learning discrimination method has limited types of identification and cannot accurately identify incomplete dynamic objects, resulting in misjudgment and missed detection.
The graph building method of joint optimization of point, line and surface features and dynamic segmentation is adopted. By obtaining color images and depth images, points, lines and plane features are extracted, reprojection error terms, residual regular terms and spatial regular terms are constructed, and the objective function is generated. Optimize pose and dynamic object segmentation, update static probability maps with weights, and improve robustness and adaptability.
It improves the robustness and adaptability of dynamic object segmentation, makes full use of environmental information, reduces calculation amount, shortens optimization time, improves system real-time, and maintains stable pose estimation in weak texture environments.
Smart Images

Figure CN118015115B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of positioning and mapping, and in particular to a mapping method and device for joint optimization of point, line and surface features and dynamic segmentation. Background Art
[0002] Simultaneous Localization and Mapping (SLAM) is an important technology for robots to perceive and recognize in complex and unknown environments. In the past two decades, advances in filtering and nonlinear optimization research have promoted the development of visual SLAM and produced fruitful results, which can be divided into monocular, binocular and RGB-D SLAM. For indoor environments, RGB-D SLAM systems that can directly measure depth are usually used. For open outdoor environments, monocular and binocular SLAM systems are usually used. At present, visual SLAM is mainly used in robots, mobile terminals and head-mounted devices. In recent years, it has also been widely used in AR applications and 3D reconstruction due to its low cost and rich visual effects.
[0003] However, there are some problems with the existing technologies. Among the commonly used methods for removing dynamic feature points in dynamic SLAM, the multi-view geometry discrimination method needs to rely on strict environmental assumptions, is not robust enough, and is prone to positioning failure. Most of them directly eliminate features whose reprojection errors exceed the threshold. When dynamic objects occupy most of the image field of view, a large number of misjudgments will be caused. The deep learning discrimination method can only recognize a limited number of dynamic objects, which are limited to the limited types of neural network model training sets. It is also unable to accurately identify incomplete dynamic objects in the image, which can easily lead to missed detections. Summary of the invention
[0004] The present invention provides a mapping method, device, terminal equipment and storage medium for joint optimization of point, line and surface features and dynamic segmentation, which can realize joint optimization of point, line and surface features and dynamic segmentation mapping for different grid ranges and different broadcast frequencies.
[0005] The present invention provides a mapping method for jointly optimizing point, line and surface features and dynamic segmentation, comprising: acquiring a color image and a depth image corresponding to the color image;
[0006] According to the color image, obtaining point features and line features corresponding to the color image, and according to the depth image, obtaining plane features corresponding to the depth image;
[0007] Performing pose and map initialization according to the point features and line features corresponding to the color image and the plane features corresponding to the depth image to generate an initialized map with point, line and plane features;
[0008] According to the depth value of each pixel in the depth image, the initialized image is divided into various regions, and a preset weight value is assigned to each region to generate an initialized static probability map;
[0009] According to the point features and line features corresponding to the color image, and the plane features corresponding to the depth image, reprojection error terms of the point, line and plane features for pose estimation are constructed, as well as residual regularization terms, regional regularization terms and space regularization terms for dynamic object segmentation; according to the reprojection error terms, residual regularization terms, regional regularization terms and space regularization terms, an objective function for jointly optimizing pose and dynamic object segmentation is generated, so as to calculate and generate weight estimates of pose state quantities and various regional features through the objective function;
[0010] The static probability map is updated and optimized according to the posture state quantity and the weight estimation value of each regional feature, and a map surface is constructed.
[0011] Furthermore, according to the point features and line features corresponding to the color image and the plane features corresponding to the depth image, the pose and map initialization is performed to obtain an initialized map with point, line and plane features, including:
[0012] According to the depth information provided by the depth map, the point features and line features are back-projected to the preset camera coordinate system corresponding to the color map, and the plane features are projected to the preset camera coordinate system to generate an initialized map with point, line and surface features.
[0013] Furthermore, according to the depth value of each pixel in the depth image, the initialized image is divided into various regions, and a preset weight value is assigned to each region to generate an initialized static probability map, including:
[0014] According to the depth value of each pixel in the depth image, the initialization image is clustered based on the depth value, the initialization image is divided into several regions, and the point features, line features and plane features in the same region are assigned the same preset weight value to generate an initialized static probability map.
[0015] Furthermore, the reprojection error terms of point, line and plane features for pose estimation are constructed by the following formula:
[0016]
[0017]
[0018]
[0019]
[0020]
[0021] Among them, e p , e l and e π are the reprojection error terms of point, line and plane features respectively; p obs represents the pixel coordinates of the point feature, Π(·) represents the projection function of the camera model; P w Represents the coordinates of the map point, R cw and t cw Represent the posture and position of the current frame, T cw Indicated by R cw and t cw The posture formed, represents the transformation matrix that transforms the plane coefficients from the world coordinate system to the camera coordinate system; The normal vector representing the line feature in the image; Indicates the endpoint of the line feature, x∈{start, end}; q(π c ) represents plane π c The spherical coordinate parameterization form of π w Represents the plane coefficient in the world coordinate system; and represents the projection error term of parallel and perpendicular planes; q n (·) represents the spherical coordinate parameterized form of the plane normal vector, n c Represents the coordinates of the plane normal vector in the camera coordinate system, n w Represents the coordinates of the plane normal vector in the world coordinate system, R ⊥ represents the rotation matrix of 90°; E(ξ, b) represents the reprojection error of point, line and plane features, ξ represents the Lie algebra set of posture state quantities, b represents the set of weights of regional features; p represents point features, represents a set of point features; b i (·) represents the weight of region i containing the corresponding feature; F(r) represents the Cauchy robust kernel function with variable r; l represents the line feature, represents the line feature set; k represents the plane feature; ∏ represents the plane feature set; π k Indicates the same plane constraint; Represents parallel plane constraints; Vertical plane constraints; represents the inverse matrix of the covariance matrix; c represents the boundary between low residual and high residual.
[0022] Furthermore, the residual regularization term is constructed by the following formula:
[0023]
[0024]
[0025]
[0026] Among them, S E (b) represents the residual regularization term; K represents the number of regions; is a control parameter; represents the total number of observed map elements in each region i; is the average residual of points, lines and surfaces in the current frame; r min represents the minimum residual two norm;
[0027] The regional regularization term is constructed by the following formula:
[0028]
[0029] Among them, S R (b) Regional regularization term; G is an adjacency matrix. When regions i and j are adjacent in space, the corresponding matrix element G(i, j) is 1, otherwise it is 0; λ R Indicates S R (b) The weight compared to other residual terms and regularization terms;
[0030] The spatial regularization term is constructed by the following formula:
[0031]
[0032]
[0033]
[0034]
[0035]
[0036] Among them, S G (b) represents the spatial regularization term, λ P , and λ π Both represent the weights of these geometric constraints in the overall optimization. and i Respectively represent the set of map points, lines and plane features observed in region i, Z D (·) represents the depth of the feature in the camera coordinate system of the current frame, Z M (·) represents the depth of the feature in the camera coordinate system of the previous frame, x k Represents feature points, and Represents the two endpoints of the line feature, π krepresents a plane, |Z D (x k )-Z M (x k )| represents the depth difference of the feature point between two frames, and represents the depth difference between the two endpoints of the line feature in two frames, |Z D (π k )-Z M (π k )| represents the depth difference of the plane between the two frames, k p , k p and k p They are all control parameters for adjusting the influence of the corresponding depth difference. and Respectively represent the parameters that affect the weight optimization process by the spatial consistency of point features, line features, and surface features.
[0037] Furthermore, the objective function for jointly optimizing pose and dynamic object segmentation is:
[0038] S(b)=S E (b)+S R (b)+S G (b)
[0039]
[0040] Among them, S(b) represents the constraint term for dynamic segmentation; ξ represents the Lie algebra set of posture state quantities; b represents the set of regional feature weights.
[0041] Further, the updating and optimizing of the static probability map according to the posture state quantity and the weight estimation value of each regional feature includes:
[0042] The weights of the map elements in the static probability map are optimized by the following formula:
[0043]
[0044]
[0045]
[0046] Among them, ω k Represents the weight of the map element before updating, ω k+1 Indicates the updated weight of the map element; Δw k represents the weight update amount; b represents the weight of the regional feature, Represents the initial update amount of weight calculated based on regional feature weights;
[0047] Determine whether the number of map points observed in the current frame is less than 90% of the reference key frame. If so, set the current frame as the new key frame;
[0048] After generating a new key frame, the static probability map and the corresponding camera pose state are optimized by the following formula:
[0049]
[0050] Among them, p c Indicates a map point, l c represents a map line, π c represents the map plane, b c represents the weight, ξ c Represents the corresponding camera posture state.
[0051] Furthermore, after generating the initialized static probability map, it also includes:
[0052] Using the real-time semantic segmentation network STDC-Seg2 to perform semantic segmentation on the color image and count the number of pixels of potential dynamic objects;
[0053] Comparing the number of pixels of the potential dynamic object with the total number of pixels in the color image, when the proportion of the number of pixels of the potential dynamic object to the total number of pixels of the image exceeds 60%;
[0054] In the static probability map, the weight of the region where the pixels of the potential dynamic object are located is reduced by the following formula:
[0055] b i '=kb i and k∈(0,1)
[0056] Among them, b i represents the weight of region i.
[0057] Furthermore, after acquiring point features and line features corresponding to the color image according to the color image, and acquiring plane features corresponding to the depth image according to the depth image, the method further includes:
[0058] For point features, the Hamming distance between the point feature and the Steer BRIEFT descriptor vector corresponding to the point feature of the previous frame of color image is calculated by the FLANN algorithm, and the point with the smallest distance is selected as the matching item to obtain the matching of point features between different frames and generate a map point set;
[0059] For line features, the Hamming distance between the line feature and the LBD descriptor vector of the line feature corresponding to the previous frame of color image is calculated by a brute force matching algorithm, and the line segment with the smallest distance is selected as the matching item to obtain the matching of line features between different frames and generate a map line segment set;
[0060] For plane features, the plane features of the depth image are projected to the world coordinate system, the existing map planes in the global map are traversed, the planes that match the plane features of the depth image of the current frame are obtained, and a map plane set is generated.
[0061] Based on the above method embodiment, the present invention provides a corresponding device embodiment;
[0062] The present invention provides a mapping device for joint optimization of point, line and surface features and dynamic segmentation, including: an image acquisition module, a feature acquisition module, an initialization map module, a static probability map module, a feature weight module and a map construction module;
[0063] The image acquisition module is used to acquire a color image and a depth image corresponding to the color image;
[0064] The feature acquisition module is used to acquire point features and line features corresponding to the color image according to the color image, and to acquire plane features corresponding to the depth image according to the depth image;
[0065] The initialization map module is used to perform posture and map initialization according to the point features and line features corresponding to the color image and the plane features corresponding to the depth image, so as to generate an initialization map with point, line and plane features;
[0066] The static probability map module is used to divide the initialized image into various regions according to the depth value of each pixel in the depth image, and assign a preset weight value to each region to generate an initialized static probability map;
[0067] The feature weight module is used to construct reprojection error terms of point, line and plane features for pose estimation according to the point features and line features corresponding to the color image and the plane features corresponding to the depth image, and to construct residual regularization terms, regional regularization terms and space regularization terms for dynamic object segmentation; based on the reprojection error terms, residual regularization terms, regional regularization terms and space regularization terms, generate an objective function for jointly optimizing pose and dynamic object segmentation, so as to calculate and generate weight estimation values of pose state quantities and various regional features through the objective function;
[0068] The map construction module is used to update and optimize the static probability map and construct a map surface according to the posture state quantity and the weight estimation value of each regional feature.
[0069] The embodiments of the present invention have the following beneficial effects:
[0070] The present invention provides a mapping method and device for jointly optimizing point, line and plane features and dynamic segmentation. The method first obtains corresponding point features, line features and plane features according to a color image and a depth image to generate an initialization map with point, line and plane features. Then, according to the depth value of each pixel in the depth image, the initialization map is divided into different areas and a preset weight value is assigned to generate a static probability map. Then, according to the point features, line features and plane features, a reprojection error term of the point, line and plane features for pose estimation is constructed, and a residual regularization term, a regional regularization term and a spatial regularization term for dynamic object segmentation are constructed. Then, according to these constructed terms, an objective function for jointly optimizing pose and dynamic object segmentation is generated, and the pose state quantity and the weight estimation value of each regional feature are calculated and generated by the objective function. Finally, according to the pose state quantity and the weight estimation value of each regional feature, the map elements in the static probability map are updated and optimized, and the map surface is constructed. By implementing the present invention, the present invention uses weight and posture optimization instead of direct feature elimination in the process of dynamic object segmentation, so that the extraction and utilization of environmental information are more sufficient and more robust, so as to improve the adaptability to weak texture environments. At the same time, the present invention divides the map area and assigns the same weight to each area, so that the matrix scale in the optimization process is reduced, the calculation amount is reduced, the optimization time is shortened, and the real-time performance of the system is improved. BRIEF DESCRIPTION OF THE DRAWINGS
[0071] Figure 1 It is a schematic diagram of the flow of a mapping method for joint optimization of point, line and surface features and dynamic segmentation provided by an embodiment of the present invention;
[0072] Figure 2 It is a schematic diagram of the overall process of a mapping method for joint optimization of point, line and surface features and dynamic segmentation provided by an embodiment of the present invention;
[0073] Figure 3 It is a structural schematic diagram of a mapping device for joint optimization of point, line and surface features and dynamic segmentation provided by an embodiment of the present invention. DETAILED DESCRIPTION
[0074] The technical solution of the present invention will be described clearly and completely below in conjunction with the accompanying drawings of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0075] like Figure 1 As shown, an embodiment provides a mapping method for jointly optimizing point, line, and surface features and dynamic segmentation, including:
[0076] Step S101: Acquire a color image and a depth image corresponding to the color image;
[0077] Step S102: acquiring point features and line features corresponding to the color image according to the color image, and acquiring plane features corresponding to the depth image according to the depth image;
[0078] Step S103: performing pose and map initialization according to the point features and line features corresponding to the color image and the plane features corresponding to the depth image to generate an initialized map with point, line and plane features;
[0079] Step S104: Divide the initialization image into various regions according to the depth value of each pixel in the depth image, and assign a preset weight value to each region to generate an initialized static probability map;
[0080] Step S105: constructing reprojection error terms of point, line and plane features for pose estimation according to the point features and line features corresponding to the color image and the plane features corresponding to the depth image, and constructing residual regularization terms, regional regularization terms and space regularization terms for dynamic object segmentation; generating an objective function for jointly optimizing pose and dynamic object segmentation according to the reprojection error terms, residual regularization terms, regional regularization terms and space regularization terms, so as to calculate and generate pose state quantities and weight estimates of various regional features through the objective function;
[0081] Step S106: Update and optimize the static probability map according to the posture state quantity and the weight estimation value of each regional feature, and construct a map surface.
[0082] For step S101, in a preferred embodiment, a color image of the environment and a depth image matching the color image are collected by an RGB-D camera;
[0083] For step S102, in a preferred embodiment, the color image is converted into a grayscale image for feature extraction. First, extract the ORB feature points and calculate their Steer BRIEFT descriptor vectors to obtain point features. Then, use the LSD line segment detector to extract the line segments of the image and calculate the LBD descriptor vector to obtain line features. In addition, based on the depth image to restore the point cloud of a single frame, use the AHC method to extract plane features from the downsampled point cloud, calculate the plane coefficients (n, d) and support points, n represents the normal vector of the plane, d represents the distance between the plane and the origin of the camera coordinate system, and the support point is a fixed point of the plane; the descriptor vector calculation method belongs to any descriptor vector calculation method in the prior art, and the present invention does not make a specific limitation.
[0084] In a preferred embodiment, after obtaining point features and line features corresponding to the color image according to the color image, and obtaining plane features corresponding to the depth image according to the depth image, the method further includes:
[0085] For point features, the Hamming distance between the point feature and the Steer BRIEFT descriptor vector corresponding to the point feature of the previous frame of color image is calculated by the FLANN algorithm, and the point with the smallest distance is selected as the matching item to obtain the matching of point features between different frames and generate a map point set;
[0086] For line features, the Hamming distance between the line feature and the LBD descriptor vector of the line feature corresponding to the previous frame of color image is calculated by a brute force matching algorithm, and the line segment with the smallest distance is selected as the matching item to obtain the matching of line features between different frames and generate a map line segment set;
[0087] For plane features, the plane features of the depth image are projected to the world coordinate system, the existing map planes in the global map are traversed, the planes that match the plane features of the depth image of the current frame are obtained, and a map plane set is generated.
[0088] Specifically, for the feature points of the current frame, the Hamming distance between them and the SteerBRIEFT descriptor vector of the feature points of the previous frame is calculated using the FLANN strategy, and the point with the smallest distance is selected as the matching item. Then, the mismatch is filtered out using the rotation angle histogram to obtain the matching of point features between different frames, thereby determining the set of map points observed in the current frame. For line features, the Hamming distance between them and the LBD descriptor vector of the line features of the previous frame is calculated using a brute force matching strategy, and the line segment with the smallest distance is selected as the matching item to obtain the matching of line features between different frames, thereby finding the observed map line segment set. For the matching of surface features, the plane coefficients are projected to the world coordinate system based on the approximate posture of the current frame, and the map planes in the global map are traversed for matching. If the normal vector angle between the two planes is large, they are matched as vertical planes; if the normal vector angle between the two planes is small, the distance between the two planes is judged. If the distance is small, they are matched as the same plane, and if the distance is large, they are matched as parallel planes. In this way, the map plane (same plane), parallel planes and vertical planes observed in the current frame are determined, which is denoted as Π.
[0089] For step S103, in a preferred embodiment, the pose and map initialization is performed according to the point features and line features corresponding to the color image and the plane features corresponding to the depth image to obtain an initialized map with point, line and surface features, specifically including:
[0090] According to the depth information provided by the depth map, the point features and line features are back-projected to the preset camera coordinate system corresponding to the color map, and the plane features are projected to the preset camera coordinate system to generate an initialized map with point, line and surface features.
[0091] Specifically, according to the depth information provided by the depth map, the first input frame is set as the key frame, and then its point features and line features are back-projected to the preset camera coordinate system corresponding to the color map according to the depth map to become map points and map lines. Since the plane features extracted from the depth map are three-dimensional features themselves, the plane features do not need to be back-projected; then, the camera coordinate system is used as the world coordinate system, and the frame pose is set to the unit matrix, thereby completing the initialization and determining the pose of the first frame, the world coordinate system and the initial map points, lines and surfaces.
[0092] For step S104, in a preferred embodiment, the initialization image is divided into various regions according to the depth value of each pixel in the depth image, and a preset weight value is assigned to each region to generate an initialized static probability map, including:
[0093] According to the depth value of each pixel in the depth image, the initialization image is clustered based on the depth value, the initialization image is divided into several regions, and the point features, line features and plane features in the same region are assigned the same preset weight value to generate an initialized static probability map.
[0094] Specifically, based on the depth value of each pixel in the depth image, the initialization image is subjected to K-Means clustering to divide the initialization image into multiple regions. Then, the same preset weight values are assigned to the points, lines, and plane features in each region, and these weight values are used to characterize the probability that the pixel points are static, thereby generating an initial static probability map.
[0095] Considering that dynamic objects often occupy the entire area of the image, the present invention performs depth-based K-Means clustering on the depth image and divides it into several regions. It is assumed that the motion properties of the features in the region are the same. Therefore, the point, line and plane features in the region have the same weight b. i ∈[0, 1], the weight represents the probability that the feature of the i-th region is static, close to 1 for static, close to 0 for dynamic. Compared with assigning different weights to each feature point, the method of assigning weights based on regions in the present invention can significantly reduce the dimension of the state vector, thereby reducing the dimension of the Hessian matrix of the optimization process, greatly speeding up the solution of the optimization process.
[0096] In a preferred embodiment, after generating the initialized static probability map, the method further includes:
[0097] Using the real-time semantic segmentation network STDC-Seg2 to perform semantic segmentation on the color image and count the number of pixels of potential dynamic objects;
[0098] Comparing the number of pixels of the potential dynamic object with the total number of pixels in the color image, when the proportion of the number of pixels of the potential dynamic object to the total number of pixels of the image exceeds 60%;
[0099] In the static probability map, the weight of the region where the pixels of the potential dynamic object are located is reduced by the following formula:
[0100] b i '=kb i and k∈(0,1)
[0101] Among them, b i represents the weight of region i.
[0102] Specifically, semantic segmentation of key frames. The real-time semantic segmentation network STDC-Seg2 is used to perform semantic segmentation on the latest key frame (current key frame) every ten key frames, and the number of pixels of potential dynamic objects (such as people, cats and dogs, etc.) is counted. The system sets the first frame of the input as a key frame by default, so starting from the first frame, semantic segmentation is performed every ten key frames. If the proportion of the potential dynamic area to the total number of image pixels exceeds 60%, it is considered that the dynamic object occupies most of the field of view. At this time, the weights of these areas need to be reduced to reduce the estimation error caused by the dynamic object. Otherwise, it is considered that the area they occupy in the image is not large, and the optimized weights have sufficient robustness and no additional processing is performed. This method not only reduces the false negatives caused by dynamic objects occupying most of the field of view, reduces the error of the joint optimization results of pose estimation and dynamic object segmentation, but also reduces the frequency of use of neural networks, significantly reducing the computational burden of neural networks on the SLAM system.
[0103] For step S105, in a preferred embodiment, the pose refers to the position and posture of the camera at the current moment, the map refers to the points, lines and surfaces in the three-dimensional space, and the region refers to the region in the image;
[0104] First, according to the point features and line features corresponding to the color image, and the plane features corresponding to the depth image, the reprojection error terms of the point, line and plane features for pose estimation are constructed respectively, as well as the residual regularization term, regional regularization term and space regularization term for dynamic object segmentation. Then, according to the reprojection error term, residual regularization term, regional regularization term and space regularization term constructed above, the objective function for jointly optimizing pose and dynamic object segmentation is generated. Then, the pose state quantity of the camera at the current moment and the weight estimation value of each regional feature in the image can be calculated through the constructed objective function.
[0105] In a preferred embodiment, the reprojection error terms of point, line and plane features for pose estimation are constructed by the following formula:
[0106]
[0107]
[0108]
[0109]
[0110]
[0111] Among them, e p , e l and e π are the reprojection error terms of point, line and plane features respectively; p obs represents the pixel coordinates of the point feature, Π(·) represents the projection function of the camera model; P w Represents the coordinates of the map point, R cw and t cw Represent the posture and position of the current frame, T cw Indicated by R cw and t cw The posture formed, represents the transformation matrix that transforms the plane coefficients from the world coordinate system to the camera coordinate system; The normal vector representing the line feature in the image; Indicates the endpoint of the line feature, x∈{start, end}; q(π c ) represents plane π c The spherical coordinate parameterization form of π w Represents the plane coefficient in the world coordinate system; and represents the projection error term of parallel and perpendicular planes; q n (·) represents the spherical coordinate parameterized form of the plane normal vector, n c Represents the coordinates of the plane normal vector in the camera coordinate system, n w Represents the coordinates of the plane normal vector in the world coordinate system, R ⊥ represents the rotation matrix of 90°; E(ξ, b) represents the reprojection error of point, line and plane features, ξ represents the Lie algebra set of posture state quantities, b represents the set of weights of regional features; p represents point features, represents a set of point features; b i (·) represents the weight of region i containing the corresponding feature; F(r) represents the Cauchy robust kernel function with variable r; l represents the line feature, represents the line feature set; k represents the plane feature; Π represents the plane feature set; π k Indicates the same plane constraint; Represents parallel plane constraints; Vertical plane constraints; represents the inverse matrix of the covariance matrix; c represents the boundary between low residual and high residual.
[0112] In a preferred embodiment, the residual regularization term is constructed by the following formula:
[0113]
[0114]
[0115]
[0116] Among them, S E (b) represents the residual regularization term; K represents the number of regions; is a control parameter; represents the total number of observed map elements in each region i; is the average residual of points, lines and surfaces in the current frame; r min represents the minimum residual two-norm;
[0117] The regional regularization term is constructed by the following formula:
[0118]
[0119] Among them, S R (b) Regional regularization term; G is an adjacency matrix. When regions i and j are adjacent in space, the corresponding matrix element G(i, j) is 1, otherwise it is 0; λ R Indicates S R (b) The weight compared to other residual terms and regularization terms;
[0120] The spatial regularization term is constructed by the following formula:
[0121]
[0122]
[0123]
[0124]
[0125]
[0126] Among them, S G (b) represents the spatial regularization term; λ P , and λ π Both represent the weights of these geometric constraints in the overall optimization; and Π i Respectively represent the set of map points, lines and plane features observed in area i; Z D (·) represents the depth of the feature in the camera coordinate system of the current frame, Z M (·) represents the depth of the feature in the camera coordinate system of the previous frame; x k Represents feature points, and Represents the two endpoints of the line feature, π k Represents a plane; |Z D (x k )-Z M (x k )| represents the depth difference of the feature point between two frames, k p It is the control parameter that adjusts the degree of its influence; and Represents the depth difference between the two endpoints of the line feature in two frames, is the control parameter that adjusts its influence; |Z D (π k )-Z M (π k )| represents the depth difference of the plane between the two frames, k π Indicates the control parameter that adjusts its influence. and They respectively represent the influencing parameters of the spatial consistency of point features, line features and surface features on the weight optimization process.
[0127] In a preferred embodiment, the objective function for jointly optimizing pose and dynamic object segmentation is:
[0128] S(b)=SE (b)+S R (b)+S G (b)
[0129]
[0130] Among them, S(b) represents the constraint term for dynamic segmentation; ξ represents the Lie algebra set of posture state quantities; b represents the set of regional feature weights.
[0131] Specifically, for the joint optimization of pose and dynamic object segmentation, the method of directly eliminating features whose reprojection errors exceed the threshold is generally adopted to complete dynamic segmentation. The present invention models dynamic segmentation as an optimization problem, and replaces direct elimination with weighted optimization to make better use of static features. In order to jointly optimize these two tasks, the present invention models dynamic segmentation as an optimization problem of feature weights, which together with the reprojection error of the features constitute a new objective function, so that the dynamic properties of the camera pose and feature weights can be estimated by minimizing the objective function. The objective function is shown in the following formula:
[0132]
[0133] E(ξ, b) is the reprojection error (residual) of point, line and plane features, which is used for pose estimation; S(b) represents the constraint term for dynamic segmentation; ξ represents the Lie algebra set of pose state quantities; b represents the set of regional feature weights. The basic idea is to identify regions with high feature reprojection errors as dynamic regions and assign them low weights to weaken their influence in E(ξ, b). In addition, the constraint term in S(b) makes the result of dynamic segmentation spatially consistent and the segmentation more accurate.
[0134] In an optional embodiment, for the pose estimation part E(ξ, b), according to the feature matching relationship between the point, line and plane features of the current frame and the reference key frame, their projection error terms are constructed, and the formula is as follows:
[0135]
[0136] Among them, e p , e l and e π are the reprojection error terms of point, line and plane features respectively, and Π(·) is the projection function of the camera model, which projects the point features and line features in the three-dimensional space to the image plane. obs is the pixel coordinate of the feature point, R cw and t cw Represent the posture and position of the current frame, T cw Indicated by R cw and t cw The pose formed. is the endpoint of the 3D line segment feature, x∈{start,end}, Represents the normal vector of the line segment feature in the image, q(π c ) is the plane π c The spherical coordinate parameterization of .
[0137] Then, according to the parallel and vertical plane matching obtained by feature matching, the projection error terms of the parallel and vertical structures are constructed respectively. and They are observational constraints on the amount of rotation and thus can improve R cw The estimated accuracy of is as follows:
[0138]
[0139] Next, the reprojection error terms of point, line, and plane features are composed of , as shown in the following formula:
[0140]
[0141]
[0142]
[0143] In an optional embodiment, for the dynamic object segmentation part S(b), in the optimization, by minimizing the residual regularization term S E (b) The regions with high average residual (reprojection error) are classified as dynamic regions and assigned low weights; the regions with low average residual are classified as static regions and assigned high weights. In addition, the regional regularization term S is reasonably set. R (b) and the spatial regularization term S G (b) They make the segmentation results have stronger spatial continuity and avoid scattered misidentification. The formula is as follows:
[0144] S(b)=S E (b)+S R (b)+S G (b).
[0145] For the residual regularization term S E (b), as shown in the following formula:
[0146]
[0147]
[0148]
[0149] The total number of observed map elements in each region i is given by Indicates that the number of regions in the image is K, is a control parameter. As shown in the formula, the smaller its value is, the faster the weight decreases. When S E (b) combined with E(ξ, b) makes b i Accelerate iterations towards 0. is the average residual of points, lines and surfaces of the current frame, as shown in the formula. min Represents the minimum residual two norm, the purpose is to prevent in a static environment Too low will cause false positives (static areas are segmented as dynamic areas). They are calculated before each round of optimization iteration.
[0150] For the regional regularization term S R (b), as shown in the following formula:
[0151]
[0152] For a frame of image, it is very effective prior information that adjacent regions have similar static / dynamic properties. Based on this, the present invention constructs the regularization term S R (b), which makes the segmentation results of adjacent regions tend to be consistent, and the spatial consistency is better. Where G is an adjacency matrix: when regions i and j are adjacent in space, the corresponding matrix element G(i, j) is 1, otherwise it is 0. R Indicates S R (b) Weight compared to other residual terms and regularization terms.
[0153] For the spatial regularization term S G (b) If a new dynamic object appears, the depth of the point, the distance from the line segment to the optical center, and the distance from the plane to the optical center in the area between the two frames will change significantly, while the static area will change very little. The present invention uses this prior information to construct the spatial constraint term S for dynamic segmentation. G (b), which makes the optimization result more accurate. In addition, when false negatives occur (dynamic regions are segmented into static regions), compared with E(ξ, b), S G (b) Without pre-weighting based on weight b, the dynamic region makes S G (b) With a larger value, the optimization direction can be corrected so that the dynamic area is segmented, as shown in the following formula:
[0154]
[0155]
[0156]
[0157]
[0158]
[0159] Among them, λ P , P′ and λ π is the weight used to weight these geometric constraints in the overall optimization. p , k p′ and k π Respectively reflects the depth difference of the feature points between the two frames (|Z D (x k )-Z m (x k )|), Depth difference between line segment endpoints and the distance difference from the plane to the optical center (|d D (π k )-d M (π k )|) has an impact on dynamic segmentation. The greater the depth difference and distance difference, the greater the dynamic probability, and the optimization process will make the b of the corresponding area i The smaller. and Π i They represent the set of map points, lines and plane features observed in area i respectively.
[0160] The objective function is composed of all residual terms, and then the estimated values of the posture state quantity ξ and the weight b of the regional feature are obtained by minimizing the objective function. The objective function is a nonlinear non-convex function of ξ and does not have an analytical solution for ξ, but it is a convex function of b and has an analytical solution for b. Therefore, the present invention decouples the solution of ξ and b in the process of optimizing the objective function. First, the initial value of b is set to the weighted average of the map points, lines and surfaces observed in each area, and then the iterative reweighted least squares method (IRLS) is used to minimize the objective function. The analytical solution of b is calculated after each iteration. Through this solution method, the estimation accuracy and convergence speed are effectively improved.
[0161] In an optional embodiment, the local map is tracked for pose optimization: the key frames, their adjacent key frames and parent-child key frames that have co-viewing map elements with the current frame are selected as local key frames, and the map elements whose static probability ω of the local key frames is greater than 0.5 are used as local maps. The local map is projected to the current frame to establish more feature matching relationships, and the objective function is constructed based on the observation constraints of these features to optimize the pose ξ and weight b.
[0162] For step S106, in a preferred embodiment, updating and optimizing the static probability map according to the posture state quantity and the weight estimation value of each regional feature specifically includes:
[0163] The weights of the map elements in the static probability map are optimized by the following formula:
[0164]
[0165]
[0166]
[0167] Among them, ω k Represents the weight of the map element before updating, ω k+1 Indicates the updated weight of the map element; Δw k represents the weight update amount; b represents the weight of the regional feature, Represents the initial update amount of weight calculated based on regional feature weights;
[0168] Specifically, in order to flexibly update the map, the present invention uses logarithmic probability to update the static probability of map elements; after optimizing the feature weights of each area of the current frame, the static probability ω of the map element is updated according to the observation of the local map element and the weight b. k Updated to ω k+1 When the map element is observed to be dynamic (b<0.5), the logarithmic probability decreases, and if it is static (b>0.5), it increases, and the static attributes of the map are dynamically updated based on the observation. As the number of frames increases, the static probability map is dynamically updated by observing, realizing the process of "from coarse to fine" of the static map, and the dynamic objects gradually disappear in the map, which is more in line with the application requirements of the time-varying real environment. When tracking map elements in subsequent frames for pose estimation and static dense mapping, the logarithmic probability is converted into probability to determine whether it is a static feature.
[0169] Determine whether the number of map points observed in the current frame is less than 90% of the reference key frame. If so, set the current frame as the new key frame.
[0170] After generating a new key frame, the static probability map and the corresponding camera pose state are optimized by the following formula:
[0171]
[0172] Among them, p c Indicates a map point, l c represents a map line, π c represents the map plane, b c represents the weight, ξ c Represents the corresponding camera posture state.
[0173] Specifically, map points, lines and planes with poor observation quality are removed according to the observation of the map; new map points, lines and planes are created according to the characteristics of the current key frame, and their static probabilities (weights) are initialized; and the repeatedly observed map points, lines and planes are fused. The objective function is constructed according to the observation constraints of the map points, lines and planes, and the current common view key frame κ is optimized by minimizing the objective function. c The poses of the map points they observe are c , line l c and plane π c and weight b c .
[0174] In an optional embodiment, if the current frame is the first frame, it is set as a key frame by default. If a new key frame is generated, the points, line segments and plane features that are not matched with the map are created as 3D map points, lines and planes, added to the map, and then the new key frame is semantically segmented, static probability initialization of the newly created map elements is performed, and local mapping and optimization are performed.
[0175] In an optional embodiment, static probability initialization of newly created map elements: newly created map elements in the pose and map initialization phase are weighted to 0.3 based on whether their observed positions (i.e., feature point coordinates) in the first frame are located in the prior static region of the semantic segmentation result, otherwise, the weight is set to 0.7. For newly created map elements in the newly added keyframes, the static probability of the map element of the same type that is closest to them is selected as the initial value.
[0176] In an optional embodiment, a dense map is created. The key frames in the map are traversed and divided into superpixels. The superpixels are determined to be retained (the static area accounts for more than 50%) based on the obtained dynamic and static area segmentation results. The map surface is constructed based on the retained superpixels to complete the static dense map construction.
[0177] Schematically, the overall map construction flow chart described above is as follows Figure 2 shown.
[0178] The implementation of the above embodiments of the present invention has the following effects:
[0179] 1. The present invention is a dynamic SLAM method that tightly couples static point, line and surface features, which can more fully extract and utilize environmental information, especially can maintain stable pose estimation in a dynamic weak-texture environment, and the model is more robust.
[0180] 2. The present invention tightly couples dynamic segmentation and pose estimation into an optimization problem, which uses more observation constraints and more effectively utilizes static features. Iterative optimization of dynamic segmentation results based on observation relationships more effectively eliminates the error accumulation caused by staged processing of dynamic segmentation and pose estimation. At the same time, the tight coupling of the two also reduces the delay of staged processing, and the real-time performance is stronger.
[0181] 3. The present invention processes the dynamic attributes of the features in different regions and assigns the same weight to the same region, thereby greatly reducing the dimension of the state vector, reducing the size of the Hessian matrix in the optimization process, significantly reducing the amount of calculation, shortening the time required for optimization, and improving the real-time performance of the system.
[0182] 4. The present invention adds static probability (weight) attributes to the sparse feature map, maintains and updates them in the form of logarithmic probability, and updates the static probability in real time through frame observation. This not only makes the form more flexible, but also ensures the long-term availability of the static probability map.
[0183] 5. The present invention performs semantic segmentation every ten key frames. When potential dynamic objects occupy most of the field of view, the weights of these areas are reduced, thereby realizing the constraint effect of semantic prior information and being more robust to high dynamic environments.
[0184] Based on the above method embodiment, the present invention provides a corresponding device embodiment.
[0185] like Figure 3 As shown, an embodiment of the present invention provides a mapping device for joint optimization of point, line and surface features and dynamic segmentation, including: an image acquisition module, a feature acquisition module, an initialization map module, a static probability map module, a feature weight module and a map construction module;
[0186] The image acquisition module is used to acquire a color image and a depth image corresponding to the color image;
[0187] The feature acquisition module is used to acquire point features and line features corresponding to the color image according to the color image, and to acquire plane features corresponding to the depth image according to the depth image;
[0188] The initialization map module is used to perform posture and map initialization according to the point features and line features corresponding to the color image and the plane features corresponding to the depth image, so as to generate an initialization map with point, line and plane features;
[0189] The static probability map module is used to divide the initialized image into various regions according to the depth value of each pixel in the depth image, and assign a preset weight value to each region to generate an initialized static probability map;
[0190] The feature weight module is used to construct reprojection error terms of point, line and plane features for pose estimation according to the point features and line features corresponding to the color image and the plane features corresponding to the depth image, and to construct residual regularization terms, regional regularization terms and space regularization terms for dynamic object segmentation; based on the reprojection error terms, residual regularization terms, regional regularization terms and space regularization terms, generate an objective function for jointly optimizing pose and dynamic object segmentation, so as to calculate and generate weight estimation values of pose state quantities and various regional features through the objective function;
[0191] The map construction module is used to update and optimize the static probability map and construct a map surface according to the posture state quantity and the weight estimation value of each regional feature.
[0192] It should be noted that the device embodiments described above are merely schematic, wherein the modules described as separate components may or may not be physically separated, and the components displayed as modules may or may not be physical modules, that is, they may be located in one place, or they may be distributed on multiple network modules. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. In addition, in the accompanying drawings of the device embodiments provided by the present invention, the connection relationship between the modules indicates that there is a communication connection between them, which may be specifically implemented as one or more communication buses or signal lines. Those of ordinary skill in the art can understand and implement it without paying creative labor.
[0193] Those skilled in the art can clearly understand that, for the sake of convenience and brevity, the specific working process of the device described above can refer to the corresponding process in the aforementioned method embodiment, and will not be repeated here.
[0194] By implementing the above-mentioned embodiments of the present invention, by replacing the direct feature elimination method with weight and posture optimization in the process of dynamic object segmentation, the extraction and utilization of environmental information is more sufficient and robust, so as to improve the adaptability of weak texture environment. At the same time, the present invention divides the map area and assigns the same weight to each area, so as to reduce the matrix size in the optimization process, reduce the calculation amount, shorten the optimization time, and improve the real-time performance of the system.
[0195] The above is a preferred embodiment of the present invention. It should be pointed out that a person skilled in the art can make several improvements and modifications without departing from the principle of the present invention. These improvements and modifications are also considered to be within the scope of protection of the present invention.
Claims
1. A mapping method for joint optimization of point, line and surface features and dynamic segmentation, characterized in that: include: Obtain a color image and a depth image corresponding to the color image; According to the color image, obtaining point features and line features corresponding to the color image, and according to the depth image, obtaining plane features corresponding to the depth image; Performing pose and map initialization according to the point features and line features corresponding to the color image and the plane features corresponding to the depth image to generate an initialized map with point, line and plane features; According to the depth value of each pixel in the depth image, the initialized image is divided into various regions, and a preset weight value is assigned to each region to generate an initialized static probability map; According to the point features and line features corresponding to the color image, and the plane features corresponding to the depth image, reprojection error terms of the point, line and plane features for pose estimation are constructed, as well as residual regularization terms, regional regularization terms and space regularization terms for dynamic object segmentation; according to the reprojection error terms, residual regularization terms, regional regularization terms and space regularization terms, an objective function for jointly optimizing pose and dynamic object segmentation is generated, so as to calculate and generate weight estimates of pose state quantities and various regional features through the objective function; Among them, the reprojection error terms of point, line and plane features used for pose estimation are constructed by the following formula: Among them, e p , e l and e π are the reprojection error terms of point, line and plane features respectively; p obs represents the pixel coordinates of the point feature, ∏(·) represents the projection function of the camera model; P w Represents the coordinates of the map point, R cw and t cw Represent the posture and position of the current frame, T cw Indicated by R cw and t cw The posture formed, represents the transformation matrix that transforms the plane coefficients from the world coordinate system to the camera coordinate system; The normal vector representing the line feature in the image; Indicates the endpoint of the line feature, x∈{start, end}; q(π c ) represents plane π c The spherical coordinate parameterization form of π w Represents the plane coefficient in the world coordinate system; and represents the projection error term of parallel and perpendicular planes; q n (·) represents the spherical coordinate parameterized form of the plane normal vector, n c Represents the coordinates of the plane normal vector in the camera coordinate system, n w Represents the coordinates of the plane normal vector in the world coordinate system, R ⊥ represents the rotation matrix of 90°; E(ξ, b) represents the reprojection error of point, line and plane features, ξ represents the Lie algebra set of posture state quantities, b represents the set of weights of regional features; p represents point features, represents a set of point features; b i (·) represents the weight of region i containing the corresponding feature; F(r) represents the Cauchy robust kernel function with variable r; l represents the line feature, represents the line feature set; k represents the plane feature; ∏ represents the plane feature set; π k Indicates the same plane constraint; Represents parallel plane constraints; Vertical plane constraints; represents the inverse matrix of the covariance matrix; c represents the boundary between low residual and high residual; The residual regularization term is constructed by the following formula: Among them, S E (b) represents the residual regularization term; is a control parameter; represents the total number of observed map elements in each region i; is the average residual of points, lines and surfaces in the current frame; r min represents the minimum residual two-norm; K represents the number of regions in the image; The regional regularization term is constructed by the following formula: Among them, S R (b) represents the regional regularization term; G represents an adjacency matrix. When regions i and j are adjacent in space, the corresponding matrix element G(i, j) is 1, otherwise it is 0; λ R Indicates S R (b) The weight compared to other residual terms and regularization terms; The spatial regularization term is constructed by the following formula: Among them, S G (b) represents the spatial regularization term, λ P , and λ π Both represent the weights of these geometric constraints in the overall optimization. and Π i Respectively represent the set of map points, lines and plane features observed in region i, Z D (·) represents the depth of the feature in the camera coordinate system of the current frame, Z M (·) represents the depth of the feature in the camera coordinate system of the previous frame, x k Represents feature points, and Represents the two endpoints of the line feature, π k represents a plane, |Z D (x k )-Z M (x k )| represents the depth difference of the feature point between two frames, and represents the depth difference between the two endpoints of the line feature in two frames, |Z D (π k )-Z M (π k )| represents the depth difference of the plane between the two frames, k p , k p and k p These are control parameters for adjusting the influence of the corresponding depth difference. and Respectively represent the parameters that influence the spatial consistency of point features, line features, and surface features on the weight optimization process; The objective function for jointly optimizing pose and dynamic object segmentation is: S(b)=S E (b)+S R (b)+S G (b) Among them, S(b) represents the constraint term used for dynamic segmentation; According to the posture state quantity and the weight estimation value of each regional feature, the static probability map is updated and optimized, and a map surface is constructed; The weights of the map elements in the static probability map are optimized by the following formula: Among them, ω k Represents the weight of the map element before updating, ω k+1 Indicates the updated weight of the map element; Δw k represents the weight update amount; b represents the weight of the regional feature, Represents the initial update amount of weight calculated based on regional feature weights; Determine whether the number of map points observed in the current frame is less than 90% of the reference key frame. If so, set the current frame as the new key frame; After generating a new key frame, the static probability map and the corresponding camera pose state are optimized by the following formula: Among them, p c Indicates a map point, l c represents a map line, π c represents the map plane, b c represents the weight, ξ c Represents the corresponding camera posture state.
2. The mapping method for joint optimization of point, line and surface features and dynamic segmentation as claimed in claim 1, characterized in that: Performing pose and map initialization according to the point features and line features corresponding to the color image and the plane features corresponding to the depth image to obtain an initialized map with point, line and plane features, including: According to the depth information provided by the depth map, the point features and line features are back-projected to the preset camera coordinate system corresponding to the color map, and the plane features are projected to the preset camera coordinate system to generate an initialized map with point, line and surface features.
3. The mapping method for joint optimization of point, line and surface features and dynamic segmentation as claimed in claim 2, characterized in that: According to the depth value of each pixel in the depth image, the initialized image is divided into various regions, and a preset weight value is assigned to each region to generate an initialized static probability map, including: According to the depth value of each pixel in the depth image, the initialization image is clustered based on the depth value, the initialization image is divided into several regions, and the point features, line features and plane features in the same region are assigned the same preset weight value to generate an initialized static probability map.
4. The mapping method for joint optimization of point, line and surface features and dynamic segmentation as claimed in claim 3, characterized in that: After generating the initialized static probability map, it also includes: Using the real-time semantic segmentation network STDC-Seg2 to perform semantic segmentation on the color image and count the number of pixels of potential dynamic objects; Comparing the number of pixels of the potential dynamic object with the total number of pixels in the color image, when the proportion of the number of pixels of the potential dynamic object to the total number of pixels of the image exceeds 60%; In the static probability map, the weight of the region where the pixels of the potential dynamic object are located is reduced by the following formula: b i ′=kb i and k∈(0,1) Among them, b i represents the weight of region i, b i ′ represents the updated weight.
5. The mapping method for joint optimization of point, line and surface features and dynamic segmentation as claimed in claim 4, characterized in that: After acquiring point features and line features corresponding to the color image according to the color image, and acquiring plane features corresponding to the depth image according to the depth image, the method further includes: For point features, the Hamming distance between the point feature and the Steer BRIEFT descriptor vector corresponding to the point feature of the previous frame of color image is calculated by the FLANN algorithm, and the point with the smallest distance is selected as the matching item to obtain the matching of point features between different frames and generate a map point set; For line features, the Hamming distance between the line feature and the LBD descriptor vector of the line feature corresponding to the previous frame of color image is calculated by a brute force matching algorithm, and the line segment with the smallest distance is selected as the matching item to obtain the matching of line features between different frames and generate a map line segment set; For plane features, the plane features of the depth image are projected to the world coordinate system, the existing map planes in the global map are traversed, the planes that match the plane features of the depth image of the current frame are obtained, and a map plane set is generated.
6. A mapping device for joint optimization of point, line and surface features and dynamic segmentation, characterized in that: include: Image acquisition module, feature acquisition module, initialization map module, static probability map module, feature weight module and map construction module; The image acquisition module is used to acquire a color image and a depth image corresponding to the color image; The feature acquisition module is used to acquire point features and line features corresponding to the color image according to the color image, and to acquire plane features corresponding to the depth image according to the depth image; The initialization map module is used to perform posture and map initialization according to the point features and line features corresponding to the color image and the plane features corresponding to the depth image, so as to generate an initialization map with point, line and plane features; The static probability map module is used to divide the initialized image into various regions according to the depth value of each pixel in the depth image, and assign a preset weight value to each region to generate an initialized static probability map; The feature weight module is used to construct reprojection error terms of point, line and plane features for pose estimation according to the point features and line features corresponding to the color image and the plane features corresponding to the depth image, and to construct residual regularization terms, regional regularization terms and space regularization terms for dynamic object segmentation; based on the reprojection error terms, residual regularization terms, regional regularization terms and space regularization terms, generate an objective function for jointly optimizing pose and dynamic object segmentation, so as to calculate and generate weight estimation values of pose state quantities and various regional features through the objective function; Among them, the reprojection error terms of point, line and plane features used for pose estimation are constructed by the following formula: Among them, e p , e l and e π are the reprojection error terms of point, line and plane features respectively; p obs represents the pixel coordinates of the point feature, ∏(·) represents the projection function of the camera model; P w Represents the coordinates of the map point, R cw and t cw Represent the posture and position of the current frame, T cw Indicated by R cw and t cw The posture formed, represents the transformation matrix that transforms the plane coefficients from the world coordinate system to the camera coordinate system; The normal vector representing the line feature in the image; Indicates the endpoint of the line feature, x∈{start, end}; q(π c ) represents plane π c The spherical coordinate parameterization form of π w Represents the plane coefficient in the world coordinate system; and represents the projection error term of parallel and perpendicular planes; q n (·) represents the spherical coordinate parameterized form of the plane normal vector, n c Represents the coordinates of the plane normal vector in the camera coordinate system, n w Represents the coordinates of the plane normal vector in the world coordinate system, R ⊥ represents the rotation matrix of 90°; E(ξ, b) represents the reprojection error of point, line and plane features, ξ represents the Lie algebra set of posture state quantities, b represents the set of weights of regional features; p represents point features, represents a set of point features; b i (·) represents the weight of region i containing the corresponding feature; F(r) represents the Cauchy robust kernel function with variable r; l represents the line feature, represents the line feature set; k represents the plane feature; ∏ represents the plane feature set; π k Indicates the same plane constraint; Represents parallel plane constraints; Vertical plane constraints; represents the inverse matrix of the covariance matrix; c represents the boundary between low residual and high residual; The residual regularization term is constructed by the following formula: Among them, S E (b) represents the residual regularization term; is a control parameter; represents the total number of observed map elements in each region i; is the average residual of points, lines and surfaces in the current frame; r min represents the minimum residual two-norm; K represents the number of regions in the image; The regional regularization term is constructed by the following formula: Among them, S R (b) represents the regional regularization term; G represents an adjacency matrix. When regions i and j are adjacent in space, the corresponding matrix element G(i, j) is 1, otherwise it is 0; λ R Indicates S R (b) The weight compared to other residual terms and regularization terms; The spatial regularization term is constructed by the following formula: Among them, S G (b) represents the spatial regularization term, λ P , and λ π Both represent the weights of these geometric constraints in the overall optimization. and Π i Respectively represent the set of map points, lines and plane features observed in region i, Z D (·) represents the depth of the feature in the camera coordinate system of the current frame, Z M (·) represents the depth of the feature in the camera coordinate system of the previous frame, x k Represents feature points, and Represents the two endpoints of the line feature, π k represents a plane, |Z D (x k )-Z M (x k )| represents the depth difference of the feature point between two frames, and represents the depth difference between the two endpoints of the line feature in two frames, |Z D (π k )-Z M (π k )| represents the depth difference of the plane between the two frames, k p , k p and k p These are control parameters for adjusting the influence of the corresponding depth difference. and Respectively represent the parameters that influence the spatial consistency of point features, line features, and surface features on the weight optimization process; The objective function for jointly optimizing pose and dynamic object segmentation is: S(b)=S E (b)+S R (b)+S G (b) Among them, S(b) represents the constraint term used for dynamic segmentation; The map construction module is used to update and optimize the static probability map and construct a map surface according to the posture state quantity and the weight estimation value of each regional feature; The weights of the map elements in the static probability map are optimized by the following formula: Among them, ω k Represents the weight of the map element before updating, ω k+1 Represents the updated weight of the map element; Δω k represents the weight update amount; b represents the weight of the regional feature, Represents the initial update amount of weight calculated based on regional feature weights; Determine whether the number of map points observed in the current frame is less than 90% of the reference key frame. If so, set the current frame as the new key frame; After generating a new key frame, the static probability map and the corresponding camera pose state are optimized by the following formula: Among them, p c Indicates a map point, l c represents a map line, π c represents the map plane, b c represents the weight, ξ c Represents the corresponding camera posture state.
Citation Information
Patent Citations
Visual sense simultaneous localization and mapping method based on dot and line integrated features
CN106909877A
A binocular vision SLAM method combining point and line characteristics
CN109934862A
Dynamic SLAM method based on reprojection error and depth estimation
CN115619826A
Mobile robot instant positioning and mapping method in locomotive maintenance workshop environment
CN116953723A
Map construction method, apparatus, device and storage medium
EP4124829A1
