Vision slam system and method fusing object plane features

Abstract

The application provides a visual SLAMMOT system and method for fusing object plane features, which introduces an object plane feature extraction and matching method based on a normal vector on the basis of a traditional point feature. Plane regions are determined by analyzing the consistency of adjacent point normal vectors, and stable and accurate extraction and correlation of multi-plane features in a multi-frame and multi-object scene are realized in combination with point feature matching results and plane parameter similarity. In addition, the application also proposes a pose optimization method fusing object plane constraints, which introduces object map point re-projection, object constant speed motion model, point-to-plane and plane-to-plane constraints into a factor graph optimization framework, and further improves the accuracy of object surface reconstruction and pose estimation. The application significantly enhances the performance of the visual SLAMMOT system in a dynamic environment by fusing object plane features, and has high practicability and innovation.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of computer vision, and in particular to a visual SLAMMOT system and method fusing object plane features. BACKGROUND

[0002] Visual Simultaneous Localization and Mapping (SLAM) is a technology that enables a platform equipped with a visual sensor to simultaneously localize itself and build a map in an unknown environment. With the rapid development of fields such as robots and autonomous driving, visual SLAM, as an important technology in the field of computer vision, has received extensive attention in recent years and has become one of the active research directions.

[0003] Existing visual SLAM methods usually assume that the scene is static, that is, the changes between adjacent frames are only caused by camera motion. However, the actual application environment often contains a large number of dynamic objects, which can cause errors in camera pose estimation. To solve this problem, some methods use the random sample consensus (RANSAC) strategy to detect and remove outliers, while other methods reduce errors by detecting movable objects and removing dynamic feature points related to them. However, the pose, motion trend, and other information of dynamic objects are crucial for tasks such as target tracking and obstacle avoidance. Therefore, recent research has begun to explore the combination of visual SLAM and multiple object tracking (MOT), that is, visual SLAMMOT technology.

[0004] Currently, most visual SLAMMOT methods are mainly based on point features. However, the real-world environment usually contains rich and stable plane features, and existing research has shown that these features can be used to improve the accuracy of localization and mapping, and have been applied in some visual SLAM methods. For example, some methods use static plane features such as walls or floors to optimize camera pose estimation. However, the application of plane features in dynamic object pose estimation is still in the exploratory stage, and a mature method system has not yet been formed.

[0005] Therefore, how to effectively utilize plane features in visual SLAMMOT tasks to improve the accuracy of dynamic object pose estimation and reconstruction remains a technical problem to be solved. SUMMARY

[0006] The application provides a visual SLAMMOT system and method for fusing object plane features, which solves the defects in the prior art by accurately extracting and associating the plane features of dynamic objects and incorporating them as constraint conditions into an optimization framework, thereby improving the accuracy of target pose estimation and reconstruction and enhancing the performance of the visual SLAMMOT system in a dynamic environment.

[0007] In a first aspect, the application provides a visual SLAMMOT system for fusing object plane features, comprising:

[0008] an RGB-D camera, an Isaac Sim simulation platform, and a computing device;

[0009] The RGB-D camera collects RGB images and depth information of the target scene;

[0010] The Isaac Sim simulation platform synthesizes the RGB images and depth information to generate RGB-D images;

[0011] The computing device receives the RGB-D images, performs camera pose estimation and static mapping, and object pose estimation and dynamic reconstruction to generate a target scene environment map.

[0012] In a second aspect, the application also provides a visual SLAMMOT method for fusing object plane features, comprising:

[0013] Obtaining RGB-D images of a target scene, performing instance segmentation and normal estimation on the RGB-D images respectively to obtain instance segmentation results and normal estimation results;

[0014] Extracting static point features of the input images, filtering abnormal point features, performing camera initial pose estimation by tracking and minimizing the re-projection error of the static point features, and constructing a camera local map to optimize the camera pose and static map point position;

[0015] Using the instance segmentation results to associate objects in different frames, performing key point tracking and object initial pose estimation on the associated objects, extracting plane features of dynamic objects based on the normal estimation results, and constructing an object local optimization framework that fuses plane constraints by using an association strategy based on matching features and plane parameters to optimize the object pose and dynamic feature parameters;

[0016] Integrating the static processing results and the dynamic processing results to output a target environment map including camera pose, static map points, object pose, dynamic map points, and dynamic planes.

[0017] According to the present invention, a visual SLAMMOT method for fusing planar features of an object extracts static point features from an input image, filters outomaly features, and estimates the initial pose of the camera by tracking the static point features and minimizing the reprojection error. The method includes:

[0018] Extract Shi-Tomasi feature points from the input image, remove feature points on dynamic objects, and retain feature points in the static background;

[0019] The Lucas-Kanade optical flow method is used to track the extracted feature points frame by frame, and the correspondence between key points is established between consecutive frames;

[0020] After keypoint tracking is completed, a reprojection error model between static map points and feature points in the current frame is established by matching them with existing static map points. :

[0021]

[0022] in, For static map points The corresponding number Pixel coordinates of frame feature points For camera projection model, For the first The camera pose of the frame. For static map points Coordinates in the world coordinate system;

[0023] By minimizing the first objective function, we obtain the... Frame-based camera initial pose estimation :

[0024]

[0025] in, In order to be with the first The set of static map points corresponding to the feature points in the frame. is a constant covariance matrix.

[0026] According to the present invention, a visual SLAMMOT method for fusing planar features of objects is provided, which constructs a local camera map to optimize camera pose and static map point positions, including:

[0027] A sliding window optimization framework is constructed to locally optimize the initial pose of the camera and the position of static map points. The frames in the sliding window are set to include fixed frames and frames to be optimized. The fixed frames constrain the frames to be optimized, and the frames to be optimized are adjusted in the local optimization.

[0028] A second objective function is constructed in the sliding window optimization to optimize the camera poses and static map point positions within the sliding window:

[0029]

[0030] wherein, and respectively represent a frame index set and a static map point index set in the to-be-optimized part of the sliding window, is a total frame index set within the sliding window.

[0031] According to the visual SLAM MOT method for fusing object plane features provided by the application, the object in different frames is associated by using the instance segmentation result, the key points of the associated object are tracked and the initial pose of the object is estimated, and the method comprises the following steps:

[0032] The intersection-over-union of the object current frame and the object instance segmentation mask of the last frame is calculated, an association cost matrix is constructed, the association cost matrix is solved by using the KM algorithm, and the optimal matching pair is determined;

[0033] The key points are uniformly sampled according to the size of the object, the key points are tracked between adjacent frames by using the Lucas-Kanade optical flow method, and the key points are verified so that the key points are located in the matched object mask;

[0034] Based on the corresponding relationship between the object map point and the key point obtained by the object data association, an object map point reprojection error term is constructed :

[0035]

[0036] wherein, is the map point of the object , is the pixel coordinate of the corresponding key point of the object frame, is the projection matrix of the camera, is the conversion matrix from the world coordinate system to the camera coordinate system of the object frame, is the conversion matrix from the object coordinate system of the object to the world coordinate system at the object frame, is the coordinate of the map point of the object in the object coordinate system of the object ;

[0037] The object initial pose estimation is obtained by minimizing the third objective function :

[0038]

[0039] wherein, is the set of map points associated with the key points in the frame, is a constant covariance matrix.

[0040] According to the MOT method for fusing plane features of objects provided by the application, the plane features of dynamic objects are extracted based on normal estimation results, and the method comprises the following steps:

[0041] A key point is randomly selected from the set of key points on the surface of the object as a seed point, and the normal vector of the seed point is taken as the reference, and the region is gradually expanded by comparing the similarity of the normal vectors, wherein the normal vector of the current point to be expanded is , and the seed point is expanded into a plane region if the following similarity condition is met:

[0042]

[0043] wherein, is the normal vector similarity threshold in the formula (2) and the formula (3);

[0044] In the process of region expansion, the plane normal vector is constantly updated according to the characteristics of the newly added region to adapt to the expanded plane feature region:

[0045]

[0046] wherein, is the updated plane normal vector, is the plane normal vector before updating, is the total number of plane points;

[0047] The process of region expansion is continuously carried out until there is no more key point meeting the similarity condition;

[0048] When the set of plane key points and the corresponding plane normal vectors are extracted, the final plane parameters are calculated by the following method:

[0049]

[0050] wherein, is the plane normal vector in the camera coordinate system, is the coordinates of the point cloud center corresponding to the set of plane key points in the camera coordinate system, is the plane distance parameter in the camera coordinate system, and the expression of the plane parameters in the camera coordinate system is obtained ;

[0051] ​​When the plane is first observed, it is initialized by transforming it from the camera coordinate system to the object coordinate system:

[0052]

[0053] in, and These represent the transformation matrices between the camera and the world coordinate system, and between the object and the world coordinate system, respectively.

[0054] According to the present invention, a visual SLAMMOT method for fusing planar features of an object is provided. This method constructs a local optimization framework for the object that fuses planar constraints, based on a strategy of associating matching features with planar parameters, to optimize the object's pose and dynamic feature parameters. The method includes:

[0055] After completing the extraction and association of planar features of the object, an error term from point to plane is constructed. and plane-to-plane error term :

[0056]

[0057]

[0058] in, For objects plane Parameters in the object coordinate system It is an object Map points Coordinates in its object coordinate system For objects plane Parameters in the object coordinate system It is the world coordinate system to the 1st The transformation matrix of the frame camera coordinate system. It is an object In the The transformation matrix from the object coordinate system to the world coordinate system at frame time. For objects plane In the Frame-time observation;

[0059] Based on the smoothness of the object's motion and the absence of significant abrupt changes in position, an error term is constructed for the constant-velocity motion model of the object. :

[0060]

[0061] in, This represents the mapping from the Lie group to the Lie algebra. representative object in the first motion transformation between the first frame and the second is the object in the first frame, and the conversion matrix from the object coordinate system to the world coordinate system, is the object in the second frame, and the conversion matrix from the object coordinate system to the world coordinate system;

[0062] Local BA optimization is performed on the object pose, object map points and object planes in the sliding window, and an overall objective function of object BA optimization is constructed:

[0063]

[0064] wherein, are the index sets of the camera frames, objects, object map points and object planes in the sliding window to be optimized, are the covariance matrices corresponding to the object map point re-projection error term, the object constant velocity motion model error term, the point-to-plane error term and the plane-to-plane error term.

[0065] In a third aspect, the present application also provides a visual SLAM MOT device fusing object plane features, comprising:

[0066] An acquisition module is configured to acquire an RGB-D image of a target scene, perform instance segmentation and normal estimation on the RGB-D image respectively, and obtain an instance segmentation result and a normal estimation result;

[0067] A static processing module is configured to extract static point features of an input image, filter abnormal point features, perform camera initial pose estimation by tracking the static point features and minimizing re-projection errors, and construct a camera local map to optimize camera pose and static map point position;

[0068] A dynamic processing module is configured to associate objects in different frames by using the instance segmentation result, track key points of the associated objects and estimate object initial pose, extract plane features of dynamic objects based on the normal estimation result, and construct an object local optimization framework fusing plane constraints by using an association strategy based on matching features and plane parameters to optimize object pose and dynamic feature parameters;

[0069] A comprehensive module is configured to comprehensively integrate static processing results and dynamic processing results, and output a target environment map including camera pose, static map points, object pose, dynamic map points and dynamic planes.

[0070] In a fourth aspect, the present application provides an electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the visual SLAM MOT method of fusing object plane features as described above.

