A multi-sensor fusion method for simultaneous estimation of the pose of a moving object and multiple targets in a metaverse environment

Abstract

The present invention belongs to the field of metaverse object pose estimation technology and robotics, and discloses a multi-sensor fusion simultaneous estimation method for the pose of a moving object itself and multiple targets in a metaverse environment. A multi-probability fusion tracker based on a Kalman filter and optical flow inliers is used to establish more robust object-level data association. On this basis, a dynamic point cloud culling method that fuses the velocity prior of a 3D Kalman filter and camera optical flow decoupling is proposed to improve the accuracy of the odometer's own pose estimation. Finally, multiple constraint factors are combined in our proposed framework to refine the pose of itself and objects. We conducted an evaluation on the KITTI raw dataset, and the experimental results showed that our method achieved better pose accuracy for itself and the tracked targets compared with the baseline and state-of-the-art methods. In addition, the proposed system demonstrates the feasibility of real-time performance to ensure its practical application value.

Description

Technical Field

[0001] The present invention relates to metaverse object pose estimation technology and the field of robotics, and in particular to a method for simultaneously estimating the pose of a moving object itself and multiple targets using multi-sensor fusion in a metaverse environment. Background Art

[0002] Simultaneous localization and mapping (SLAM) is crucial for digitizing physical objects in the metaverse, enabling real-time 3D reconstruction and localization in unknown environments. Multi-sensor fusion approaches have demonstrated excellent performance. While some SLAM systems have achieved high accuracy, most assume a static environment and reject observations of dynamic objects during optimization, making them difficult to apply in real-world, highly dynamic scenarios. Furthermore, some applications, such as autonomous driving and advanced augmented reality (AR), require the perception of surrounding motion to ensure safety and information exchange. Therefore, it is crucial to develop a tightly coupled approach that achieves joint pose estimation of the system itself and multiple targets through sensor fusion.

[0003] Recent research has combined SLAM and multi-object tracking (MOT) to simultaneously estimate the pose of the robot itself and surrounding moving objects, demonstrating impressive performance and demonstrating that dynamic objects are beneficial for estimating its own pose. Furthermore, it is particularly important to simultaneously estimate the poses of surrounding objects while ensuring the accuracy of its own pose estimation. This can help robots handle various goal-oriented tasks such as path planning, human-robot interaction, and behavior prediction. However, most current algorithms use a single measurement sensor. Due to the complex environments of the real world and the increasing need for robots to perceive surrounding objects, it is difficult for the system to achieve accurate and robust estimation of the pose of the self-vehicle and moving objects. Therefore, for the above problems, obtaining more information through multi-sensor fusion is a reliable solution.

[0004] In addition, a large amount of current work focuses on developing accurate 3D detectors to estimate object poses, which largely depends on the performance of the detectors. They only use single-frame information to improve detection accuracy, but ignore historical observation information related to the self and surrounding objects. In order to retain more valuable observation information when tracking objects, object data association plays a vital role. Existing object data association mainly relies on visual features or the intersection of 2D or 3D detection boxes, which cannot completely avoid tracking or optimization failures due to occlusion or detector misdetection caused by rapid object motion. In addition, some studies focus on specific objects or estimate object poses based on model priors, which are often designed for specific scenarios and are not easy to extend to general object pose estimation in the real world. Summary of the Invention

[0005] Based on the above problems, the purpose of the present invention is to propose a multi-sensor fusion simultaneous estimation method for the mobile object's own and multi-target poses in a metaverse environment, so as to realize the joint estimation of the tightly coupled poses of itself and multiple targets, so as to obtain better pose accuracy of itself and the tracked targets.

[0006] The technical solution of the present invention is as follows: a method for simultaneously estimating the pose of a moving object itself and multiple targets using multi-sensor fusion in a metaverse environment, the specific steps of which are as follows:

[0007] Step 1: Acquire data and object perception information: The data includes complete laser point cloud, camera and IMU data; the object perception information is the 2D bounding box and corresponding object mask obtained by instance segmentation of the rigid object in each frame of the image;

[0008] Step 2, feature acquisition: Obtain object feature points by projecting laser points onto the object mask in the camera image, track the feature points using optical flow, and remove outliers caused by tracking accuracy;

[0009] Step 3. Object data association: Construct a multi-probability tracker based on the fusion of optical flow tracking and 2D IOU. Specifically, first perform optical flow tracking based on the correct object feature points to obtain the object feature matching points in the current frame, and calculate the proportion of its feature matching points falling within the 2D bounding box detected in the current frame; then predict the position of the 2D bounding box of the object in the previous frame in the current frame based on constant speed motion, and calculate the 2D IOU with the 2D bounding box of the object detected in the current frame; finally, construct a weighted association matrix through the cost function of the fusion of optical flow tracking and 2D IOU, so as to achieve object matching between adjacent frames;

[0010] Step 4: Dynamic point removal: By decoupling the 2D optical flow of the object's feature points and combining it with the motion state prior in the 3D Kalman filter, a method for estimating the object's motion state is designed to accurately remove dynamic points; the motion state includes velocity and angular velocity;

[0011] Step 5: Self-pose estimation: Self-pose estimation is achieved through the lidar inertial odometry in a tightly coupled iterative error state Kalman filter framework. Taking the laser point cloud and IMU data as input, the forward propagation of the IMU is used to provide state prediction and the laser point cloud is motion compensated based on the backward propagation. The point cloud in the local map is organized and maintained by a kd-tree data structure that supports dynamic addition and deletion and self-balancing. Before the odometry is iteratively updated, the dynamic point culling method in step 4 is used to remove the laser point cloud of dynamic objects, thereby reducing the impact of the dynamic point cloud on the self-odometry.

