Moving object self and multi-target pose multi-sensor fusion simultaneous estimation method oriented to meta-universe environment

Abstract

The invention belongs to the field of meta-universe object pose estimation technology and robots, and discloses a meta-universe environment-oriented moving object self and multi-target pose multi-sensor fusion simultaneous estimation method. A more robust object-level data association is established using a multi-probability fusion tracker based on a Kalman filter and points in the optical flow. On the basis, a dynamic point cloud elimination method fusing the speed prior of a 3D Kalman filter and camera optical flow decoupling is provided so as to improve the precision of self pose estimation of the speedometer. And finally, combining multiple constraint factors in the proposed framework to refine the poses of the object and the object. Evaluation is carried out in a KITTI original data set, and compared with a reference and most advanced method, the method obtains better attitude precision of the KITTI and a tracking target. In addition, the proposed system shows the feasibility of real-time performance so as to ensure the practical application value of the system.

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 mobile object itself and multiple targets by multi-sensor fusion in a metaverse environment. Background Art

[0002] Simultaneous localization and mapping (SLAM) is essential for digitizing physical objects in the space of the metaverse in order to perform real-time 3D reconstruction and positioning in unknown environments, among which multi-sensor fusion methods have shown excellent performance. Some SLAM systems have achieved high accuracy, but most of them are based on the assumption of a static environment and reject the observation of dynamic objects during the optimization process, which makes the system difficult to apply in real-world high-dynamic scenes. In addition, in some applications, it is necessary to perceive surrounding motion information to ensure safety and information interaction, such as autonomous driving and advanced augmented reality (AR). Therefore, it is of great significance to study a tightly coupled method that achieves joint estimation of the pose of itself and multiple targets through sensor fusion.

[0003] Recent studies have combined SLAM and multi-target tracking (MOT) to simultaneously estimate the pose of the self and surrounding moving objects, showing impressive performance, indicating that dynamic objects are beneficial for estimating their own pose. In addition, it is particularly important to estimate the pose of surrounding objects while ensuring the accuracy of their own pose estimation. This can help robots handle various goal-oriented tasks such as path planning, human-computer interaction, and behavior prediction. However, most current algorithms use a single measurement sensor. Due to the complex environment in the real world and the increasing perception requirements of robots for 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 lot of current work focuses on developing accurate 3D detectors to estimate object poses, which largely depends on the performance of the detectors, which 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 multiple target poses in a metaverse environment, so as to realize the tightly coupled joint estimation of its own and multiple target poses, so as to obtain better pose accuracy for itself and the tracked targets.

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

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

[0008] Step 2, feature acquisition: obtain the object feature points by projecting the laser point onto the object mask in the camera image, track the feature points through optical flow and remove the 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 the matching points falling in 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 elimination: by decoupling the 2D optical flow of the object feature points and combining the motion state prior in the 3D Kalman filter, a method for estimating the object motion state is designed to accurately eliminate dynamic points; the motion state includes speed and angular velocity;

[0011] Step 5, self-pose estimation: self-pose estimation is achieved through the lidar inertial odometer in a tightly coupled iterative error state Kalman filter framework; laser point cloud and IMU data are used 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 back propagation, where 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 odometer is iteratively updated, the dynamic point removal 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-odometer;

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

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

[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 depth difference exceeding the set value are removed; finally, in the cluster with the possibility of object feature points, the nearest neighbor is used for the current object feature point. Rapid iteration finds potential object feature points, thereby obtaining the correct object feature points; for the acquired 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] Further, the multi-probability tracker based on the fusion of optical flow tracking and 2D IOU includes a fusion strategy of the object 2DIOU and the number of inliers of the feature points of the object 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 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 at k-1 The state covariance posterior estimate of the 2D bounding box at time instant;

[0023] Constructing a 2D IOU association matrix according to 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 additional weights for the number of internal points in the 2D IOU association matrix based on optical flow tracking. The position of the object feature points projected by the laser point cloud is predicted using optical flow in the current frame to obtain the object feature matching points, and the number of object feature matching points falling in the 2D bounding box 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 2D bounding box of the object; 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, different association cycle strategies for the object data association are:

[0029] For the successfully obtained matching relationship, the object will inherit its successfully matched trajectory ID and proceed to the next step of optimization; in addition, in order to solve the problem of 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, a new ID is assigned to it when it is detected for the first time. If it has not been successfully matched for more than α frames in a row, the trajectory ID is deleted and no longer matched with the newly detected object; if it has been tracked for a period of time and has not been successfully matched by any newly detected object in the λ frame, it is deleted.

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

[0031] The IMU, camera, laser, object and global reference frame are represented as {I}, {C}, {L}, {O}, {W} respectively; the reference frame {A} is at time t k The posture at this time is A k Indicates that the pose in frame {B} is expressed 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 time 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 Moment:

[0034]

[0035] Given a pair of object feature matching points based on optical flow tracking, find an optimal motion state estimate To 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 motion state prior [V, ω] of the object is obtained from the 3D Kalman filter KF and the covariance matrix ∑ KF , fusion optical flow decoupled motion state [V, ω] and covariance ∑, update speed [V, ω] update It is expressed as:

[0041]

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

[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 inside; 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 in 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 local state of the object and the real-time performance of the system, and batch optimization of the object's continuous tracking frames is realized; a tightly coupled joint estimation is realized through a factor graph optimization framework that integrates the state nodes of the object and the surrounding objects and the constraint edges provided by various factors; based on the self-pose obtained by the laser inertial odometer and the initial state of the dynamic object, the odometer factor, object observation factor, image vision factor, object smoothness factor, object speed factor and prior factor are introduced into the factor graph optimization framework for joint optimization to constrain the self-pose and object pose;

[0050] The odometer 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 speed between two frames, and the object speed factor constrains the object's speed consistency between two frames; the state of the oldest frame is selected for marginalization, and is 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:

[0052]

[0053] Here, 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 points of the objects in the world system are transformed to static points in the object system through the rigid body assumption and projected onto the image; then the jth dynamic feature of the ith object at time t k The image visual factor on is expressed as:

[0057]

[0058] Among them, π(·) is the camera projection model, The 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 is composed of the linear velocity and angular velocity of the object. It is 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 the continuously tracked object is defined as:

[0063]

[0064] The final optimization problem is defined as (15), 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;

[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 respectively.

[0067] Beneficial effects of the present invention: For the limitations of self-positioning and state estimation of surrounding objects in dynamic environments such as metaverse scenes: Most algorithms are based on the assumption of static environment, while ignoring the mutual benefit between dynamic objects and themselves, which greatly limits the application in actual high-dynamic scenes. In order to overcome these limitations, we propose a tightly coupled system that realizes joint estimation of self-position and multi-target pose through sensor fusion. By formulating a multi-probability fusion tracker to perform data association and using rigid assumptions to convert laser point clouds to object coordinate systems to achieve state decoupling, dynamic points are eliminated to improve odometer accuracy. In addition, a compact framework is developed to fuse laser, camera, and IMU data, and multiple constraint factors are combined in a sliding window to jointly optimize self-position and multi-target pose. Experimental results Compared with the most advanced dynamic slam technology, the method proposed by us achieves the best accuracy, indicating that 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 and ensure computational efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 The figure is a flowchart of a method for simultaneous estimation of the position and posture of a mobile object itself and multiple targets by multi-sensor fusion 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 simultaneously estimating the position and posture of a moving object and multiple targets by multi-sensor fusion in a metaverse environment, the specific steps are as follows:

[0071] Step 1: Data and object perception information acquisition: In the autonomous driving scenario, a car is used as a carrier to record an algorithm evaluation data set, which contains the true value of the camera pose and sensor external parameters, as well as some manual annotations of object poses. The pre-trained model of mask-rcnn is used to perform instance segmentation on the image to obtain the mask and 2D bounding box information of the vehicle.

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

[0073] Step 2, feature acquisition: Obtain the optical flow features of the object by projecting the laser points onto the image. Due to the detection accuracy and viewing angle differences, the edge information of the object is not reliable, which may lose some information or cause the depth of the projected point cloud to be incorrect. Therefore, three simple and effective methods are used to obtain the correct object points, and the object feature points are continuously updated using optical flow tracking and laser point cloud projection 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 produce incorrect calculations and bring additional errors, thus 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 clusters that may be 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: In order to ensure the performance and accuracy of object association, a multi-probability tracker based on optical flow tracking and 2D IOU fusion is designed. The association matrix is ​​constructed through the object 2D bounding box obtained in step 1 and the object optical flow inner point obtained in step 2 to improve the robustness and accuracy of object association and tracking;

[0075] Step 4: Dynamic point elimination: The autonomous driving scene contains a large number of dynamic points. In order to reduce the impact on its own odometer, a method is designed to accurately estimate the current speed of the object and eliminate dynamic points by decoupling the 2D optical flow of the object features and combining the speed prior.

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

[0077] Step 6: Factor graph joint optimization: In order 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 the efficient operation of the system, we opened a new thread for map publishing for parallel processing. Since invisible points under different perspectives are removed, through Lidar to camera external parameter transformation and accurate pose estimation, we can obtain accurate color information of the point cloud from the image, and maintain a globally consistent multi-target trajectory and RGB color point cloud map in real time through an independent thread. For each frame of the tracked object, we continuously update the map and distinguish between dynamic and static to publish the color point cloud of the object tracked in the last frame or all frames, and publish all motion trajectories.

[0079] The object feature acquisition is based on optical flow tracking of laser point projection;

[0080] For single-frame LiDAR and camera data, we obtain the semantics and 2D bounding box information of the object by performing instance segmentation on each frame of the image. Then we obtain the object features by projecting the laser points on the image based on the object perception results, avoiding additional feature extraction or depth recovery. In order to reduce the errors caused by detection accuracy or perspective differences, three simple and effective methods are used to obtain the correct object points: ① Through LiDAR-camera transformation, we project the LiDAR points into the image and only retain the points with the smallest distance per pixel to obtain the camera visible points. ② Based on geometric clustering, we quickly remove object points with inconsistent depth. ③ In the clusters that may be object points, we use the nearest neighbor to quickly iterate to find object points that may be partially lost. For the acquired object points, we use optical flow to track the features of the object in the next frame, and retain more accurate feature matching points by executing forward and backward optical flow algorithms, while continuously updating with laser point projection to ensure enough feature points.

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

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

[0083]

[0084] where x c and 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 predicted noise n(i)~N(0,Q(i)) that is 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 prediction, is the time t k-1 The posterior estimate of the bounding box state in .

[0090] The predicted 2D bounding box and the currently detected 2D bounding box are used to construct an association matrix, and the Hungarian algorithm matching is performed to realize object association tracking. In addition, in order to deal with associations in more complex scenes, such as when occlusion occurs. Therefore, we also add an additional metric in the association matrix based on the Lucas-Kanade (LK) optical flow field prediction: the number of internal points. The projected laser points are matched through the 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, so 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 2D bounding box of the object; 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 between adjacent frames can be matched. If a matching relationship is successfully obtained, the object will inherit its successfully matched trajectory id and proceed to the next step of optimization. In addition, in order to solve the inconsistency of object information due to occlusion, false detection or loss, we use different association cycles. If this 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 α frames in a row; 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 an estimation method based on the decoupling of the optical flow of the object point and combined with the velocity prior;

[0095] We denote the IMU, camera, laser, object, and global reference frames as {I}, {C}, {L}, {O}, {W}, respectively; the reference frame {A} is at time t k The posture at this time is A k Indicates that the pose in frame {B} is expressed 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 time 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 Moment:

[0098]

[0099] Perform forward and backward optical flow to retain more accurate feature matching points. Given a pair of tracked 2D optical flow points, we find an optimal estimate To 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, and ||·|| γ 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 to the object system according to the rigidity assumption, use ceres optimization to solve this least squares problem, and decouple the motion state of the object. For the tracked object, we can also obtain the object's velocity state prior [V, ω] from the 3D Kalman filter. KF and the covariance matrix ∑ KF , the speed and covariance ∑ decoupled from the fusion optical flow estimation, the updated speed [V, ω] update It is 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 inside. We convert the state vector of the object Defined as:

[0109]

[0110] in, is the angle of rotation of the ith 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. Finally, 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 sliding window technology to ensure the consistency of the local state of the object and the real-time performance of the system, and realize batch optimization of the continuous tracking frames of the object. Through a factor graph optimization framework that integrates the state nodes of itself and surrounding objects and the constraint edges provided by various factors, a tightly coupled joint estimation is achieved. We first obtain the laser odometer results and estimate the initial pose of the dynamic object. In the sliding window, the odometer factor, object observation factor, visual factor, object smoothness factor, object speed factor and prior factor are introduced for joint optimization, which effectively constrains the pose of itself and the object.

[0114] The odometer factor constrains the robot's own posture, the object observation factor constrains the relative transformation between the robot and the object's posture, the visual factor constrains the object's posture, the object smoothness factor constrains the object's posture and speed between two frames, and the object speed factor constrains the object's speed consistency between two frames. In order to maintain the consistency of the sliding window and make the system execute quickly, we choose to marginalize the state of the oldest frame instead of adding the Hessian block of dense landmarks, and at the same time constrain the first posture of the batch processing optimization framework in the sliding window.

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

[0116]

[0117] Here, 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 ith object at time t k The image visual factor on can be expressed as:

[0121]

[0122] where π(·) is the camera projection model, The projection point The actual pixel position 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 is composed of the linear velocity and angular velocity of the object. It is 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) Through a multi-probability tracker that associates optical flow tracking with 2D-Iou fusion, the tracking points and 3D bounding box are continuously updated, which improves 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, and dynamic points can be eliminated to improve 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 can be constructed in a sliding window to enhance robustness and local consistency, thereby realizing the 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 through instance segmentation, and the object features are obtained based on the projection of 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 is designed. The weighted association matrix is ​​constructed by the IOU of the 2D bounding box predicted and detected by the uniform velocity model and the number of inliers based on the optical flow field, and then the Hungarian algorithm matching is performed to achieve fast and stable object association between adjacent frames. After obtaining the initial state of the object and itself, we send them together into a unified factor graph optimization framework to perform multi-constrained joint estimation. To ensure the efficient operation of the system, we open a new thread for map publishing for parallel processing. Since invisible points under different perspectives are removed, we can obtain accurate color information of the point cloud from the image through Lidar to camera extrinsic parameter transformation and accurate pose estimation, so as to reconstruct a globally accurate and consistent RGB color point cloud map. For the tracked objects, we distinguish between dynamic and static to publish the color point cloud of the object tracked in the last frame or all frames, and publish all motion trajectories at the same time.

Claims

1. A method for simultaneous estimation of the position and posture of a mobile object and multiple targets by multi-sensor fusion in a metaverse environment, characterized in that: The specific steps are as follows: Step 1: Obtain data and object perception information: the data includes complete laser point cloud, camera and IMU data; the object perception information is a 2D bounding box and corresponding object mask obtained by instance segmentation of the rigid object in each frame image; Step 2, feature acquisition: obtain the object feature points by projecting the laser point onto the object mask in the camera image, track the feature points through optical flow and remove the outliers caused by tracking accuracy; Step 3, object data association: Construct a multi-probability tracker based on optical flow tracking and 2D IOU fusion, 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 falling into 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 elimination: by decoupling the 2D optical flow of the object feature points and combining the motion state prior in the 3D Kalman filter, a method for estimating the object motion state is designed to accurately eliminate dynamic points; the motion state includes speed and angular velocity; Step 5, self-pose estimation: self-pose estimation is achieved through the lidar inertial odometer in a tightly coupled iterative error state Kalman filter framework; laser point cloud and IMU data are used 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 back propagation, where 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 odometer is iteratively updated, the dynamic point removal 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-odometer; Step 6, factor graph joint optimization: In order to obtain the initial pose of the object, a 3D Kalman filter is constructed to predict the object state based on velocity, and the object velocity is continuously updated through the object motion state estimation method in step 4 and the motion state prior of the 3D Kalman filter to ensure the accuracy of the object velocity; in addition, the object feature matching points obtained by optical flow tracking and the object state prediction are obtained through PNP, and finally the method with the largest number of internal points is retained as the initial pose of the object; Then the initial pose of the object and the own pose obtained in step 5 are sent 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 laser radar to camera and accurate object and self-pose estimation, the laser point cloud is projected into the image to obtain the accurate color information of the point cloud, and a global consistent multi-target trajectory and RGB color point cloud map is maintained in real time using an independent thread.

2. The method for simultaneously estimating the position and posture 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 the object mask of the object perception information through the laser radar to obtain the object feature points, and only retaining the points with the minimum depth of each pixel; then removing the object feature points whose depth difference exceeds the 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 the potential object feature points, so as to obtain the correct object feature points; for the acquired 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 used to update to ensure enough feature points.

3. The method for simultaneously estimating 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 2D IOU and the number of internal points of the object 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 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 at k-1 The state covariance posterior estimate of the 2D bounding box at time instant; Constructing a 2D IOU association matrix according to 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 additional weights for the number of internal points in the 2D IOU association matrix based on optical flow tracking. The position of the object feature points projected by the laser point cloud is predicted using optical flow in the current frame to obtain the object feature matching points, and the number of object feature matching points falling in the 2D bounding box 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 2D bounding box of the object; 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 simultaneously estimating 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 the object data association are: For the successfully obtained matching relationship, the object will inherit its successfully matched trajectory ID and proceed to the next step of optimization; in addition, in order to solve the problem of 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, a new ID is assigned to it when it is detected for the first time. If it has not been successfully matched for more than α frames in a row, the trajectory ID is deleted and no longer matched with the newly detected object; if it has been tracked for a period of time and has not been successfully matched by any newly detected object in the λ frame, it is deleted.

5. The method for simultaneously estimating 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: The IMU, camera, laser, object and global reference frame are represented as {I}, {C}, {L}, {O}, {W} respectively; the reference frame {A} is at time t k The posture at this time is A k Indicates that the pose in frame {B} is expressed 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 To 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 motion state prior [V, ω] of the object is obtained from the 3D Kalman filter KF and the covariance matrix ∑ KF , fusion optical flow decoupled motion state [V, ω] and covariance ∑, update speed [V, ω] update It is expressed as: Before the odometer is iteratively updated, the laser point cloud of dynamic objects is removed based on the motion state estimation to reduce the impact on its own odometer.

6. The method for simultaneously estimating 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 to predict the object 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 inside; 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 in the object.

7. The method for simultaneously estimating 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 local state of the object and the real-time performance of the system, and batch optimization of the object's continuous tracking frames is realized; a tightly coupled joint estimation is realized through a factor graph optimization framework that integrates the state nodes of the object and the surrounding objects and the constraint edges provided by various factors; based on the self-pose obtained by the laser inertial odometer and the initial state of the dynamic object, the odometer factor, object observation factor, image vision factor, object smoothness factor, object speed factor and prior factor are introduced into the factor graph optimization framework for joint optimization to constrain the self-pose and object pose; The odometer 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 speed between two frames, and the object speed factor constrains the object's speed consistency between two frames; the state of the oldest frame is selected for marginalization, and is 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: Here, 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 points of the objects in the world system are transformed to static points in the object system through the rigid body assumption and projected onto the image; then the jth dynamic feature of the ith object at time t k The image visual factor on is expressed as: Among them, π(·) is the camera projection model, The 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 is composed of the linear velocity and angular velocity of the object. It is 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 the continuously tracked object is defined as: The final optimization problem is defined as (15), 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; 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 respectively.

Images