[0071] In a fifth aspect, the present application provides a non-transitory computer readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the visual SLAM MOT method of fusing object plane features as described above.

[0072] The visual SLAM MOT system and method of fusing object plane features provided by the present application add object plane features to the point features commonly used in existing visual SLAM MOT systems, propose an object plane feature extraction and matching method based on normal vectors, determine the plane region through the consistency of adjacent point normal vectors, and perform plane matching in combination with the point feature matching results and plane parameter similarity, thereby realizing stable and accurate extraction and correlation of multiple plane features in a multi-frame and multi-object scene. In addition, an object pose optimization method that fuses object plane constraints is proposed, which incorporates object map point re-projection, object constant speed motion model, object point to plane and plane to plane constraints into the factor graph optimization framework, thereby improving the accuracy of object surface reconstruction and pose estimation. The present application is simple to implement and has strong practicality, effectively solves the limitations of the prior art, improves the robustness and applicability of the system, and has important market application value. BRIEF DESCRIPTION OF DRAWINGS

[0073] In order to more clearly illustrate the technical solutions in the present application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or prior art description. Obviously, the drawings described below are some embodiments of the present application, and for those skilled in the art, other drawings can also be obtained without creative labor.

[0074] Figure 1 is one of the flowcharts of the visual SLAM MOT method of fusing object plane features provided by the present application;

[0075] Figure 2 is another flowchart of the visual SLAM MOT method of fusing object plane features provided by the present application;

[0076] Figure 3 is an object plane constraint diagram provided by the present application;

[0077] Figure 4 is an object BA optimization factor graph provided by the present application;

[0078] Figure 5is a schematic diagram of an image and a trajectory of a synthetic data set provided by the present application;

[0079] Figure 6 is an object plane extraction and correlation visualization result provided by the present application;

[0080] Figure 7 is a structural schematic diagram of a visual SLAM MOT device fusing object plane features provided by the present application;

[0081] Figure 8 is a structural schematic diagram of an electronic device provided by the present application. DETAILED DESCRIPTION

[0082] To make the objectives, technical solutions and advantages of the present application clearer, the technical solutions in the present application will be described clearly and completely below with reference to the drawings in the present application. Obviously, the described embodiments are some embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative work fall within the protection scope of the present application.

[0083] The embodiments of the present application provide a visual SLAM MOT system and method fusing object plane features. The visual SLAM MOT system and method fusing object plane features provided by the embodiments of the present application can be integrated in an electronic device, which can be a terminal, a server or the like, wherein the terminal can include a tablet computer, a notebook computer, a personal computer (PC), a micro processing box or other devices, etc.

[0084] The visual SLAM MOT system and method fusing object plane features provided by the embodiments of the present application can also be integrated on a special system related to a scenario, and the present application provides a visual SLAM MOT system fusing object plane features, which includes:

[0085] an RGB-D camera, an Isaac Sim simulation platform and a computing device;

[0086] The RGB-D camera collects an RGB image and depth information of a target scene;

[0087] The Isaac Sim simulation platform synthesizes the RGB image and the depth information to generate an RGB-D image;

[0088] The computing device receives the RGB-D image, performs camera pose estimation and static mapping and object pose estimation and dynamic reconstruction, and generates an environment map of the target scene.

[0089] Specifically, the hardware device and its use adopted in the embodiment of the present application include: an RGB-Deepth (RGB-D) camera, wherein the RGB-D sensor is mainly used to collect the RGB image and depth information of the scene, and provide visual and spatial data input for the visual SLAM MOT algorithm. The RGB image can be used for feature extraction, instance segmentation, etc., and the depth information can be directly used to generate a three-dimensional point cloud, supporting scene mapping and object reconstruction.

[0090] In the embodiment, the device used in the experiment is an Intel RealSense D435 depth camera, and an NVIDIA Isaac Sim simulation platform is also used to generate high-quality synthetic images and depth data. The depth camera is a device that can capture the depth information of objects in the scene (i.e., the distance between the object and the camera). Unlike traditional two-dimensional cameras, in addition to capturing the color and brightness of the image, the depth camera can also generate a depth map of the scene, which is a depth data about the distance of each pixel in the scene from the camera. The NVIDIA Isaac Sim simulation platform is a cloud-based robot simulation platform designed to accelerate the development and testing of robots through high-fidelity simulation environments. Isaac Sim uses NVIDIA's GPU acceleration technology to provide real-time physical simulation and rendering, helping developers quickly iterate and optimize robot designs in a virtual environment.

[0091] The computing device is used to support the running of the algorithm. Since the visual SLAM MOT involves computationally intensive tasks such as instance segmentation, normal estimation, and motion optimization, a high-performance computing device is required for deep learning inference and real-time data processing. In the embodiment, an Intel i7 processor and an NVIDIA RTX 3050 GPU are used in the experiment, which can meet the real-time processing needs of medium-sized scenes.

[0092] It should be noted that the above hardware configuration is an implementation scheme of the embodiment of the present application, which can effectively support the running and optimization of the visual SLAM MOT algorithm, and can be adjusted according to the specific scene requirements in actual application.

[0093] Specifically, in the field of autonomous robots, the present application can be applied to scenarios such as warehouse logistics, service robots, etc., using the planar features of dynamic objects to improve the positioning and navigation capabilities of robots in complex environments, ensuring that they can stably track moving goods or other dynamic targets. In addition, the present method can be used for dynamic target grasping tasks in industrial robots, improving the success rate of grasping and operation accuracy by accurately tracking the planes of target objects. In the field of autonomous driving, the present application can be used to enhance the perception capabilities of vehicles in dynamic environments and improve the stable tracking performance of dynamic targets such as vehicles. By fusing the planar information on the vehicle body, the accuracy of target trajectory prediction can be effectively improved, and the decision-making of the autonomous driving system can be optimized. In augmented reality (AR) and virtual reality (VR) applications, the present application can be used to enhance the interactive experience and motion scene modeling. In AR / VR interactive devices, the use of planar features of dynamic objects can improve the stable tracking capability of moving objects, achieving more accurate virtual-real combination.