[0012] Step 6: Joint optimization of the factor graph: To obtain the object's initial pose, a 3D Kalman filter is constructed based on velocity prediction of the object's state. The object's velocity is continuously updated using the object motion state estimation method from step 4 and the motion state prior of the 3D Kalman filter to ensure the accuracy of the object's velocity. In addition, the object's feature matching points obtained by optical flow tracking are used to obtain the object's state prediction through PNP, and the method with the largest number of inliers is retained as the object's initial pose. The object's initial pose and the object's own pose obtained in step 5 are then fed into a tightly coupled factor graph for joint optimization to further refine the state of the object and the tracked object.

[0013] Step 7: Map data publishing: Through the external parameter transformation from the lidar to the camera and accurate object and self-pose estimation, the laser point cloud is projected into the image to obtain accurate color information of the point cloud, and a separate thread is used to maintain a globally consistent multi-target trajectory and RGB color point cloud map in real time.

[0014] Furthermore, the step 2 is specifically as follows: through the external parameter transformation from the laser radar to the camera, the laser point cloud is projected onto the object mask of the object perception information to obtain the object feature points, and only the points with the smallest depth of each pixel are retained; then, based on geometric clustering, the object feature points with a depth difference exceeding the set value are removed; finally, in the cluster with the possibility of object feature points, the potential object feature points are found for the current object feature point using the nearest neighbor fast iteration, so as to obtain the correct object feature points; for the obtained object feature points, the optical flow is used to track the features of the object in the next frame, and the laser point cloud projection is continuously updated to ensure a sufficient number of feature points.

[0015] Furthermore, the multi-probability tracker based on the fusion of optical flow tracking and 2D IOU includes a fusion strategy of the object's 2DIOU and the number of inliers of the object's feature points tracked by optical flow and different association period strategies;

[0016] The fusion strategy of the object 2D IOU and the number of inliers of the object feature points tracked by optical flow is:

[0017] A 2D Kalman filter is used to maintain only the 2D bounding box obtained by instance segmentation; the state vector of the 2D bounding box of the i-th object is defined as follows:

[0018]

[0019] Among them, x c and y c is the image coordinate of the upper left corner of the 2D bounding box; v x and v y is the velocity of the 2D bounding box; a and h are the area and height of the 2D bounding box respectively; and are the derivatives of a and h respectively;

[0020] For an object moving at a constant speed, the predicted position of the 2D bounding box is obtained through the state transfer matrix F(k):

[0021]

[0022] Among them, the state prediction process contains prediction noise n(i)~N(0,Q(i)) that conforms to the normal distribution and is independent. and are the 2D bounding boxes of the i-th object at t k The state prediction at time t k-1 The posterior estimate of the moment; is the prior covariance of the state prediction; is in t k-1 The posterior estimate of the state covariance of the 2D bounding box at time t;

[0023] Constructing a 2D IOU association matrix based on the predicted position of the 2D bounding box and the detected position of the current frame;

[0024] The object data association takes occlusion into account and adds an additional weight for the number of inliers in the 2D IOU association matrix based on optical flow tracking. The position of the object feature points projected from the laser point cloud in the current frame is predicted using optical flow to obtain object feature matching points. The number of object feature matching points falling within the 2D bounding box of the object detected in the current frame is calculated. The overall cost function is as follows:

[0025]

[0026] in, and are the 2D bounding boxes of the i-th object detected in the current frame and the previous frame respectively, and p k-1,i The feature points of the i-th object are detected for the current frame and the previous frame respectively; N(·) represents the number of object feature points calculated within the object's 2D bounding box; Represents the calculation of the predicted position of the 2D bounding box or object feature points in the current camera frame;

[0027] Object matching between adjacent frames is achieved by solving the cost function and performing Hungarian algorithm matching.

[0028] Furthermore, the different association cycle strategies for the object data association are:

[0029] For successfully matched objects, the object inherits its successfully matched trajectory ID and proceeds to the next optimization step. In addition, to address inconsistent object information due to occlusion, false detection, or loss, different association cycles are used to maintain and manage the object trajectory ID. If the object has never been tracked before, that is, the first time the object is detected, a new ID is assigned to it. If there are no successful matches for more than α frames in a row, the trajectory ID is deleted and no longer matched with newly detected objects. If it has been tracked for a period of time and is not successfully matched by any newly detected object in the λ frame, it is deleted.

[0030] Furthermore, the dynamic point elimination is specifically as follows:

[0031] Denote the IMU, camera, laser, object and global reference frame as {I}, {C}, {L}, {O}, {W} respectively; the reference frame {A} at time t k The posture at this time is expressed as A k Indicates that the pose in frame {B} is represented as The reference frame {A} is from the previous time t k-1 To the current time t k The posture transformation is recorded as Put the i-th object in the world system t k The position and velocity at the moment are recorded as From the previous moment t k-1 To the current time t k The relative posture and velocity are The object pose transformation relationship is as follows:

[0032]

[0033] For k-1 The homogeneous coordinates of the jth point on the i-th object in the object system at the moment are in Corresponding to the positions on the x, y, and z axes respectively; due to the t k The point at the moment and at the previous moment t k-1 The next point Equal, through the object's transformation matrix under the camera Pose transformation matrix between camera frames And the camera's position in the world system Transform the object point to the tth k At this moment:

[0034]

[0035] Given a pair of object feature matching points based on optical flow tracking, find an optimal motion state estimate Minimize the distance error:

[0036]

[0037] Among them, d(·) is the moving distance of the object feature point on the image, p′ is the position of the object feature point tracked by the optical flow field in the current image, ||·|| γ is the Huber norm, (·)′ is the estimated motion distance of the object feature point, which is defined as:

[0038]

[0039] Among them, p u and p v is the position of the object feature point in the image, f x ,f y ,c x ,c y is the camera internal parameter, X, Y, Z are the 3D coordinates of the object feature point in the camera system;

[0040] Since the optical flow field is combined with the self and object motion, the 3D points of the laser system are transformed into the object system according to the rigidity assumption, and the least squares problem is solved by ceres optimization to decouple the motion state of the object; for the tracked object, the object's motion state prior [V, ω] is obtained from the 3D Kalman filter. KF and covariance matrix ∑ KF , fusion of the motion state [V, ω] and covariance ∑ decoupled from the optical flow, the updated speed [V, ω] update Expressed as:

[0041]

[0042] Before the odometry is iteratively updated, the laser point cloud of dynamic objects is removed based on the motion state estimation to reduce the impact on the own odometry.

[0043] Furthermore, the motion state prior is object prediction based on a 3D Kalman filter;

[0044] The 3D Kalman filter predicts the state of the object in the current frame and updates the 3D bounding box of the object; for each object point in each frame, the 3D bounding box is continuously refined in the 3D Kalman filter to ensure that it is within the state vector of the object. Defined as:

[0045]

[0046] in are the coordinates of the i-th object in the world system and the angles of rotation around the three axes, is the length, width and height of the object, and are the velocity and angular velocity of the object respectively;

[0047] In the 3D Kalman filter, the object state is predicted based on the solved velocity, and the initial state of the object is calculated using the PNP of the optical flow feature matching points, and the final initial state of the object is retained in the way with the largest number of points within the object.

[0048] Furthermore, the factor graph joint optimization is a joint estimation method based on the object and its own state;

[0049] For continuously tracked objects, a sliding window is used to ensure the consistency of the object's local state and the real-time performance of the system, enabling batch optimization of the object's continuous tracking frames. A tightly coupled joint estimation is achieved through a factor graph optimization framework that integrates the state nodes of the object and surrounding objects and the constraint edges provided by various factors. Based on the self-position obtained by the laser inertial odometry and the initial state of the dynamic object, the odometry factor, object observation factor, image vision factor, object smoothing factor, object velocity factor, and prior factors are introduced into the factor graph optimization framework for joint optimization to constrain the self-position and object pose.

[0050] The odometry factor constrains the robot's own posture, the object observation factor constrains the relative transformation between the robot and the object's posture, the image vision factor constrains the object's posture, the object smoothness factor constrains the object's posture and velocity between two frames, and the object velocity factor constrains the object's velocity consistency between two frames. The state of the oldest frame is marginalized and used to constrain the first posture of the batch processing optimization framework in the sliding window.

[0051] Knowing that the self is in the world system at time t k Posture and the previous frame time t k-1 The odometer factor is defined as follows:

[0052]

[0053] where Log(·) is a mapping from a Lie group in SE(3) to its Lie algebra in se(3);

[0054] After obtaining the initial estimate of the i-th object in the laser coordinate system The object at the current t k The object observation factor at the moment is recorded as:

[0055]

[0056] For the visual features of dynamic objects, the dynamic point of the object in the world system is transformed to the static point of the object system through the rigid body assumption and projected onto the image; then the jth dynamic feature of the i-th object at time t k The image visual factor on is expressed as:

[0057]

[0058] Among them, π(·) is the camera projection model, Projection point The actual pixel location on the image plane; is the camera time t k Position in the world system;

[0059] The moving object is modeled using a constant linear velocity and angular velocity model, and the smoothing factor is defined as:

[0060]

[0061] in It consists of the linear velocity and angular velocity of the object. Expressed as the timestamp difference between two frames; Exp(·) is the inverse operation of Log(·);

[0062] Assuming that the dynamic object moves at a constant speed in a short period of time, the constant speed factor for continuously tracking the object is defined as:

[0063]

[0064] The final optimization problem is defined as Equation (15), which obtains the best estimates of the ego vehicle and the tracked object by minimizing the sum of the prior norm and the Mahalanobis distance norm of all measurement residuals;

[0065]

[0066] Among them, c is the set of all variables, γ prior (c) is the marginalized prior residual term, θ(·) is the robust Huber kernel function, ∑ O ,∑ C ,∑ P ,∑ S ,∑ V are the covariance matrices of the corresponding factors.

[0067] The beneficial effects of the present invention are as follows: The limitations of self-localization and state estimation of surrounding objects in dynamic environments such as metaverse scenarios are addressed: Most algorithms are based on the assumption of a static environment and ignore the mutual benefit between dynamic objects and the self, which greatly limits their application in actual high-dynamic scenarios. To overcome these limitations, we propose a tightly coupled system that achieves joint estimation of self-position and multi-target pose through sensor fusion. By formulating a multi-probabilistic fusion tracker to perform data association and using the rigidity assumption to transform the laser point cloud into the object coordinate system to achieve state decoupling, dynamic points are eliminated to improve odometry accuracy. In addition, a compact framework is developed to fuse laser, camera, and IMU data, and multiple constraints are combined in a sliding window to jointly optimize the self-position and multi-target pose. Experimental results show that compared with the state-of-the-art dynamic SLAM technology, our proposed method achieves the best accuracy, indicating that the simultaneous estimation of self-position and multi-target objects is mutually beneficial. In addition, our algorithm can be applied to more types of lidars in a wide range of applications while ensuring computational efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 The figure shows a flowchart of a method for simultaneous multi-sensor fusion estimation of the position and posture of a moving object itself and multiple targets in a metaverse environment. DETAILED DESCRIPTION

