CN116630392A - Visual SLAM method for coupling multi-target tracking - Google Patents

Abstract

The invention proposes a visual SLAM method coupled with multi-target tracking. The method is divided into coupled visual mileage and front-end and graph optimization back-end. First, the Extended Kalman Filter (EKF) is used to track the target based on the target detection information; then, the current features are classified according to the target motion information obtained by the EKF, and the static features are used to obtain the pose information of the camera. The dynamic features are used to obtain the 6‑DOF pose information of the target; finally, a multivariate factor graph optimization model is established to jointly optimize the camera pose, target pose, static 3D points, and dynamic 3D points in motion. The present invention aims at the problem of low positioning accuracy and insufficient target information of the SLAM system in a dynamic environment, and studies the role of target information in the scene on the robot in the process of positioning and mapping; the present invention can realize mobile devices in a dynamic environment. Accurate camera pose information and target pose information, and track the target in the scene.

Description

A visual SLAM method coupled with multi-target tracking

technical field

The invention belongs to the fields of computer vision and SLAM, and in particular relates to a visual SLAM method coupled with multi-target tracking

Background technique

With the development of various artificial intelligences, the application of various virtual reality technologies in various fields has been greatly promoted, such as autonomous navigation of mobile robots, human-machine collaboration, and industrial-level scene reconstruction. Simultaneous Localization and Mapping (SLAM) technology can help robots/sensors realize scene perception and accurate self-positioning at the same time. It has been greatly developed, but it has relatively few applications in reality. The main reason is that the current mainstream SLAM method relies on the rigid target in the scene to remain globally static. Obviously, this condition is very harsh. There are many moving objects in a real scene, making it a dynamic environment. This makes traditional SLAM methods no longer applicable. In recent years, some excellent methods have emerged focusing on solving the problems faced by SLAM in dynamic scenes. Most of these methods use prior information (such as semantic information or motion structure information in the scene) to remove moving objects in the scene as outliers in the process of localization and mapping. The main goal of the above method is to greatly improve the calculation accuracy of the pose by eliminating the dynamic target. Obviously, these methods lose the information of dynamic objects in the scene.

To sum up, in view of the shortcomings of the above research, a visual SLAM method coupled with multi-target tracking is proposed.

Contents of the invention

In view of the above problems, the object of the present invention is to provide a visual SLAM method coupled with multi-target tracking.

A visual SLAM method coupled with multi-target tracking, comprising the following steps:

Step 1: Use the COCO dataset to train the instance segmentation network Yolact;

Step 2: Design a two-dimensional object tracker to track moving objects in the scene;

Step 3: Extract features from the current image, use the tracked target information to classify the features, divide the features into dynamic target features and static background features, and static features are used to obtain the camera pose;

Step 4: Obtain the pose information of the target based on the minimum reprojection error principle by using the target feature;

Step 5: Establish a multi-factor graph model, and jointly optimize the camera pose, target pose, static 3D points, and dynamic 3D points on the target.

Compared with the prior art, the present invention has the following beneficial effects:

1. Compared with the existing SLAM method, the pose estimation accuracy of the camera in a dynamic environment is greatly improved;

2. The movement information of the target in the scene can be obtained in real time.

Description of drawings

Figure 1 is a schematic diagram of the COCO dataset;

Figure 2 is a schematic diagram of instance segmentation effect diagram and feature classification;

Fig. 3 is a transformation relationship diagram of the camera in consecutive frames;

Figure 4 is a structural diagram of the back-end multivariate factor graph model;

FIG. 5 is a schematic diagram of acquired target motion information and camera motion trajectory;

Detailed ways

The technical solution of the present invention will be described in detail below in conjunction with the accompanying drawings.

A visual SLAM method coupled with multi-target tracking, specifically comprising the following steps:

Step 1: Use the COCO dataset to train the instance segmentation network Yolact

Step 11: In order to obtain the instance mask information and detection frame information of the moving objects in the scene, Yolact is trained on the COCO instance segmentation dataset. Humans and robots in the dataset are considered as dynamic objects in the scene. The images in the dataset are as follows: As shown in Figure 1;

Step 12: Send the instance mask information and detection frame information output by Yolact to the SLAM end through ROS.

Step 2: Design a 2D target tracker to track moving targets in the scene

Step 21: using the target detection information output by Yolact;

Step 22: Estimate the target state, define is the motion state to be estimated, where /> Indicates the three-dimensional position information of the center of the target mask, /> Represents the speed information of the target in three directions, and the state estimation process of the entire target can be expressed as:

In the formula, Represents the target state that needs to be estimated, F is the state transition matrix, w _i-1 is the process noise in the state transition process, /> is the uncertainty in the state estimation process, Q is the covariance matrix of the process noise, is the distribution matrix of the process noise, Δt represents the observation time, /> is the covariance matrix of the target velocity model;

Step 23: Update the target state. The update process mainly uses the Kalman gain coefficient K _i to estimate the target state and /> The measurements are fused:

In the formula, H _i represents the observation function The Jacobian matrix of the observation function h(·)=[f _α ,f _β ,f _γ ,1] can realize the transition of the target from the estimated state to the measured state, and its specific expression is as follows:

In the formula, [x _o , y _o , z _o ] represent the coordinates of the camera center point;

Step 24: Update the Kalman gain coefficient and uncertainty, the main process is as follows:

In the formula, is the covariance matrix of the observation noise.

Step 3: Use the FAST method to extract features from the current image, use the tracked target information to classify the features, and use the static features to obtain the camera pose

Step 31: extracting FAST features from the current picture to obtain the target feature set;

Step 32: Using the target state obtained in step 2, the estimated speed of the target can be obtained:

is the first estimated velocity of the acquired target, in addition, according to the 6-DOF pose information of the target in the previous frame You can get the target to get a second estimated velocity:

Finally, the final velocity of the target is obtained using the following formula:

α and β are set to 0.4 and 0.6 respectively in the experiment, and finally if V _i ^j ≥ l, the feature on the target is considered as a dynamic feature;

Step 33: Eliminate dynamic features from the feature set, and use PnP to solve the camera pose for the remaining static features.

Step 4: Using the target features, based on the principle of minimum reprojection error, obtain the pose information of the target

Step 41: Establish the motion model of the camera and the target as shown in Figure 3;

Step 42: Establish a nonlinear transformation model of 3D points, and initially express the motion transformation information of the target between image frames:

In the above formula, represents a 3D point on the target, /> Represents the target pose information of the i-1th frame and the i-th frame, /> is the pose transformation between two adjacent frames;

Step 43: According to the camera projection model, establish the reprojection error of the 3D point on the target:

In the above formula, Indicates the pixel corresponding to the matching feature point pair, /> represents the projection function, /> represents the camera pose of the i-th frame, meanwhile, for all 3D points on the target, the minimum reprojection error can be expressed as:

In the above formula, n _d represents the total number of features extracted from the target, Indicates the final pose transformation information that needs to be obtained;

Step 44: Map the above minimum reprojection error model into the Lie algebraic space:

In the formula, yes /> The exponential map in the Lie algebraic space, that is ρ is the translation vector of the target, and φ represents the rotation vector of the target. Finally, the Gauss-Newton method is used to solve the above formula.

Step 5: Establish a graph optimization model, and jointly optimize the camera pose, target pose, static 3D points, and dynamic 3D points on the target

Step 51: Establish a factor graph model as shown in Figure 4. The factor graph includes target 3D points, static 3D points, camera pose information, target pose information and various variables to be optimized;

Step 52: Establish various errors corresponding to the nodes of the factor graph:

In the formula, Indicates the projection error of a static 3D point, /> represents the projection error of a dynamic 3D point, /> Represents the motion error of the target, ω represents the influence parameter of the camera pose on the target pose, and the measurement error of the camera pose adopts the existing method;

Step 53: The above multiple error models are jointly determined as:

Represents the set of variables to be optimized, and finally uses the Levenberg-Marquardt method to solve the above formula.

A complete description of the entire network structure is as follows:

Step 1: Use the COCO dataset to train the instance segmentation network Yolact;

Step 2: Design a two-dimensional object tracker to track moving objects in the scene;

Step 3: Extract the features of the current image, classify the features by using the tracked target information, divide the features into dynamic target features and static background features, and static features are used to obtain the camera pose;

Step 4: Using the target features, based on the principle of minimum reprojection error, obtain the pose information of the target;

Step 5: Establish a graph optimization model, and jointly optimize the camera pose, target pose, static 3D points, and dynamic 3D points on the target;

Step 6: Obtain the movement trajectory of the target and the movement trajectory of the camera as shown in FIG. 5 .

Claims (1)