[0094] Figure 1 is one of the flowcharts of the visual SLAM MOT method fusing object plane features provided by the embodiments of the present application, as shown in Figure 1 , comprising:

[0095] Step 100: Obtain the RGB-D image of the target scene, and perform instance segmentation and normal estimation on the RGB-D image respectively to obtain the instance segmentation result and the normal estimation result;

[0096] Step 200: Extract the static point features of the input image, filter the abnormal point features, perform camera initial pose estimation by tracking and minimizing the re-projection error of the static point features, and construct a camera local map to optimize the camera pose and the static map point position;

[0097] Step 300: Use the instance segmentation result to associate objects in different frames, track the key points of the associated objects, and estimate the initial pose of the objects, extract the plane features of dynamic objects based on the normal estimation result, and construct an object local optimization framework fusing plane constraints by using the association strategy based on matching features and plane parameters to optimize the object pose and dynamic feature parameters;

[0098] Step 400: Integrate the static processing result and the dynamic processing result, and output the target environment map including the camera pose, the static map point, the object pose, the dynamic map point, and the dynamic plane.

[0099] Specifically, as Figure 2As shown, the embodiment of the present application takes RGB-D images as input, and respectively obtains instance segmentation and surface normal image for subsequent target tracking processing. The system mainly consists of two core modules: camera pose estimation and static mapping module and object pose estimation and dynamic reconstruction module. In the camera pose estimation and static mapping module, first, point feature extraction is performed on the input image, and potential abnormal points caused by dynamic objects are filtered to avoid introducing errors in camera pose estimation. Subsequently, initial pose estimation is performed through feature point tracking and minimization of re-projection error, and finally local map optimization is used to further improve the accuracy of camera pose. In the object pose estimation and dynamic reconstruction module, the instance segmentation mask is used to associate objects in different frames, and key point tracking and initial pose estimation are performed on the successfully associated objects. On this basis, the plane features of dynamic objects are extracted based on the normal image, and through the association strategy based on matching features and plane parameters, stable tracking of object plane features is realized. Subsequently, an optimization framework integrating plane constraints is constructed to further optimize the initial pose of the object, improve the pose estimation and reconstruction accuracy of dynamic targets. Finally, the system of the present application can generate a high-precision environment map including camera pose, static map points, object pose, dynamic map points and dynamic planes, providing more stable and reliable mapping and positioning capabilities for robot autonomous navigation and augmented reality applications.

[0100] In one embodiment, as Figure 2 shown, the input of the embodiment of the present application is an RGB-D image, and YOLACT algorithm and Metric3D algorithm are respectively used to obtain instance segmentation results and normal estimation results. The subsequent implementation process mainly includes two modules: camera pose estimation and static mapping module, and object pose estimation and dynamic reconstruction module.

[0101] ‌YOLACT (You Only Look At Coefficients) is a real-time instance segmentation algorithm, which decomposes the instance segmentation task into two parallel subtasks: generating a set of prototype masks and predicting the mask coefficients of each instance, and then linearly combining the prototype and the mask coefficient to generate the segmented instance. Metric3D algorithm is a geometric basis model algorithm for zero-shot monocular scaleable depth and surface normal estimation, which is used for normal estimation of objects.

[0102] First, in the camera pose estimation and static mapping module, this invention extracts Shi-Tomasi feature points from the input image. To improve the accuracy of camera pose estimation, feature points on dynamic objects are removed, retaining only feature points in the static background. Next, the Lucas-Kanade (LK) optical flow method is used to track these feature points frame by frame, thereby establishing a correspondence between key points across consecutive frames. After completing key point tracking, this invention establishes a reprojection error model between static map points and feature points in the current frame by matching them with existing static map points. The specific expression is as follows:

[0103]

[0104] in, For static map points The corresponding number Pixel coordinates of frame feature points For camera projection model, For the first The camera pose of the frame. For static map points The coordinates of in the world coordinate system.

[0105] The th objective function can be estimated by minimizing the following objective function. Camera pose of the frame:

[0106]

[0107] in, In order to be with the first The set of static map points corresponding to the feature points in the frame. is a constant covariance matrix.

[0108] To reduce pose accumulation errors and ensure local consistency of static map points, this invention constructs a sliding window optimization framework to locally optimize the camera pose and the positions of static map points. The sliding window contains a fixed number of frames, divided into two parts: a fixed part and a part to be optimized. The fixed part contains earlier frames that only provide constraints; the part to be optimized contains newer frames that are the focus of local optimization. In this invention, the sliding window contains 20 frames, with 10 frames in the fixed part and 10 frames in the part to be optimized.

[0109] In sliding window optimization, this invention constructs the following objective function to optimize the camera pose and static map point positions within the sliding window:

[0110]

[0111] in, and These represent the set of frame indices and the set of static map point indices in the part of the sliding window to be optimized, respectively. This is the set of all frame indices within the sliding window.

[0112] Through the above implementation methods, the present invention can accurately eliminate the influence of dynamic objects in dynamic scenes, retain effective information in static backgrounds, thereby achieving high-precision estimation of camera pose, and ensuring local consistency and optimization effect of static map points through sliding window optimization.

[0113] After completing camera pose estimation and static reconstruction, dynamic object data association is performed to accurately match dynamic objects between adjacent frames, supporting object pose estimation and 3D reconstruction in dynamic scenes. Specifically, the intersection-over-union (IoU) ratio of the object instance segmentation masks in the current and previous frames is first calculated to construct the association cost matrix. The KM algorithm is then used to solve the cost matrix to determine the optimal matching pair. To ensure the accuracy of object pose estimation and reconstruction, keypoints are uniformly sampled according to the object size, and the LK optical flow method is used to track the keypoints between adjacent frames. The keypoints are then verified to ensure they are located within the matched object mask, further improving the reliability of the association results.