[0069] Figure 1 It is the main flow chart of the technical solution of the present invention.

[0070] A method for simultaneous multi-sensor fusion estimation of the pose of a moving object and multiple targets in a metaverse environment is proposed. The specific steps are as follows:

[0071] Step 1: Data and object perception information acquisition: In an autonomous driving scenario, we recorded an algorithm evaluation dataset using a car as the carrier. This dataset contains the true values ​​of the camera pose and sensor extrinsic parameters, as well as some manual annotations of object poses. We used the pre-trained mask-rcnn model to perform instance segmentation on the image and obtain the vehicle mask and 2D bounding box information.

[0072] Furthermore, we conduct tests on the original KITTI dataset, which contains the true values ​​of camera poses and sensor extrinsics, as well as manual annotations of some object poses, which effectively verifies the performance of our proposed method.

[0073] Step 2, feature acquisition: Obtain the optical flow features of the object by projecting laser points onto the image. Due to differences in detection accuracy and viewing angle, the edge information of the object is unreliable, which may result in the loss of some information or cause depth errors in the projected point cloud. Therefore, three simple and effective methods are used to obtain the correct object points, and optical flow tracking and laser point cloud projection are used to continuously update the object feature points to avoid additional feature extraction or depth recovery. In addition, some LiDAR point clouds may be occluded and invisible to the camera. If all Lidar point clouds are simply projected onto the object for tracking, it is likely to cause incorrect calculations and bring additional errors, thereby affecting the optimization results. Therefore, first, through the LiDAR-camera transformation, we project the LiDAR points into the image and only retain the points with the smallest distance to each pixel to obtain the camera-visible points; then, based on geometric clustering, object points with inconsistent depths are quickly removed; finally, in the cluster of possible object points, the nearest neighbor is used for rapid iteration to find object points that may be partially lost, so as to obtain the correct object points;

[0074] Step 3: Object Data Association: To ensure the performance and accuracy of object association, a multi-probabilistic tracker based on the fusion of optical flow tracking and 2D IOU is designed. The association matrix is ​​constructed using the object's 2D bounding box obtained in step 1 and the object's optical flow inliers obtained in step 2 to improve the robustness and accuracy of object association and tracking.

[0075] Step 4: Dynamic point removal: Autonomous driving scenes contain a large number of dynamic points. To reduce the impact on the odometer, a method is designed to accurately estimate the current speed of objects and remove dynamic points by decoupling the 2D optical flow of object features and combining it with speed priors.

[0076] Step 5: Estimation of own pose: Estimation of own pose is achieved through the lidar inertial odometry in a tightly coupled iterative error state Kalman filter framework;

[0077] Step 6, Factor Graph Joint Optimization: To refine the state of itself and the tracked object, we feed them together into a tightly coupled factor graph for joint optimization to obtain accurate positioning.

[0078] Step 7: Map Data Publishing: To ensure efficient system operation, we created a new thread for map publishing to facilitate parallel processing. By removing invisible points from different viewpoints, using lidar-to-camera extrinsic parameter transformation and accurate pose estimation, we can obtain accurate color information from the point cloud image. A separate thread maintains a globally consistent multi-target trajectory and RGB color point cloud map in real time. For each frame of tracked objects, we continuously update the map and distinguish between dynamic and static data to publish the color point cloud of the object tracked in the last frame or for all frames, along with the complete motion trajectory.

[0079] The object features are acquired by optical flow tracking based on laser point projection;

[0080] For single-frame LiDAR and camera data, we perform instance segmentation on each frame to obtain object semantics and 2D bounding box information. We then project laser points onto the image to obtain object features based on the object perception results, avoiding additional feature extraction or depth recovery. To reduce errors caused by detection accuracy or viewpoint differences, we use three simple and effective methods to obtain the correct object points: ① Through LiDAR-camera transformation, we project LiDAR points onto the image, retaining only points with the smallest per-pixel distance to obtain camera-visible points. ② Based on geometric clustering, we quickly remove object points with inconsistent depth. ③ Within clusters of possible object points, we use nearest neighbor iteration to quickly find object points that may have been partially lost. For each acquired object point, we use optical flow to track the object's features in the next frame. We perform forward and backward optical flow algorithms to retain more accurate feature matching points, while continuously updating with laser point projection to ensure a sufficient number of feature points.

[0081] The object data association is achieved by a multi-probability tracker based on optical flow tracking and 2D-Iou fusion;

[0082] To ensure the performance and accuracy of object association, a multi-probabilistic tracker is designed. First, a 2D Kalman filter is used to maintain only the 2D bounding boxes of objects obtained through instance segmentation. The state vector of the i-th object bounding box is defined as follows:

[0083]

[0084] where x c and y c is the image coordinate of the upper left corner of the bounding box, v x and v y is the velocity of the bounding box, a is the area of ​​the bounding box, h is the height of the bounding box, and are the derivatives of a and h respectively