1. a visual SLAM method for coupling multiple target tracking, is characterized in that, comprises the steps: Step 1: Build a visual SLAM method framework coupled with multi-target tracking; The entire framework includes a coupled odometry front-end and a graph-optimized back-end. The coupled odometer front-end includes a real two-dimensional target tracker, target pose estimation, and camera pose estimation. Information and camera pose information are optimized; Step 2: Design a 2D object tracker; Based on the detection information of the target in the scene, the extended Kalman filter (EKF) is used to estimate the motion state of the target; at the i-th moment, the estimated value of the motion state of the target j Expressed as: In the formula, w _i-1 is the process noise in the state transition process, is the final motion state of the target j to be tracked, /> is the three-dimensional position information of the mask of the target j, /> is the speed information of the target j in the three directions of x, y, and z, /> is the state transition matrix, I _3×1 is an identity matrix, Δt is the estimated frequency for the target motion state; the estimated value of the uncertainty in the target state estimation process /> It can be expressed as: In the formula, is the uncertainty of target j at time i-1, Q=A×∑× ^AT with/> represents the noise term in the uncertainty estimation process, /> is the distribution matrix of the process noise, /> is used to represent the speed model /> The covariance matrix of (moving speeds of different types of targets in the workshop), I _3×3 is an identity matrix; next, the moving state of target j /> is updated by the following formula: In the formula, Indicates the Kalman gain coefficient at the i-th moment, /> Represents the covariance matrix of the observation noise, the value on the diagonal of R _i represents the variance of the three-dimensional coordinates of the mask center of the target j, /> Indicates the observed value of target j, that is, the three-dimensional coordinates corresponding to the center of its mask, /> is the observation function /> The Jacobian matrix of , the observation function h(·)=[f _α ,f _β ,f _γ ,1] is expressed by the following formula: In the formula, α, β, γ are the Euler angles of the movement of the target j, and the observation function h( ) can realize the observed value of the target j Mapping to Motion State Estimates /> [x _o , y _o , z _o ] represents the world coordinates of the camera center point; Step 3: Use the FAST method to perform feature extraction on the current image to obtain a feature set, which includes target features on the dynamic target and static features belonging to the static background; Step 4: Target motion state estimation; Use the tracked motion state of target j The medium velocity component V _i ^j can obtain the linear velocity of target j: Set l as the speed threshold of the moving target in the scene. If the speed of the target V _i ^j >l, the target j is considered to be a dynamic target in the scene, and the features extracted from the target j belong to the target feature. If V _i ^j < l, the target j is considered static, and the features extracted from the target j are static features; Step 5: camera pose estimation; Use the target mask information output by Yolact to mark the position of all static features in the image at the i-th moment, and use the PnP method to obtain the initial pose of the camera for the static features Step 6: target pose estimation; For a 3D point on target j at time i According to the camera projection model, its pixel value P _i ^j,n and its coordinates in its own reference system can be obtained /> So /> The reprojection error ^ζi at consecutive frames is expressed as: In the formula, is the camera pose information at frame i, /> is the projection function (camera internal parameters), /> is the target pose information of the target at time i-1, /> Represents the SE(3) pose transformation of the 3D point in two consecutive frames; then, the reprojection error e(V _i ^j ) for all 3D points on the target j is expressed as: In the formula, The pose transformation of the target between arbitrary images, n _d is the total number of 3D points from the target j, and finally, the above formula is solved in the Lie algebraic space to obtain the pose of the target; Step 7: Establish a multivariate factor graph model; 3D projections that will belong to static features 3D projections belonging to target features /> The pose information of the dynamic target and the pose information of the camera are used as nodes in the multivariate factor graph; Step 8: Obtain the camera measurement error e _i ; Use the camera measurement error in ORB_SLAM2 as the camera measurement error here Step 9: Obtain the target feature projection error According to the target j pose information described in step 6 Target feature projection error for target j /> Expressed as: Step 10: Obtain the projection error of static features According to the camera pose information described in step 5 Projection error of static features/> Expressed as: In the formula, ψ( ) represents the projection function, is the camera pose information, /> yes /> Corresponding two-dimensional pixel coordinates; Step 11: Obtain the motion constraint error of target j between consecutive image frames According to the target motion state described in step 4 Express the motion error of the target between consecutive image frames as In the formula, represents the target state as described in claim 1; Step 12: Establish a joint error constraint model; The above multiple constraint models constitute a factor graph, and each constraint model is used as a node of the factor graph. Finally, the factor graph optimization problem is expressed by the following formula: In the formula, M _c is a set of image frames with a common view relationship, M _o is the total number of targets tracked in the current frame image, M _s is the static feature set extracted from the background, and M _d is the dynamic feature extracted from the target set, l _i is the Huber loss function, ∑Δt represents the covariance matrix of the observation frequency, is the parameter to be optimized in the entire factor graph, and finally the above formula is solved by Levenberg-Marquardt to obtain the optimal parameter.