[0114] When estimating the pose of a dynamic object, the correspondence between object map points and key points obtained from object data association can be used to construct an object map point reprojection error term:

[0115]

[0116] in, For objects Map points The corresponding number Pixel coordinates of frame keypoints The projection matrix of the camera, It is the world coordinate system to the 1st The transformation matrix of the frame camera coordinate system. It is an object In the The transformation matrix from the object coordinate system to the world coordinate system at frame time. It is an object Map points The coordinates in its object coordinate system.

[0117] Next, the initial pose of the object is estimated by minimizing the objective function:

[0118]

[0119] in, Is with the first Objects associated with key points in a frame a set of map point indexes, is a constant covariance matrix.

[0120] After the initial pose estimation of the object is completed, a normal vector based region growing algorithm is used for planar feature extraction. The specific implementation steps are as follows: first, a key point in the object surface key point set is randomly selected as a seed point, and the normal vector of the seed point is taken as the benchmark, and the region is gradually expanded by comparing the similarity of the normal vectors. Specifically, suppose the normal vector of the seed point is , the normal vector of the current point to be expanded is , if the following similarity condition is met, the point is expanded to a planar region:

[0121]

[0122] wherein, is the normal vector similarity threshold, which is set to 0.9 in the present application.

[0123] In the process of region expansion, the plane normal vector is constantly updated according to the characteristics of the newly added region, so as to accurately adapt to the expanded planar feature region:

[0124]

[0125] wherein, is the updated plane normal vector, is the plane normal vector before updating, is the total number of plane points.

[0126] The process of region expansion continues until there are no more key points that meet the similarity condition. Through the above process, multiple planar features can be extracted from the target object.

[0127] When the planar key point set and the corresponding plane normal vector are extracted, the final plane parameters can be calculated in the following way:

[0128]

[0129] wherein, is the plane normal vector in the camera coordinate system, is the point cloud center corresponding to the planar key point set in the camera coordinate system, is the plane distance parameter in the camera coordinate system. Thus, the expression of the plane parameters in the camera coordinate system is obtained .

[0130] When the plane is first observed, the plane can be initialized by converting it from the camera coordinate system to the object coordinate system. The conversion is represented as:

[0131]

[0132] wherein, and denote the transformation matrix between camera and world coordinate system and object and world coordinate system, respectively.

[0133] The present application employs the KM algorithm to associate the object plane features. The cost function in this algorithm is constructed based on the number of matched keypoint pairs between the newly extracted plane and the existing object planes. After obtaining the preliminary association, the present application further evaluates the similarity between the plane parameters to ensure the correctness of the match and exclude the obviously incorrect associations. In practical applications, some of the newly extracted planes can not be able to establish an effective association with the existing planes. In this case, the present application employs the aforementioned conversion formula to initialize a new object plane and incorporate it into the subsequent plane feature matching and association process.

[0134] After completing the object plane feature extraction and association, the extracted object planes are used to construct the plane constraints, including the point-to-plane constraint and the plane-to-plane constraint, as shown in Figure 3 Taking into account the structural characteristics of the object surface, ideally, the distance between a point located on a plane and the plane should be zero, i.e., satisfying However, in practical applications, this ideal condition is often difficult to fully satisfy. To solve this problem, the point-to-plane error term is proposed to improve the accuracy of object surface reconstruction and pose estimation:

[0135]

[0136] wherein, is the plane of object in the object coordinate system, is the coordinate of the map point of object in its object coordinate system.

[0137] In addition, since the plane feature is extracted from multiple points, even if individual points are affected by noise, the plane feature can still maintain high stability, which makes the plane feature itself have strong anti-interference ability. Therefore, the inter-frame plane association relationship provides reliable information for object pose estimation. To fully utilize this advantage, the present application proposes the plane-to-plane error term, the expression of which is as follows:

[0138]

[0139] wherein, is the plane of object the parameters in the object coordinate system, is the transformation matrix from the world coordinate system to the camera coordinate system in the first frame, is the transformation matrix from the object coordinate system to the world coordinate system in the first frame, is the plane of the object in the first frame, is the observation of the object in the first frame. is the transformation matrix from the object coordinate system to the world coordinate system in the first frame, is the plane of the object in the first frame, is the observation of the object in the first frame.

[0140] The above error terms fully combine the structural characteristics of the object surface and utilize the stability of the plane features to provide additional geometric constraints for object pose estimation. By minimizing the corresponding error function, not only the accuracy of surface reconstruction can be improved, but also the object pose estimation can be effectively optimized.

[0141] After completing the initial object pose estimation, the present application further optimizes the object pose in the local map. The method separates the BA (Bundle Adjustment) optimization process of the camera and the object to reduce the mutual interference between the initial rough poses, thereby improving the calculation efficiency of the overall optimization. In the optimization process, in addition to the re-projection error term of the object map points, an object constant motion model error term, as well as a point-to-plane and plane-to-plane error term are introduced. These error terms are uniformly constructed into the factor graph of the object BA, as shown in Figure 4 .

[0142] In view of the smoothness of the object motion and the characteristics of no obvious mutation in position, the object constant motion model error term is constructed:

[0143]

[0144] wherein, represents the mapping from the Lie group to the Lie algebra, represents the motion transformation of the object between the first frame and the second frame, is the transformation matrix from the object coordinate system to the world coordinate system in the first frame, is the transformation matrix from the object coordinate system to the world coordinate system in the first frame. is the transformation matrix from the object coordinate system to the world coordinate system in the first frame. is the transformation matrix from the object coordinate system to the world coordinate system in the first frame. is the transformation matrix from the object coordinate system to the world coordinate system in the first frame. is the transformation matrix from the object coordinate system to the world coordinate system in the first frame. is the transformation matrix from the object coordinate system to the world coordinate system in the first frame. is the transformation matrix from the object coordinate system to the world coordinate system in the first frame. is the transformation matrix from the object coordinate system to the world coordinate system in the first frame.