[0085] We assume that the object moves at a constant speed and calculate the predicted position of the object's 2D bounding box through the state transfer matrix F(k) and the prediction noise n(i)~N(0,Q(i)) assumed to be normally distributed and independent. The motion transfer equation is:

[0086]

[0087] The state prediction of the 2D bounding box of an object can be achieved as follows:

[0088]

[0089] in, is the i-th object at time t k The state prediction of the lower bounding box, is at time t k-1 The posterior estimate of is the covariance of the state predictions, is time t k-1 The posterior estimate of the bounding box state in

[15] .

[0090] The predicted 2D bounding box and the currently detected 2D bounding box are used to construct an association matrix, and Hungarian algorithm matching is performed to achieve object association tracking. In addition, to cope with associations in more complex scenes, such as when occlusion occurs, we also add an additional metric to the association matrix based on the Lucas-Kanade (LK) optical flow field prediction: the number of inliers. The projected laser points are matched with optical flow feature point pairs to predict their positions in the current frame and their number in the detected object 2D bounding box is calculated. Therefore, the overall cost function is as follows:

[0091]

[0092] in, and are the 2D bounding boxes of the i-th object detected in the current frame and the previous frame respectively, and p k-1,i The feature points of the i-th object are detected for the current frame and the previous frame respectively; N(·) represents the number of object feature points calculated within the object's 2D bounding box; Represents the calculation of the predicted position of the 2D bounding box or object feature points in the current camera frame;

[0093] By solving the cost function, objects can be matched between adjacent frames. If a match is successfully obtained, the object will inherit the track ID of the successfully matched object and proceed to the next optimization step. In addition, to address inconsistent object information due to occlusion, false detection, or loss, we use different association cycles. If the object has never been tracked before, we assign it a new ID and delete it when it has not been successfully matched for more than α consecutive frames; if it has been tracked for a period of time and has not been successfully matched by any new detection in the λ frame, it will be deleted. This allows us to have a certain tolerance for the detection module, thereby ensuring continuous object tracking in the system.

[0094] The dynamic point culling is based on the decoupling of the optical flow of the object point and the estimation method combined with the velocity prior;

[0095] We denote the IMU, camera, laser, object, and global reference frame as {I}, {C}, {L}, {O}, {W} respectively; the reference frame {A} is at time t k The posture at this time is expressed as A k Indicates that the pose in frame {B} is represented as The reference frame {A} is from the previous time t k-1 To the current time t k The posture transformation is recorded as Put the i-th object in the world system t k The position and velocity at the moment are recorded as From the previous moment t k-1 To the current time t k The relative posture and velocity are The object pose transformation relationship is as follows:

[0096]

[0097] For k-1 The homogeneous coordinates of the jth point on the i-th object in the object system at the moment are in Corresponding to the positions on the x, y, and z axes respectively; due to the t k The point at the moment and at the previous moment t k-1 The next point Equal, through the object's transformation matrix under the camera Pose transformation matrix between camera frames And the camera's position in the world system Transform the object point to the tth k At this moment:

[0098]

[0099] Perform forward and backward optical flow algorithms to retain more accurate feature matching points. Given a pair of tracked 2D optical flow points, we find an optimal estimate Minimize the distance error:

[0100]

[0101] Where d(·) is the optical flow distance that the object point moves on the image, p′ is the position tracked by the optical flow field in the current image, ||·|| γ is the Huber norm, The estimated distance of the object can be defined as:

[0102]

[0103] Among them, p u and p v is the position of the object feature point in the image, f x ,f y ,c x ,c y is the camera internal parameter, X, Y, Z are the 3D coordinates of the object feature point in the camera system;

[0104] Since the optical flow field is combined with the self and object motion, we transform the 3D points of the laser system into the object system based on the rigidity assumption, use ceres optimization to solve this least squares problem, and decouple the object's motion state. For the tracked object, we can also obtain the object's velocity state prior [V,ω] from the 3D Kalman filter. KF and covariance matrix ∑ KF , the speed and covariance ∑ decoupled from the fusion optical flow estimation, the updated speed [V,ω] update Expressed as:

[0105]

[0106] Before the odometry is iteratively updated, laser points of dynamic objects are removed based on the motion state estimation.

[0107] The velocity prior is an object prediction method based on a 3D Kalman filter;

[0108] In order to ensure the optimization effect of the object, a 3D Kalman filter is used to predict the state of the object in the current frame and update the 3D bounding box of the object. For each object point in each frame, the 3D bounding box is continuously refined in the 3D Kalman filter to ensure that it is within the bounding box. We take the state vector of the object as Defined as:

[0109]

[0110] in, is the angle of rotation of the i-th object around the three axes, is the length, width and height of the object, and are the velocity and angular velocity of the object respectively.

[0111] Unlike the 2D Kalman filter, in the 3D Kalman filter, we predict the object state based on the solved velocity and calculate the object state using the PNP of the optical flow feature matching points. Ultimately, we retain the method with the largest number of inliers as the initial result of the sliding window optimization.

[0112] The factor graph joint optimization is a joint estimation method based on the object and its own state;

[0113] For continuously tracked objects, we use a sliding window technique to ensure local state consistency and system real-time performance, enabling batch optimization of continuously tracked frames. A tightly coupled joint estimation is achieved through a factor graph optimization framework that integrates the state nodes of the object and surrounding objects with the constraint edges provided by various factors. We first obtain laser odometry results and estimate the initial pose of the dynamic object. We then introduce an odometry factor, object observation factor, vision factor, object smoothing factor, object velocity factor, and a priori factors into the sliding window for joint optimization, effectively constraining the pose of the object and the object.