[0145] Finally, the present application performs local BA optimization on the object pose, object map points and object planes in the sliding window to improve the object pose estimation and reconstruction accuracy. The overall objective function of the object BA optimization is defined as follows: ​​

[0146]

[0147] wherein, are the index sets of camera frames, objects, object map points, and object planes in the sliding window to be optimized, respectively, are the covariance matrices corresponding to the object map point re-projection error term, the object constant velocity motion model error term, the point-to-plane error term, and the plane-to-plane error term, respectively.

[0148] The system of the present application can generate a high-precision environment map including camera poses, static map points, object poses, dynamic map points, and dynamic planes, and provide more stable and reliable mapping and positioning capabilities for robot autonomous navigation and augmented reality applications.

[0149] The visual SLAM MOT method and system fusing object plane features proposed in the present application can effectively improve the object pose estimation and reconstruction accuracy, mainly in the following aspects.

[0150] Firstly, to verify the effectiveness of the plane constraint method proposed in the present application, the present application uses the Isaac Sim platform to construct a warehouse environment and generates five experimental sequences: cube_3dof_cam_static, cube_3dof_cam_moving, cube_6dof_cam_static, cube_6dof_cam_moving, and box_cam_moving. The corresponding RGB images, depth images, and instance segmentation masks are generated by the renderer. The generated image and trajectory diagram are shown as Figure 5

[0151] In the experiment, the plane features of the object are extracted and associated, and the results are shown as Figure 6