[0114] The odometry factor constrains the robot's own pose, the object observation factor constrains the relative transformation between the robot and the object's pose, the vision factor constrains the object's pose, the object smoothness factor constrains the object's pose and velocity between two frames, and the object velocity factor constrains the object's velocity consistency between two frames. To maintain the consistency of the sliding window and enable fast system execution, we choose to marginalize the state of the oldest frame instead of adding a Hessian block of dense landmarks. This is also used to constrain the first pose in the batch optimization framework in the sliding window.

[0115] Knowing its own posture at time t k and the previous frame time t k-1 The odometer factor is defined as follows:

[0116]

[0117] where Log(·) is a mapping from a Lie group in SE(3) to its Lie algebra in se(3);

[0118] After obtaining the initial estimate of the object, the observation factor of the object at the current moment is recorded as:

[0119]

[0120] For the visual features of dynamic objects, we transform the dynamic points of the object in the world system to the static points of the object system through the rigid body assumption and project them onto the image. Then the jth dynamic feature of the i-th object at time t k The image visual factor on can be expressed as:

[0121]

[0122] where π(·) is the camera projection model, Projection point The actual pixel location on the image plane.

[0123] We use the constant linear velocity and angular velocity model (CLAV) to model the moving object, and the smoothing factor is defined as:

[0124]

[0125] in It consists of the linear velocity and angular velocity of the object. Expressed as the timestamp difference between two frames; Exp(·) is the inverse operation of Log(·).

[0126] In order to ensure the continuity of the tracked object trajectory, it is assumed that the dynamic object moves at a constant speed in a short period of time. The constant speed factor for the continuously tracked object is defined as:

[0127]

[0128] The final optimization problem of the system is defined as Equation (16), which obtains the best estimate of the ego vehicle and the tracked object by minimizing the sum of the prior norm and the Mahalanobis distance norm of all measurement residuals.

[0129]

[0130] Where c is the set of all variables, γ prior (c) is the marginalized prior residual term, θ(·) is the robust Huber kernel function, ∑ O ,∑ C ,∑ P ,∑ S ,∑ V are the covariance matrices of the corresponding factors. The dynamic object set O in each frame k,i and the observed object points It is closely related to its own posture, ensuring the robustness of the system in severe dynamic scenes and the satisfactory accuracy of its own and object trajectories.

[0131] Aiming at the simultaneous estimation of the pose of the self and multiple targets in a dynamic environment, a method for self pose estimation and dynamic object perception through multi-sensor fusion is proposed. The method has three significant advantages: (1) A multi-probabilistic tracker is used to associate optical flow tracking with 2D-Iou fusion, in which the tracking points and 3D bounding boxes are continuously updated, thereby improving the robustness and accuracy of object association and tracking. (2) The velocity prior of the 3D Kalman filter and the camera optical flow can be combined to decouple the object velocity, thereby eliminating dynamic points and improving the accuracy of self pose estimation. (3) In a compact multi-target tracking framework of lidar-camera-inertial fusion, the state optimization factor graph under tightly coupled multi-constraints is constructed in a sliding window to enhance robustness and local consistency, thereby achieving joint estimation of the pose of the self and the object. The specific operation is as follows: First, the object mask and 2D bounding box information are obtained by instance segmentation, and the object features are obtained by projecting the laser points onto the image mask. In view of the uncertainty of object perception information, three simple and effective methods are used to obtain the correct object points, and then sufficient object feature points are continuously maintained by continuously using optical flow tracking and laser point cloud projection.

[0132] For object data association and optimization, a multi-probabilistic tracker was designed. This tracker constructs a weighted association matrix using the IOU of the 2D bounding boxes predicted and detected by the uniform velocity model and the number of inliers based on the optical flow field. Hungarian matching is then performed to achieve fast and stable object association between adjacent frames. After obtaining the initial states of the object and itself, we feed them into a unified factor graph optimization framework for multi-constrained joint estimation. To ensure efficient system operation, we created a new thread for map publishing to parallelize processing. By removing invisible points from different viewpoints, we use lidar-to-camera extrinsic parameter transformation and accurate pose estimation to obtain accurate color information from the point cloud image, thereby reconstructing a globally accurate and consistent RGB color point cloud map. For tracked objects, we distinguish between dynamic and static objects and publish the object's color point cloud for the last frame or all frames, along with the complete motion trajectory.

Claims

1. A method for simultaneous multi-sensor fusion estimation of the pose of a moving object and multiple targets in a metaverse environment, characterized by: The specific steps are as follows: Step 1: Acquire data and object perception information: The data includes complete laser point cloud, camera and IMU data; the object perception information is the 2D bounding box and corresponding object mask obtained by instance segmentation of the rigid object in each frame of the image; Step 2, feature acquisition: Obtain object feature points by projecting laser points onto the object mask in the camera image, track the feature points using optical flow, and remove outliers caused by tracking accuracy; Step 3. Object Data Association: Construct a multi-probability tracker based on the fusion of optical flow tracking and 2D IOU. Specifically, first perform optical flow tracking based on the correct object feature points to obtain the object feature matching points in the current frame, and calculate the proportion of its feature matching points falling within the 2D bounding box detected in the current frame; then predict the position of the 2D bounding box of the object in the previous frame in the current frame based on constant speed motion, and calculate the 2D IOU with the 2D bounding box of the object detected in the current frame; finally, construct a weighted association matrix through the cost function of the fusion of optical flow tracking and 2D IOU, so as to achieve object matching between adjacent frames; Step 4: Dynamic point removal: By decoupling the 2D optical flow of the object's feature points and combining it with the motion state prior in the 3D Kalman filter, a method for estimating the object's motion state is designed to accurately remove dynamic points; the motion state includes velocity and angular velocity; Step 5: Self-pose estimation: Self-pose estimation is achieved through the lidar inertial odometry in a tightly coupled iterative error state Kalman filter framework. Taking the laser point cloud and IMU data as input, the forward propagation of the IMU is used to provide state prediction and the laser point cloud is motion compensated based on the backward propagation. The point cloud in the local map is organized and maintained by a kd-tree data structure that supports dynamic addition and deletion and self-balancing. Before the odometry is iteratively updated, the dynamic point culling method in step 4 is used to remove the laser point cloud of dynamic objects, thereby reducing the impact of the dynamic point cloud on the self-odometry. Step 6: Factor graph joint optimization: To obtain the object's initial pose, a 3D Kalman filter is constructed to predict the object's state based on velocity. The object's velocity is continuously updated using the object motion state estimation method from step 4 and the 3D Kalman filter's motion state prior to ensure the accuracy of the object's velocity. Furthermore, the object's feature matching points obtained by optical flow tracking are used to obtain the object's state prediction through PNP. The method with the largest number of inliers is retained as the object's initial pose. Then the initial pose of the object and the self-pose obtained in step 5 are fed into the tightly coupled factor graph for joint optimization to further refine the state of itself and the tracked object; Step 7: Map data publishing: Through the external parameter transformation from the lidar to the camera and accurate object and self-pose estimation, the laser point cloud is projected into the image to obtain accurate color information of the point cloud, and a separate thread is used to maintain a globally consistent multi-target trajectory and RGB color point cloud map in real time.

2. The method for simultaneous estimation of the position of a moving object and multiple targets by multi-sensor fusion according to claim 1, characterized in that: The step 2 specifically comprises: projecting the laser point cloud onto an object mask of the object perception information through the laser radar to camera extrinsic parameter transformation to obtain object feature points, and retaining only the points with the minimum depth per pixel; then removing object feature points whose depth difference exceeds a set value based on geometric clustering; Finally, in the cluster with possible object feature points, the nearest neighbor is used for the current object feature point to quickly iterate and find potential object feature points, so as to obtain the correct object feature points; for the acquired object feature points, optical flow is used to track the features of the object in the next frame, and laser point cloud projection is continuously used to update to ensure a sufficient number of feature points.

3. The method for simultaneous estimation of the position and posture of a moving object and multiple targets by multi-sensor fusion according to claim 2, characterized in that: The multi-probability tracker based on the fusion of optical flow tracking and 2D IOU includes a fusion strategy of the object's 2D IOU and the number of inliers of the object's feature points tracked by optical flow and different association cycle strategies; The fusion strategy of the object 2D IOU and the number of inliers of the object feature points tracked by optical flow is: A 2D Kalman filter is used to maintain only the 2D bounding box obtained by instance segmentation; the state vector of the 2D bounding box of the i-th object is defined as follows: Among them, x c and y c is the image coordinate of the upper left corner of the 2D bounding box; v x and v y is the velocity of the 2D bounding box; a and h are the area and height of the 2D bounding box respectively; and are the derivatives of a and h respectively; For an object moving at a constant speed, the predicted position of the 2D bounding box is obtained through the state transfer matrix F(k): Among them, the state prediction process contains prediction noise n(i)~N(0,Q(i)) that conforms to the normal distribution and is independent. and are the 2D bounding boxes of the i-th object at t k The state prediction at time t k-1 The posterior estimate of the moment; is the prior covariance of the state prediction; is in t k-1 The posterior estimate of the state covariance of the 2D bounding box at time t; Constructing a 2D IOU association matrix based on the predicted position of the 2D bounding box and the detected position of the current frame; The object data association takes occlusion into account and adds an additional weight for the number of inliers in the 2D IOU association matrix based on optical flow tracking. The position of the object feature points projected from the laser point cloud in the current frame is predicted using optical flow to obtain object feature matching points. The number of object feature matching points falling within the 2D bounding box of the object detected in the current frame is calculated. The overall cost function is as follows: in, and are the 2D bounding boxes of the i-th object detected in the current frame and the previous frame respectively, and p k-1,i The feature points of the i-th object are detected for the current frame and the previous frame respectively; N(·) represents the number of object feature points calculated within the object's 2D bounding box; Represents the calculation of the predicted position of the 2D bounding box or object feature points in the current camera frame; Object matching between adjacent frames is achieved by solving the cost function and performing Hungarian algorithm matching.

4. The method for simultaneous estimation of the position and posture of a moving object and multiple targets by multi-sensor fusion according to claim 3, characterized in that: The different association cycle strategies for object data association are: For successfully matched objects, the object inherits its successfully matched trajectory ID and proceeds to the next optimization step. In addition, to address inconsistent object information due to occlusion, false detection, or loss, different association cycles are used to maintain and manage the object trajectory ID. If the object has never been tracked before, that is, the first time the object is detected, a new ID is assigned to it. If there are no successful matches for more than α frames in a row, the trajectory ID is deleted and no longer matched with newly detected objects. If it has been tracked for a period of time and is not successfully matched by any newly detected object in the λ frame, it is deleted.