[0152] ​​For each experimental sequence, this invention designed four experimental modes: no planar constraints, using only point-to-planar constraints, using only planar-to-planar constraints, and using both planar constraints simultaneously. Since local object BA optimization does not affect camera pose, the experiments focused on comparing the object pose accuracy under different modes. The results are shown in Table 1. The experimental data show that even without planar constraints, the availability of point features still ensures the basic functionality of the system. However, adding planar constraints to the local object BA improved the evaluation metrics for most experimental sequences. Further analysis shows that planar-to-planar constraints provide a more significant improvement in accuracy compared to point-to-planar constraints. This is because planar-to-planar constraints directly provide geometric measurement information by adjusting the object pose to align the object surface; while point-to-planar constraints indirectly adjust the object pose by optimizing the position of the object map points. Furthermore, planar constraints reduce relative rotation error (…). The effect is particularly significant in terms of [the object's pose estimation]. This is mainly due to the fact that the normal vector of the object's plane can encode orientation information, and the accurate normal vector estimated by the method of this invention provides additional constraints for object pose estimation, thereby improving rotation accuracy. In more complex motion scenarios (such as cube_6dof_cam_moving) or more realistic environments (such as box_cam_moving), adding planar constraints can achieve comparable or better object pose estimation accuracy.

[0153] Table 1. Object pose estimation results under different planar constraint modes on the synthetic dataset.

[0154]

[0155] Furthermore, Table 2 presents the comparison results with existing visual SLAMMOT methods in camera and object pose estimation. Experimental results show that compared with existing methods, especially the state-of-the-art SDPL-SLAM method utilizing line features, the present invention performs comparably in camera pose estimation, but shows a significant advantage in object pose estimation. Specifically, the relative translation error of the object ( ) and relative rotation error ( The accuracy was reduced by 60.0% and 62.6% respectively. This significant improvement is attributed to two key factors. First, this invention utilizes the planar characteristics of the object's surface to optimize the position of the object's map points, thereby enhancing the accuracy of object pose estimation, especially when noise exists in depth measurement. Second, as a surface structural feature, the planar feature of the object is relatively stable and insensitive to noise; its inter-frame correlation further provides additional useful information, thus improving the accuracy of object pose estimation.

[0156] Table 2 Comparison results of camera and object pose estimation with existing visual SLAM MOT method

[0157]

[0158] The visual SLAM MOT device fusing object plane features provided by the present application is described below, and the visual SLAM MOT device fusing object plane features described below can be mutually corresponding with the visual SLAM MOT method fusing object plane features described above.

[0159] Figure 7 is a structural schematic diagram of the visual SLAM MOT device fusing object plane features provided by the embodiment of the present application, as Figure 7 shown, comprising: an acquisition module 71, a static processing module 72, a dynamic processing module 73 and a comprehensive module 74, wherein:

[0160] The acquisition module 71 is configured to acquire an RGB-D image of a target scene, perform instance segmentation and normal estimation on the RGB-D image respectively, and obtain an instance segmentation result and a normal estimation result; the static processing module 72 is configured to extract static point features of an input image, filter abnormal point features, perform camera initial pose estimation by tracking and minimizing re-projection errors on the static point features, construct a camera local map to optimize camera pose and static map point position; the dynamic processing module 73 is configured to associate data of objects in different frames by using the instance segmentation result, track key points of the associated objects and perform object initial pose estimation, extract plane features of dynamic objects based on the normal estimation result, and construct an object local optimization framework fusing plane constraints by using an association strategy based on matching features and plane parameters to optimize object pose and dynamic feature parameters; and the comprehensive module 74 is configured to comprehensively integrate static processing results and dynamic processing results, and output a target environment map including camera pose, static map points, object pose, dynamic map points and dynamic planes.

[0161] Figure 8 An example of an entity structure schematic diagram of an electronic device is shown in Figure 8As shown, the electronic device can include a processor 810, a communications interface 820, a memory 830, and a communications bus 840, wherein the processor 810, the communications interface 820, and the memory 830 complete mutual communication through the communications bus 840. The processor 810 can invoke the logic instructions in the memory 830 to execute the visual SLAM MOT method of fusing object plane features, which includes: acquiring an RGB-D image of a target scene, performing instance segmentation and normal estimation on the RGB-D image respectively to obtain an instance segmentation result and a normal estimation result; extracting static point features of the input image, filtering abnormal point features, performing camera initial pose estimation by tracking and minimizing re-projection error on the static point features, constructing a camera local map to optimize camera pose and static map point position; using the instance segmentation result to associate objects in different frames, tracking key points of the associated objects and estimating object initial pose, extracting plane features of dynamic objects based on the normal estimation result, constructing an object local optimization framework that fuses plane constraints by using an association strategy based on matching features and plane parameters to optimize object pose and dynamic feature parameters; and integrating static processing results and dynamic processing results to output a target environment map including camera pose, static map points, object pose, dynamic map points, and dynamic planes.

[0162] In addition, the logic instructions in the memory 830 described above can be implemented in the form of a software function unit and sold or used as an independent product, and can be stored in a computer readable storage medium. Based on this understanding, the technical solutions of the present application essentially or the part that contributes to the prior art or part of the technical solutions can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a plurality of instructions to make a computer device (which can be a personal computer, a server, or a network device, etc.) execute all or part of the steps of the method described in various embodiments of the present application. The foregoing storage medium includes: a U disk, a mobile hard disk, a read-only memory (ROM, Read-Only Memory), a random access memory (RAM, Random Access Memory), a magnetic disk or an optical disk, and various program code storage media.

[0163] In another aspect, the present application also provides a non-transitory computer readable storage medium having stored thereon a computer program, which, when executed by a processor, implements the visual SLAM of fused object plane features method provided by the above method, and the method comprises: acquiring an RGB-D image of a target scene, performing instance segmentation and normal estimation on the RGB-D image respectively to obtain an instance segmentation result and a normal estimation result; extracting static point features of the input image, filtering abnormal point features, performing initial camera pose estimation on the static point features by tracking and minimizing re-projection error, constructing a camera local map to optimize the camera pose and the static map point position; using the instance segmentation result to associate objects in different frames, tracking key points of the associated objects and estimating initial object poses, extracting plane features of dynamic objects based on the normal estimation result, constructing an object local optimization framework fused with plane constraints by using an association strategy based on matching features and plane parameters to optimize the object pose and the dynamic feature parameters; and combining the static processing result and the dynamic processing result to output a target environment map including the camera pose, the static map point, the object pose, the dynamic map point and the dynamic plane.

[0164] The device embodiments described above are merely illustrative, wherein the units described as separate components can or can not be physically separate, and the components displayed as units can or can not be physical units, i.e., can be located in one place, or can be distributed on multiple network units. Part or all of the modules can be selected to achieve the purpose of the embodiment scheme according to actual needs. Those skilled in the art can understand and implement without creative labor.

[0165] From the above description of the embodiments, those skilled in the art can clearly understand that the embodiments can be realized by means of software plus necessary general hardware platforms, and of course can also be realized by hardware. Based on such understanding, the above technical solutions, essentially or in other words, the part that contributes to the prior art, can be embodied in the form of a software product, which can be stored in a computer readable storage medium, such as a ROM / RAM, a magnetic disk, an optical disk, etc., and includes a plurality of instructions to make a computer device (which can be a personal computer, a server, or a network device, etc.) execute the methods described in each embodiment or some parts of the embodiments.

[0166] It should be pointed out finally that the above embodiments are only used to illustrate the technical solutions of the present application, but not to limit the same; and although the present application has been described in detail with reference to the foregoing embodiments, it should be appreciated by those skilled in the art that the technical solutions recorded in the foregoing embodiments can be modified, or some technical features thereof can be replaced equivalently; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. A visual SLAMMOT method for fusing planar features of an object, characterized in that, This includes a visual SLAMMOT system that fuses planar features of objects, specifically comprising: RGB-D camera, Isaac Sim simulation platform and computing devices; An RGB-D camera acquires RGB images and depth information of the target scene; The Isaac Sim simulation platform synthesizes RGB images and depth information to generate RGB-D images; The computing device receives RGB-D images, performs camera pose estimation and static mapping, and object pose estimation and dynamic reconstruction to generate a target scene environment map. The method is based on the system and includes the following specific steps: Acquire the RGB-D image of the target scene, perform instance segmentation and normal estimation on the RGB-D image respectively, and obtain the instance segmentation result and normal estimation result; Extract static point features from the input image, filter out abnormal point features, estimate the initial camera pose by tracking the static point features and minimizing the reprojection error, construct a local camera map to optimize the camera pose and the position of static map points; Data association is performed on objects in different frames using instance segmentation results. Keypoint tracking and initial pose estimation are then performed on the associated objects. Planar features of dynamic objects are extracted based on normal estimation results. A local optimization framework for objects, incorporating planar constraints, is constructed using an association strategy based on matching features and planar parameters to optimize object pose and dynamic feature parameters. This includes: Calculate the intersection-union ratio (IUU) of the object instance segmentation mask in the current frame and the previous frame, construct the association cost matrix, solve the association cost matrix using the KM algorithm, and determine the optimal matching pair. Key points are sampled uniformly according to the size of the object. The key points are tracked between adjacent frames using the Lucas-Kanade optical flow method, and the key points are verified to ensure that the key points are located within the matching object mask. Based on the correspondence between object map points and key points obtained from object data association, an object map point reprojection error term is constructed. : in, For objects Map points The corresponding number Pixel coordinates of frame keypoints The projection matrix of the camera, It is the world coordinate system to the 1st The transformation matrix of the frame camera coordinate system. It is an object In the The transformation matrix from the object coordinate system to the world coordinate system at frame time. It is an object Map points The coordinates in its object coordinate system; Minimize the third objective function to obtain the initial pose estimate of the object. : in, Is with the first Objects associated with key points in a frame A set of map point indices It is a constant covariance matrix; The combined static and dynamic processing results output a target environment map including camera pose, static map points, object pose, dynamic map points, and dynamic planes.

2. The visual SLAMMOT method for fusing planar features of an object according to claim 1, characterized in that, Extract static point features from the input image, filter out outlier features, and estimate the initial camera pose by tracking the static point features and minimizing the reprojection error, including: Extract Shi-Tomasi feature points from the input image, remove feature points on dynamic objects, and retain feature points in the static background; The Lucas-Kanade optical flow method is used to track the extracted feature points frame by frame, and the correspondence between key points is established between consecutive frames; After keypoint tracking is completed, a reprojection error model between static map points and feature points in the current frame is established by matching them with existing static map points. : in, For static map points The corresponding number Pixel coordinates of frame feature points For camera projection model, For the first The camera pose of the frame. For static map points Coordinates in the world coordinate system; By minimizing the first objective function, we obtain the... Frame-based camera initial pose estimation : in, In order to be with the first The set of static map points corresponding to the feature points in the frame. is a constant covariance matrix.

3. The visual SLAMMOT method for fusing planar features of an object according to claim 2, characterized in that, Constructing a local camera map optimizes camera pose and static map point positions, including: A sliding window optimization framework is constructed to locally optimize the initial pose of the camera and the position of static map points. The frames in the sliding window are set to include fixed frames and frames to be optimized. The fixed frames constrain the frames to be optimized, and the frames to be optimized are adjusted in the local optimization. In sliding window optimization, a second objective function is constructed to optimize the camera pose and static map point positions within the sliding window: in, and These represent the set of frame indices and the set of static map point indices in the part of the sliding window to be optimized, respectively. This is the set of all frame indices within the sliding window.

4. The visual SLAMMOT method for fusing planar features of an object according to claim 1, characterized in that, Based on the normal estimation results, planar features of dynamic objects are extracted, including: Randomly select a key point from the set of key points on the object surface as a seed point, and use the normal vector of the seed point... Based on this, the region is gradually expanded by comparing the similarity of the normal vectors. The normal vector of the current point to be expanded is... If the following similarity conditions are met, the seed point will be expanded into a planar region: in, The threshold value for normal vector similarity is in the middle. During the region expansion process, the plane normal vector Continuously update based on newly added regional characteristics to adapt to the expanded planar feature regions: in, For the updated plane normal vector, The plane normal vector before the update. This represents the total number of points on the plane. The process of expanding the region continues until no more key points meet the similarity condition. Once the set of key points and the corresponding plane normal vectors are extracted, the final plane parameters are calculated as follows: in, It is the plane normal vector in the camera coordinate system. These are the coordinates of the center of the point cloud corresponding to the set of planar keypoints in the camera coordinate system. Given the planar distance parameters in the camera coordinate system, we obtain the representation of the planar parameters in the camera coordinate system. ; When the plane is first observed, it is initialized by transforming it from the camera coordinate system to the object coordinate system: in, and These represent the transformation matrices between the camera and the world coordinate system, and between the object and the world coordinate system, respectively.

5. The visual SLAMMOT method for fusing planar features of an object according to claim 4, characterized in that, By employing a correlation strategy based on matching features and planar parameters, a local optimization framework for objects that integrates planar constraints is constructed to optimize object pose and dynamic feature parameters, including: After completing the extraction and association of planar features of the object, an error term from point to plane is constructed. and plane-to-plane error term : in, For objects plane Parameters in the object coordinate system It is an object Map points Coordinates in its object coordinate system For objects plane Parameters in the object coordinate system It is the world coordinate system to the 1st The transformation matrix of the frame camera coordinate system. It is an object In the The transformation matrix from the object coordinate system to the world coordinate system at frame time. For objects plane In the Frame-time observation; Based on the smoothness of the object's motion and the absence of significant abrupt changes in position, an error term is constructed for the constant-velocity motion model of the object. : in, This represents the mapping from the Lie group to the Lie algebra. Representative object In the Frame to the Motion transformation between frames, It is an object In the The transformation matrix from the object coordinate system to the world coordinate system at frame time. It is an object In the The transformation matrix from the object coordinate system to the world coordinate system at frame time; Local object pose, object map points, and object plane optimization are performed in a sliding window to construct the overall objective function for object BA optimization: in, These are the sets of indices for the camera frames, objects, object map points, and object planes in the part of the sliding window to be optimized. These are the covariance matrices corresponding to the object map point reprojection error term, the object constant velocity motion model error term, the point-to-plane error term, and the plane-to-plane error term, respectively.

6. A visual SLAMMOT device for fusing planar features of an object, using the visual SLAMMOT method for fusing planar features of an object as described in any one of claims 1 to 5, characterized in that, include: The acquisition module is used to acquire the RGB-D image of the target scene, and to perform instance segmentation and normal estimation on the RGB-D image to obtain the instance segmentation result and the normal estimation result. The static processing module is used to extract static point features from the input image, filter out abnormal point features, estimate the initial pose of the camera by tracking the static point features and minimizing the reprojection error, and construct a local map of the camera to optimize the camera pose and the position of static map points. The dynamic processing module is used to associate objects in different frames using instance segmentation results, perform key point tracking and initial pose estimation for associated objects, extract planar features of dynamic objects based on normal estimation results, and construct a local optimization framework for objects that integrates planar constraints to optimize object pose and dynamic feature parameters through an association strategy based on matching features and planar parameters. The integration module is used to integrate static and dynamic processing results, and outputs a target environment map including camera pose, static map points, object pose, dynamic map points, and dynamic planes.

7. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the visual SLAMMOT method for fusing planar features of an object as described in any one of claims 1 to 5.

8. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the visual SLAMMOT method for fusing planar features of an object as described in any one of claims 1 to 5.

Images