5. The method for simultaneous estimation of the position and posture of a moving object and multiple targets by multi-sensor fusion according to claim 4, characterized in that: The dynamic point removal is specifically as follows: Denote the IMU, camera, laser, object and global reference frame as {I}, {C}, {L}, {O}, {W} respectively; the reference frame {A} at time t k The posture at this time is expressed as A k Indicates that the pose in frame {B} is represented as The reference frame {A} is from the previous time t k-1 To the current time t k The posture transformation is recorded as Put the i-th object in the world system t k The position and velocity at the moment are recorded as From the previous moment t k-1 To the current time t k The relative posture and velocity are The object pose transformation relationship is as follows: For k-1 The homogeneous coordinates of the jth point on the i-th object in the object system at the moment are in Corresponding to the positions on the x, y, and z axes respectively; due to the t k The point at the moment and at the previous moment t k-1 The next point Equal, through the object's transformation matrix under the camera Pose transformation matrix between camera frames And the camera's position in the world system Transform the object point to the tth k At this moment: Given a pair of object feature matching points based on optical flow tracking, find an optimal motion state estimate Minimize the distance error: Among them, d(·) is the moving distance of the object feature point on the image, p′ is the position of the object feature point tracked by the optical flow field in the current image, ||·|| g is the Huber norm, (·)′ is the estimated motion distance of the object feature point, which is defined as: Among them, p u and p v is the position of the object feature point in the image, f x ,f y ,c x ,c y is the camera internal parameter, X, Y, Z are the 3D coordinates of the object feature point in the camera system; Since the optical flow field is combined with the self and object motion, the 3D points of the laser system are transformed into the object system according to the rigidity assumption, and the least squares problem is solved by ceres optimization to decouple the motion state of the object; for the tracked object, the object's motion state prior [V, ω] is obtained from the 3D Kalman filter. KF and covariance matrix ∑ KF , fusion of the motion state [V, ω] and covariance ∑ decoupled from the optical flow, the updated speed [V, ω] update Expressed as: Before the odometry is iteratively updated, the laser point cloud of dynamic objects is removed based on the motion state estimation to reduce the impact on the odometry itself.

6. The method for simultaneous estimation of the position and posture of a moving object and multiple targets by multi-sensor fusion according to claim 5, characterized in that: The motion state prior is object prediction based on a 3D Kalman filter; The 3D Kalman filter predicts the state of the object in the current frame and updates the 3D bounding box of the object; for each object point in each frame, the 3D bounding box is continuously refined in the 3D Kalman filter to ensure that it is within the state vector of the object. Defined as: in are the coordinates of the i-th object in the world system and the angles of rotation around the three axes, is the length, width and height of the object, and are the velocity and angular velocity of the object respectively; In the 3D Kalman filter, the object state is predicted based on the solved velocity, and the initial state of the object is calculated using the PNP of the optical flow feature matching points, and the final initial state of the object is retained in the way with the largest number of points within the object.

7. The method for simultaneous estimation of the position and posture of a moving object and multiple targets by multi-sensor fusion according to claim 6, characterized in that: The factor graph joint optimization is a joint estimation method based on the object and its own state; For continuously tracked objects, a sliding window is used to ensure the consistency of the object's local state and the real-time performance of the system, enabling batch optimization of the object's continuous tracking frames. A tightly coupled joint estimation is achieved through a factor graph optimization framework that integrates the state nodes of the object and surrounding objects and the constraint edges provided by various factors. Based on the self-position obtained by the laser inertial odometry and the initial state of the dynamic object, the odometry factor, object observation factor, image vision factor, object smoothing factor, object velocity factor, and prior factors are introduced into the factor graph optimization framework for joint optimization to constrain the self-position and object pose. The odometry factor constrains the robot's own posture, the object observation factor constrains the relative transformation between the robot and the object's posture, the image vision factor constrains the object's posture, the object smoothness factor constrains the object's posture and velocity between two frames, and the object velocity factor constrains the object's velocity consistency between two frames. The state of the oldest frame is marginalized and used to constrain the first posture of the batch processing optimization framework in the sliding window. Knowing that the self is in the world system at time t k Posture and the previous frame time t k-1 The odometer factor is defined as follows: where Log(·) is a mapping from a Lie group in SE(3) to its Lie algebra in se(3); After obtaining the initial estimate of the i-th object in the laser coordinate system The object at the current t k The object observation factor at the moment is recorded as: For the visual features of dynamic objects, the dynamic point of the object in the world system is transformed to the static point of the object system through the rigid body assumption and projected onto the image; then the jth dynamic feature of the i-th object at time t k The image visual factor on is expressed as: Among them, π(·) is the camera projection model, Projection point The actual pixel location on the image plane; is the camera time t k Position in the world system; The moving object is modeled using a constant linear velocity and angular velocity model, and the smoothing factor is defined as: in It consists of the linear velocity and angular velocity of the object. Expressed as the timestamp difference between two frames; Exp(·) is the inverse operation of Log(·); Assuming that the dynamic object moves at a constant speed in a short period of time, the constant speed factor for continuously tracking the object is defined as: The final optimization problem is defined as Equation (15), which obtains the best estimates of the ego vehicle and the tracked object by minimizing the sum of the prior norm and the Mahalanobis distance norm of all measurement residuals; Among them, c is the set of all variables, γ prior (c) is the marginalized prior residual term, θ(·) is the robust Huber kernel function, ∑ O ,∑ C ,∑ P ,∑ S ,∑ V are the covariance matrices of the corresponding factors.

Images