Multi-object tracking and pose optimization method based on camera parameterization

Abstract

The application provides a multi-object tracking and pose optimization method based on camera parameterization, and relates to the technical fields of computer vision and autonomous driving. The method specifically comprises: acquiring laser point data and performing 3D target detection to obtain M 3D object detection boxes at t moments, and generating a continuous area object mask by extracting object points in the object detection box; for an existing object to be detected, predicting N 3D prediction detection boxes at t moments, constructing an affinity matrix according to the N 3D prediction detection boxes at t moments and the M 3D object detection boxes at t moments, and tracking the object by using a KM algorithm; for a successfully tracked object, extracting new feature points of the object at t moments, associating the feature points of the object at t-1 moments with the new feature points extracted at t moments by using a corner tracking algorithm, and obtaining object points at t moments; constructing a loss function by constructing geometric constraints and semantic constraints, and optimizing by using a LM algorithm to obtain the optimized pose and speed of the object at t moments.

Description

Technical Field

[0001] This invention relates to the fields of computer vision and autonomous driving technology, and in particular to a method for multi-object tracking and pose optimization based on camera parameterization. Background Technology

[0002] Multi-object tracking and pose optimization are key issues for autonomous vehicles operating in real-world dynamic environments. Tracking refers to determining whether objects identified by associated sensors at different times are the same object; pose optimization refers to matching features between adjacent frames and minimizing residuals to obtain the object's position and orientation information at the observation time. The task of safe driving for autonomous vehicles is to perceive the motion state of surrounding objects in real time, predict their trajectories, and plan the optimal path. Multi-object tracking and pose optimization typically obtain object information in the scene by performing semantic recognition on the information collected by sensors, constructing residuals based on geometric and semantic constraints, and then using a sliding window optimization method to solve for the accurate pose of the objects. For example, in Simultaneous Localization and Mapping (SLAM), accurate object tracking and pose optimization can accurately determine the dynamic and static states of objects, optimizing sensor poses using only scene feature points and static object feature points, maximizing the use of extracted feature points to improve the accuracy of pose estimation.

[0003] A paper published in *IEEE Robotics and Automation Letters (RAL), 5191-5198, 2021* proposes a tightly integrated SLAM system for multi-object tracking. This system utilizes instance segmentation and ORB (Oriented Fast and Rotated Brief) features to track dynamic objects. It jointly optimizes the motion poses of static scene map points, cameras, and dynamic objects using a novel optimization scheme, and also estimates and loosely optimizes the 3D bounding boxes of dynamic objects within a fixed time window. However, this system uses sparse visual features to estimate object pose and size, limiting the accuracy of solving the 3D bounding boxes. A paper published in *IEEE Robotics and Automation Letters (RAL), 6846-6853, 2022* proposes a mechanical joint, a function of semantic classes. By modeling the definition of constraints between clustered objects, the mechanical joints between clusters constrain the estimated torsion of dynamic clusters by blocking some degrees of freedom, thereby reducing the impact of noise on the estimated object pose. However, this system cannot estimate the six degrees of freedom pose of objects, limiting its application scenarios. A paper published in IEEE Transactions on Instrumentation and Measurement (TIM), 8504614(72), 2023, proposes a multi-level data association method and a geometry-based dynamic target classification method for target tracking in a SLAM system. This system extends multi-view geometric constraints between two consecutive frames to a sliding window to accurately distinguish tracked objects and jointly optimizes the poses of the vehicle and surrounding objects in a unified estimator. However, this system uses a tightly coupled approach to optimize its own and object poses, making it susceptible to noise and mutual interference. Summary of the Invention

[0004] To address the shortcomings of previous methods that used Simultaneous Localization and Object Tracking (SLOT) to estimate object pose in the world coordinate system, where coupled camera pose is susceptible to mutual interference from measurement noise and associated noise, resulting in low overall performance, this invention proposes a multi-object tracking and pose optimization method based on camera parameterization. This method aims to decouple the vehicle and object poses and construct optimization factors suitable for dynamic objects based on the rigidity assumption of the objects.

[0005] This invention proposes a multi-object tracking and pose optimization method based on camera parameterization, comprising:

[0006] Step 1: Acquire laser point data and perform 3D target detection using a PointPillar network to obtain M 3D object detection boxes at time t. And extract the object detection box. Object points within the bounding box, by... The extracted object points are transformed to pixel coordinates to generate a continuous object mask. t M k ;

[0007] Step 2: For an existing object to be detected, predict the 3D bounding box at N time points t. And calculate the 3D predicted detection boxes at N time points t. and M 3D object detection boxes at time t The intersection-union ratio (IoU) is used to construct an affinity matrix, and the Hungarian KM algorithm is used to track objects based on this affinity matrix;

[0008] Step 3: For successfully tracked objects, extract the new feature points of the object at time t. Use the KLT corner tracking algorithm to associate the feature points of the object at time t-1 with the new feature points extracted at time t to obtain the object points at time t. At the same time, calculate the maintained object size.

[0009] Step 4: Construct a loss function by building geometric and semantic constraints, build an n-frame sliding window, and optimize the loss function using the Levenberg-Marquardt LM algorithm to obtain the optimal pose of the object at time t. and speed

[0010] The steps further include:

[0011] Step 1.1: Acquire laser point data and use a PointPillar network for 3D target detection to obtain M 3D object detection boxes at time t. Includes: object pose and object size

[0012] Step 1.2: Size the object Expand by α times, then apply the object detection box Extract object points within the area and construct an object point set. t P k , recorded as t P k ={ t p k,i},in t p k,i This indicates the position of an object point within the object detection box; i represents the i-th object point within the object detection box.

[0013] Step 1.3: Convert the extracted object points to the pixel coordinate system to obtain the object points in the pixel coordinate system;

[0014] Step 1.4: Place the object detection box Transform to pixel coordinates and generate a binary image of the projected object points based on the object points in the pixel coordinates.

[0015] Step 1.5: Perform dilation and erosion processing on the binarized image to generate a continuous object mask. t M k ;

[0016] Step 2 further includes:

[0017] Step 2.1: For an existing object to be detected, acquire its historical data and make predictions based on the acquired historical data to obtain N 3D predicted detection boxes at time t.

[0018] Step 2.2: Utilize the 3D object detection boxes at M time points t. Construct detection box detection set B d ; 3D prediction detection boxes at N time points t Construct the detection box prediction set B p ;

[0019] Step 2.3: Calculate the detection set B of the detection box d 3D object detection box and the detection box prediction set B p 3D detection box in 3D intersection-union ratio;

[0020] Step 2.4: Construct the affinity matrix based on the calculated 3D intersection-union ratio (IU), and solve the affinity matrix using the KM algorithm to obtain the detection set B of the detection box. d and detection box prediction set B p The matching results are used to update the object's pose, and object tracking is achieved by maximizing the affinity matrix.

[0021] The historical data mentioned in step 2.1 includes: the optimized object pose at time t-1. speed and the size of the maintained object

[0022] The method for updating the object's pose based on the matching results in step 2.4 is as follows: based on the detection set B of the bounding box... d and detection box prediction set B pThe matching results are used to update the pose of the object to be detected at time t for successfully matched bounding boxes; for unmatched bounding boxes, if the bounding box prediction set B... p If an unmatched 3D object detection box exists, the predicted object pose within the 3D predicted detection box is taken as the pose of the object to be detected at time t, and the number of times the object fails to match consecutively is recorded. If the recorded number exceeds a preset threshold, the detection is stopped. When the object is detected, it is removed; if the detection box detects set B d If there is an unmatched 3D object detection box, a new object is created at time t based on the unmatched 3D object detection box, and the initial pose of the object is set.

[0023] Step 3 further includes:

[0024] Step 3.1: For a successfully tracked object, obtain the projected laser points of the object based on the projection operation and use them as feature points. Homogenize the feature points and use the homogenization result as the new feature points extracted at time t.

[0025] Step 3.2: Obtain the feature points of the object at time t-1, and use the corner tracking algorithm KLT to associate the feature points of the object at time t-1 with the new feature points extracted at time t to obtain the object points at time t;

[0026] Step 3.3: Obtain the number of object points in all 3D object detection boxes in the historical data of this object, and select the average size of the object corresponding to the m frames of 3D object detection boxes with the most object points. Use the obtained average value as the maintained object size.

[0027] The geometric constraints mentioned in step 4 include: object reprojection residual e z Planar manifold factor residual e p The residual e of the uniform velocity model of the object v ;

[0028] The object reprojection residual e z The construction method is as follows: based on the feature point information of the matching object obtained in step 3, and based on the rigidity characteristics of the object, construct the object reprojection residual e. z for:

[0029]

[0030] Where ρ represents the Huber robust estimator; This represents the normalized coordinates of the associated feature points of the m-th feature point of the k-th object in the sliding window at frame n; π unit () is an operation that transforms 3D coordinate points in the camera coordinate system to normalized coordinates. This represents the pose of the k-th object in the n-th frame; This represents the pose of the k-th object in the (n-1)-th frame; This represents the m-th map point of the k-th object in the (n-1)-th frame; To solve for the L2 norm; Σ n,m It is the covariance matrix of the reprojection error;

[0031] The planar manifold factor residual e p The construction method is as follows: extract the plane parameters of the camera coordinate system as ground plane information, and construct the plane manifold factor residual e based on the extracted camera coordinate system plane parameters. p ;

[0032] The plane parameters of the camera coordinate system are denoted as: in δ represents the normal vector at time t; h Indicates the altitude at time t;

[0033] The planar manifold factor residual e p Represented as:

[0034]

[0035] in Represents object pose The rotation; e2 represents the normal vector of the ground plane in the object coordinate system, and e2 = [0 1 0]. T H p This represents the first and third values ​​extracted from the difference in the transformed normal vector, and... F h () indicates that the object height information is extracted from the transformation matrix, W p It is a diagonal weight matrix used to balance errors;

[0036] The uniform velocity model residual e of the object v The construction method is as follows: the assumption of a uniform speed model for the vehicle is used as the prior information of the uniform speed model, and the residual e of the uniform speed model of the object is constructed. v , represented as:

[0037]

[0038]

[0039]

[0040] in F represents the velocity of the k-th object in the n-th frame; t () indicates the extraction of translation components from the transformation matrix; Let Lie algebra represent the relative rotation of the k-th object in the n-th frame; Represents object pose The inverse of the rotating part; W v This is the diagonal weight matrix for the uniform velocity model; This represents the velocity of the k-th object in the (n-1)-th frame; Let Lie algebra represent the relative rotation of the k-th object in the (n-1)-th frame;

[0041] The semantic constraint mentioned in step 4 is: the semantic prior residual e constructed by obtaining semantic prior information. d ;

[0042] The semantic prior residual e d Represented as:

[0043]

[0044] Among them W d The diagonal weight matrix represents the semantic prior constraints. This represents the estimated pose of the k-th object detected in the n-th frame;

[0045] The loss function mentioned in step 4 is:

[0046]

[0047] Where N K This represents a set of image frames for a sliding window. n-1 M k This represents the map point set in the (n-1)th frame; This represents the normalized coordinates of the associated feature points of the m-th feature point of the k-th object in the sliding window at the n-th frame. This represents the pose of the k-th object in the n-th frame; This represents the pose of the k-th object in the (n-1)-th frame; n π represents the plane parameters estimated in the nth frame.

[0048] The beneficial effects of adopting the above technical solution are as follows:

[0049] To address the issue that traditional SLOT systems are susceptible to noise, which reduces overall performance, the method of this invention directly estimates the object pose based on the current camera coordinate system by maintaining the object points in the camera coordinate system within the sliding window, and constructs an optimization factor suitable for dynamic objects based on the rigidity assumption of the object, thereby decoupling the object pose and the camera pose.

[0050] This invention employs LiDAR-based 3D target detection to obtain initial object pose values ​​that are more accurate than those obtained through pure vision. Simultaneously, projected laser points are used for mask selection to ensure accurate depth and uniform distribution of feature points, laying a solid foundation for feature matching. By introducing laser points to extract depth values ​​for feature points, the problem of low accuracy in estimating feature point depth through pure vision is addressed.

[0051] The method of this invention integrates the reprojection residuals of object feature points, planar residuals, uniform velocity model residuals, and semantic prior residuals, and performs unified optimization within a sliding window to solve for the pose and velocity of each object. This greatly improves the accuracy of locating surrounding objects and is helpful for path planning and obstacle avoidance in autonomous vehicles.

[0052] The method of this invention separates object tracking and pose optimization from camera pose estimation as an independent module, thereby avoiding the impact of poor camera pose estimation on object pose estimation, and improving the accuracy of autonomous vehicles in estimating the pose of surrounding objects and tracking accuracy. Attached Figure Description

[0053] Figure 1 This is a flowchart of a multi-object tracking and pose optimization method based on camera parameterization in this embodiment;

[0054] Figure 2 Figure (a) shows the binarized image and mask image of the object projection laser points in this embodiment; Figure (b) shows the binarized image of the object projection points; Figure (c) shows the mask image generated by the object projection points.

[0055] Figure 3 This is a schematic diagram of a multi-object tracking and pose optimization method based on camera parameterization in this embodiment.

[0056] Figure 4 This is a schematic diagram of the projection of feature points based on the camera coordinate system in this embodiment;

[0057] Figure 5 Figure 1 shows a comparison of the object pose performance of this embodiment with that of pure object detection. Figure 2 shows the result of the KITTI Tracking dataset 0003 sequence 1. Figure 3 shows the result of the KITTI Tracking dataset 0003 sequence 2. Figure 4 shows the result of the KITTI Tracking dataset 0003 sequence 3. Figure 5 shows the result of the KITTI Tracking dataset 0018 sequence 1. Figure 6 shows the result of the KITTI Tracking dataset 0018 sequence 2. Figure 7 shows the result of the KITTI Tracking dataset 0018 sequence 3. Detailed Implementation

[0058] To facilitate understanding of this application, specific embodiments of the present invention will be described in further detail below with reference to the accompanying drawings and embodiments. The following embodiments are illustrative of the invention but are not intended to limit its scope. Rather, these embodiments are provided to provide a more thorough and complete understanding of the disclosure of this application.

[0059] This embodiment presents a multi-object tracking and pose optimization method based on camera parameterization, such as... Figure 1 As shown, the method includes the following steps:

[0060] Step 1: Acquire laser point data and perform 3D target detection using a PointPillar network to obtain M 3D object detection boxes at time t. And extract the object detection box. Object points within the bounding box, by... The extracted object points are transformed to pixel coordinates to generate a continuous object mask. t M k .

[0061] in This represents the k-th 3D object detection box detected at the current time t.

[0062] Step 1.1: Acquire laser point data and use a PointPillar network for 3D target detection to obtain M 3D object detection boxes at time t. Includes: object pose and object size

[0063] In this embodiment, the PointPillar network is used to perform 3D target detection on the laser points in the KITTI dataset, and the k-th 3D object detection box detected at the current time t is obtained.

[0064] Step 1.2: Size the object Expand by α times, then apply the object detection box Extract object points within the area and construct an object point set. t P k , recorded as t P k ={ t p k,i},in t p k,i This indicates the position of an object point within the object detection box; i represents the i-th object point within the object detection box.

[0065] In this embodiment, since the target detection results may not be very accurate, the size of the object obtained from the target detection is... After scaling up by α, object points within the object detection box are extracted to construct an object point set. Both the detection box information and the laser points need to be transformed to the camera coordinate system for subsequent processing.

[0066] Step 1.3: Convert the extracted object points to the pixel coordinate system to obtain the object points in the pixel coordinate system.

[0067] t z k,i =p( t p k,i (1)

[0068] Where π(·) represents the operation of reprojecting the object point according to the pinhole model; t z k,i To be t p k,i The coordinates of the image pixels obtained by transforming to the pixel coordinate system.

[0069] Step 1.4: Place the object detection box Transform to pixel coordinates and generate a binary image of the projected object points based on the object points in the pixel coordinates.

[0070] In this embodiment, since the object detection box obtained by the three-dimensional object detection is 3D, it is necessary to convert it into a 2D detection box of the image according to the projection operation in formula (1) to accelerate the extraction of object points.

[0071] Step 1.5: Perform dilation and erosion processing on the binarized image to generate a continuous object mask. t M k .

[0072] The method for dilating the binarized image is as follows: use a convolution template ψ to traverse the binarized image, and assign the maximum value of the pixels within the ψ-covered area to the traversed pixels in the binarized image.

[0073]

[0074] Where `dilate(x,y)` represents the dilation operation on pixel (x,y) in the binarized image; `B` represents the set of relative pixel differences between the convolution template `ψ` and the pixel in the binarized image; `image()` represents the pixel value of the pixel in the binarized image; `x` represents the ordinate of the pixel; `y` represents the abscissa of the pixel; `x'` represents the difference between the ordinate of the region covered by the convolution template and the pixel in the binarized image; and `y'` represents the difference between the abscissa of the region covered by the convolution template and the pixel in the binarized image.

[0075] The corrosion method is as follows: the minimum value of pixels within the area covered by the convolution template ψ is calculated by convolving the image with the convolution template ψ.

[0076]

[0077] Here, erode(x,y) represents the erosion operation performed on pixel (x,y) in the binarized image.

[0078] In this embodiment, such as Figure 2 As shown, the detection box of the kth object detected at time t is projected based on the projection operation. The image is transformed into a 2D bounding box, and the object region is extracted from it. A binarized map of the projected object points is generated, with the projected laser point position as white and other object areas as black. This is used to generate an object mask for continuous regions. t M k Two basic image morphological operations, dilation and erosion, are used to fill the hole problem. First, the binarized image is dilated to expand the white area in the image. Dilation is performed multiple times until no holes exist or the dilated area reaches the boundary of the two-dimensional detection box. After the dilation operation, although the holes in the image are filled, the obtained feature extraction area is larger than the object pixel area. Therefore, the image is then subjected to the same number of erosion operations as the dilation operation to achieve "neighborhood erosion" in order to reduce and refine the extraction area.

[0079] Step 2: For an existing object to be detected, predict the 3D bounding box at N time points t. And calculate the 3D predicted detection boxes at N time points t. and M 3D object detection boxes at time t The intersection-over-union (IoU) ratio is used to construct an affinity matrix, and the Kuhn-Munkras (KM) algorithm is used to track objects based on this affinity matrix.

[0080] Step 2.1: For an existing object to be detected, acquire its historical data and make predictions based on the acquired historical data to obtain N 3D predicted detection boxes at time t.

[0081] The historical data includes: the optimized object pose at time t-1. speed and the size of the maintained object

[0082] In this embodiment, for each object that already exists in the system t-1 O k Obtain the optimized pose of the object at time t-1. speed and the size of the maintained object Predicting 3D detection boxes at time t

[0083] Step 2.2: Utilize the 3D object detection boxes at M time points t. Construct detection box detection set B d ; Utilizing N 3D prediction detection boxes at time t Construct the detection box prediction set B p .

[0084] In this embodiment, the detection set of the detection box is denoted as B. d ={ t B d},in t B d Let B represent the 3D object detection boxes in the detection set, and there are a total of M 3D object detection boxes; let B be the prediction set of the detection boxes. p ={ t B p There are N 3D bounding boxes in total.

[0085] Step 2.3: Calculate the detection set B of the detection box d 3D object detection box and the detection box prediction set B p 3D detection box in 3D intersection-union ratio;

[0086] The detection set B of the detection frame d 3D object detection box and the detection box prediction set B p 3D prediction detection box in 3D intersection and union ratio (IoU) 3D Represented as:

[0087]

[0088] Step 2.4: Construct the affinity matrix based on the calculated 3D intersection-union ratio (IU), and solve the affinity matrix using the KM algorithm to obtain the detection set B of the detection box. d and detection box prediction set B p The matching results are used to update the object's pose, and object tracking is completed by maximizing the affinity matrix.

[0089] The method for updating the object's pose based on the matching results is as follows: based on the detection set B of the detection box... d and detection box prediction set B p The matching results are used to update the pose of the object to be detected at time t for successfully matched bounding boxes; for unmatched bounding boxes, if the bounding box prediction set B... pIf there is an unmatched 3D object detection box in it, then use the predicted object pose in the 3D predicted detection box as the pose of the object to be detected at time t, and record the number of times the object fails to match continuously. When the recorded number is greater than the preset threshold , remove the object; if there is an unmatched 3D object detection box in the detection box detection set B d , then create a new object according to the unmatched 3D object detection box at time t, and set the initial pose of the object.

[0090] In this embodiment, the KM algorithm is used to solve the affinity matrix to maximize the weighted bipartite graph, that is, the affinity matrix to find the maximum match, complete the inter-frame object tracking, and transfer the object ID. Since the number of prediction sets N and the detection set M cannot be guaranteed to be equal, when N > M, this means that there are existing objects at time t-1 that do not match. For these objects, use the predicted object pose as the object pose of the t-th frame, and record the number of times the object fails to match continuously. If it is greater than the threshold This embodiment will set to 3, then the object will be removed from the algorithm, which provides a certain degree of immunity for missed detection cases. When N < M, it means that a new object is detected at time t, and a new object is created.

[0091] Step 3: For the objects with successful tracking, extract the new feature points of the object at time t, use the corner point tracking algorithm KLT to associate the feature points of the object at time t-1 obtained with the new feature points extracted at time t, obtain the object points at time t, and calculate the object size maintained by the object

[0092] In this embodiment, to ensure the continuous and stable tracking of object features in the system, each object needs to add new feature points. Compared with point clouds, visual images are inherently more conducive to feature extraction, but the visual features extracted by traditional methods are relatively sparse. In addition, the feature points need to find the corresponding projected laser points to obtain depth information, but not all features can find the corresponding laser points. Therefore, this embodiment directly uses the projected laser points as feature points.

[0093] Step 3.1: For the objects with successful tracking, obtain the projected laser points of the object based on the projection operation and use them as feature points, homogenize the feature points, and use the homogenized result as the new feature points extracted at time t.

[0094] The method for homogenizing the feature points is: use the circle function in OpenCV to draw a circle with a radius r centered on the feature points, and extract features on the object mask t M k in the continuous region according to the tracking times of the object to obtain new feature points.

[0095] In this embodiment, due to the high horizontal resolution of the lidar, the projected points are densely arranged in the horizontal direction, resulting in a large number of redundant features, which prolongs the optimization time and increases the possibility of incorrect feature matching. Therefore, the OpenCV library is used to draw a circle function, which projects the feature points onto the object mask according to the number of tracking iterations. t M k A black circle is drawn with the feature point as the center and r as the radius. The feature point can only be extracted in the white area of ​​the mask to ensure that the object feature points are evenly distributed.

[0096] Step 3.2: Obtain the feature points of the object at time t-1, and use the corner tracking algorithm (Kanade LucasTomasi, KLT) to associate the feature points of the object at time t-1 with the new feature points extracted at time t to obtain the object points at time t.

[0097] In this embodiment, to ensure continuous tracking, in addition to tracking historical observation feature points, new object feature points are extracted by projecting laser points and performing the homogenization operation in step 3.1. Using a sparse optical flow method, namely the Kanade Lucas Tomasi (KLT) algorithm, feature points are associated based on the image texture information at time t to obtain the feature point matching results between two frames.

[0098] Step 3.3: Obtain the number of object points in all 3D object detection boxes in the historical data of this object, and select the average size of the object corresponding to the m frames of 3D object detection boxes with the most object points. Use the obtained average value as the maintained object size.

[0099] In this embodiment, in addition to associating feature points, the object size also needs to be updated and maintained for successfully tracked objects. Used to generate a new 3D prediction detection box in the next moment. Since the object size is estimated differently in 3D target detection in different observation frames, the more complete the object information, i.e., the more laser points the object has, the more accurate the estimated object size. Therefore, this embodiment uses the average of the estimated object size in up to 5 frames with the most observed object points as the size of the object in the current frame.

[0100] Step 4: Construct a loss function by building geometric and semantic constraints, construct an n-frame sliding window, and optimize the loss function using the Levenberg-Marquardt (LM) algorithm to obtain the optimal pose of the object at time t. and speed

[0101] In this embodiment, such as Figure 3As shown, geometric and semantic constraints are constructed using feature point information from object matching, ground plane information, uniform velocity model prior information, and semantic prior information to optimize the pose and velocity of all successfully tracked objects. The visual reprojection error constructed from the geometric information of adjacent frames can only optimize the relative pose between frames; long-term optimization introduces cumulative errors. Each object maintains an n-frame sliding window, using historical frame information to reduce the cumulative errors introduced by poor single-frame optimization, and an object detection optimization factor is added to provide absolute constraints. Using the plane assumption, the estimated object is aligned with the plane of its own vehicle, constraining the object's three degrees of freedom. Furthermore, a uniform velocity model factor is added to ensure consistency in local pose tracking. Data association is performed by observing moving objects and landmarks across multiple frames, establishing a landmark associated with the k-th object, i.e., the object point. and observation frame N k A set of.

[0102] The geometric constraints include: object reprojection residual e z Planar manifold factor residual e p The residual e of the uniform velocity model of the object v .

[0103] In this embodiment, such as Figure 4 As shown, the object reprojection residual e z The construction method is as follows: based on the feature point information of the matching object obtained in step 3, and based on the rigidity characteristics of the object, construct the object reprojection residual e. z for:

[0104]

[0105] Where ρ represents the Huber robust estimator; This represents the normalized coordinates of the associated feature points of the m-th feature point of the k-th object in the sliding window at frame n; π unit () is an operation that transforms 3D coordinate points in the camera coordinate system to normalized coordinates. This represents the pose of the k-th object in the n-th frame; This represents the pose of the k-th object in the (n-1)-th frame; This represents the m-th map point of the k-th object in the (n-1)-th frame; To solve for the L2 norm; Σ n,m It is the covariance matrix of the reprojection error, estimated using the median absolute deviation (MAD).

[0106] The planar manifold factor residual e p The construction method is as follows: extract the plane parameters of the camera coordinate system as ground plane information, and construct the plane manifold factor residual e based on the extracted camera coordinate system plane parameters.p ;

[0107] The plane parameters of the camera coordinate system are denoted as: in δ represents the normal vector at time t; h This represents the altitude at time t.

[0108] In this embodiment, for outdoor scenes, the vehicle always travels on the road, making the use of planar constraints reasonable. For two objects on the same plane, with the same pitch and roll angles, and the rigid body's height relative to the plane remaining constant, a planar manifold factor residual is constructed based on the extracted camera coordinate system planar parameters.

[0109] The planar manifold factor residual e p Represented as:

[0110]

[0111] in Represents object pose The rotation; e2 represents the normal vector of the ground plane in the object coordinate system, and e2 = [0 1 0]. T H p This represents the first and third values ​​extracted from the difference in the transformed normal vector, and... F h () indicates that the object height information is extracted from the transformation matrix, W p It is a diagonal weight matrix used to balance errors, which is adjusted experimentally.

[0112] The uniform velocity model residual e of the object v The construction method is as follows: the assumption of a uniform speed model for the vehicle is used as the prior information of the uniform speed model, and the residual e of the uniform speed model of the object is constructed. v ;

[0113] In this embodiment, the uniform velocity model of the object is used to constrain the object's velocity.

[0114] The uniform velocity model residual e v Represented as:

[0115]

[0116]

[0117]

[0118] in F represents the velocity of the k-th object in the n-th frame; t () indicates the extraction of translation components from the transformation matrix; Let Lie algebra represent the relative rotation of the k-th object in the n-th frame; Represents object pose The inverse of the rotating part; W v This is the diagonal weight matrix for the uniform velocity model; This represents the velocity of the k-th object in the (n-1)-th frame; Let represent the Lie algebra of the relative rotation of the k-th object in the (n-1)-th frame.

[0119] The semantic constraint is: the semantic prior residual e constructed by acquiring semantic prior information. d ;

[0120] The semantic prior residual e d Represented as:

[0121]

[0122] Among them W d The diagonal weight matrix represents the semantic prior constraints. This represents the estimated pose of the k-th object detected in the n-th frame.

[0123] In this implementation, to avoid cumulative errors caused by long-term operation, semantic prior constraints are introduced, using the target detection associated with the object at time t as the observation to provide absolute constraints.

[0124] The loss function is:

[0125]

[0126] Where N K This represents a set of image frames for a sliding window. n-1 M k This represents the map point set in the (n-1)th frame; This represents the normalized coordinates of the associated feature points of the m-th feature point of the k-th object in the sliding window at the n-th frame. This represents the pose of the k-th object in the n-th frame; This represents the pose of the k-th object in the (n-1)-th frame; n π represents the plane parameters estimated in the nth frame.

[0127] In this implementation, the LM algorithm is used to calculate the increment of the loss function by differentiating each residual with respect to the object pose, and this process is iterated until the increment is less than 10. -6 If the optimization result is considered optimal at this point, then this optimization result is used as the optimized pose of the object in the current frame. and speed

[0128] like Figure 5The diagram shows a comparison of the experimental results of this implementation method. The darkest detection box represents the estimated object as the result of target detection, the lightest detection box represents the ground truth object pose, and the detection boxes of intermediate colors represent the object pose estimated by this implementation method using the optimization algorithm. Figure 5 (a)- Figure 5 (c) is a comparison of the object pose estimation results for three consecutive frames of the KITTI Tracking dataset 0003 sequence. It can be seen that the pure object detection result has large directional fluctuations in the estimation results of consecutive frames due to the lack of constraints between adjacent frames. In contrast, the result of this implementation constructs a sliding window and uses a uniform velocity model to effectively smooth the object pose. Furthermore, the maintenance of the object size in this implementation makes the estimated detection box size close to the ground truth size. Figure 5 (d)- Figure 5 (f) is a comparison of the object pose estimation results for three consecutive frames of the KITTI Tracking dataset 0018 sequence. When the object is close to the vehicle, the target detection result is close to the true value. The estimation result of this embodiment can also estimate the object pose well even under the constraint of target detection.

[0129] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope defined by the claims of the present invention.

Claims

1. A multi-object tracking and pose optimization method based on camera parameterization, characterized in that, The method includes the following steps: Step 1: Acquire laser point data and perform 3D target detection using a PointPillar network to obtain M 3D object detection boxes at time t. And extract the object detection box. Object points within the bounding box, by... The extracted object points are transformed to pixel coordinates to generate a continuous object mask. t M k ; Step 2: For an existing object to be detected, predict the 3D bounding box at N time points t. And calculate the 3D predicted detection boxes at N time points t. and M 3D object detection boxes at time t The intersection-union ratio (IoU) is used to construct an affinity matrix, and the Hungarian KM algorithm is used to track objects based on this affinity matrix; Step 3: For successfully tracked objects, extract the new feature points of the object at time t. Use the KLT corner tracking algorithm to associate the feature points of the object at time t-1 with the new feature points extracted at time t to obtain the object points at time t. At the same time, calculate the maintained object size. Step 4: Construct a loss function by building geometric and semantic constraints, build an n-frame sliding window, and optimize the loss function using the Levenberg-Marquardt LM algorithm to obtain the optimal pose of the object at time t. and speed 2. The multi-object tracking and pose optimization method based on camera parameterization according to claim 1, characterized in that, The steps further include: Step 1.1: Acquire laser point data and use a PointPillar network for 3D target detection to obtain M 3D object detection boxes at time t. Includes: object pose and object size Step 1.2: Size the object Expand by α times, then apply the object detection box Extract object points within the area and construct an object point set. t P k , recorded as t P k ={ t p k,i },in t p k,i This indicates the position of an object point within the object detection box; i represents the i-th object point within the object detection box. Step 1.3: Convert the extracted object points to the pixel coordinate system to obtain the object points in the pixel coordinate system; Step 1.4: Place the object detection box Transform to pixel coordinates and generate a binary image of the projected object points based on the object points in the pixel coordinates. Step 1.5: Perform dilation and erosion processing on the binarized image to generate a continuous object mask. t M k .

3. The multi-object tracking and pose optimization method based on camera parameterization according to claim 2, characterized in that, Step 2 further includes: Step 2.1: For an existing object to be detected, acquire its historical data and make predictions based on the acquired historical data to obtain N 3D predicted detection boxes at time t. Step 2.2: Utilize the 3D object detection boxes at M time points t. Construct detection box detection set B d ; Utilizing N 3D prediction detection boxes at time t Construct the detection box prediction set B p ; Step 2.3: Calculate the detection set B of the detection box d 3D object detection box and detection box prediction set B p 3D detection box in 3D intersection-union ratio; Step 2.4: Construct the affinity matrix based on the calculated 3D intersection-union ratio (IU), and solve the affinity matrix using the KM algorithm to obtain the detection set B of the detection box. d and detection box prediction set B p The matching results are used to update the object's pose, and object tracking is achieved by maximizing the affinity matrix.

4. The multi-object tracking and pose optimization method based on camera parameterization according to claim 3, characterized in that, The historical data mentioned in step 2.1 includes: the optimized object pose at time t-1. speed and the size of the maintained object 5. The multi-object tracking and pose optimization method based on camera parameterization according to claim 4, characterized in that, The method for updating the object's pose based on the matching results in step 2.4 is as follows: based on the detection set B of the bounding box... d and detection box prediction set B p The matching results are used to update the pose of the object to be detected at time t for successfully matched bounding boxes; for unmatched bounding boxes, if the bounding box prediction set B... p If an unmatched 3D object detection box exists, the predicted object pose within the 3D predicted detection box is taken as the pose of the object to be detected at time t, and the number of times the object fails to match consecutively is recorded. If the recorded number exceeds a preset threshold, the detection is stopped. When the object is detected, it is removed; if the detection box detects set B d If there is an unmatched 3D object detection box, a new object is created at time t based on the unmatched 3D object detection box, and the initial pose of the object is set.

6. The multi-object tracking and pose optimization method based on camera parameterization according to claim 5, characterized in that, Step 3 further includes: Step 3.1: For a successfully tracked object, obtain the projected laser points of the object based on the projection operation and use them as feature points. Homogenize the feature points and use the homogenization result as the new feature points extracted at time t. Step 3.2: Obtain the feature points of the object at time t-1, and use the corner tracking algorithm KLT to associate the feature points of the object at time t-1 with the new feature points extracted at time t to obtain the object points at time t; Step 3.3: Obtain the number of object points in all 3D object detection boxes in the historical data of this object, and select the average size of the object corresponding to the m frames of 3D object detection boxes with the most object points. Use the obtained average value as the maintained object size.

7. The multi-object tracking and pose optimization method based on camera parameterization according to claim 6, characterized in that, The geometric constraints mentioned in step 4 include: object reprojection residual e z Planar manifold factor residual e p The residual e of the uniform velocity model of the object v ; The object reprojection residual e z The construction method is as follows: based on the feature point information of the matching object obtained in step 3, and based on the rigidity characteristics of the object, construct the object reprojection residual e. z for: Where ρ represents the Huber robust estimator; This represents the normalized coordinates of the associated feature points of the m-th feature point of the k-th object in the sliding window at frame n; π unit () is an operation that transforms 3D coordinate points in the camera coordinate system to normalized coordinates. This represents the pose of the k-th object in the n-th frame; This represents the pose of the k-th object in the (n-1)-th frame; This represents the m-th map point of the k-th object in the (n-1)-th frame; To solve for the L2 norm; Σ n,m It is the covariance matrix of the reprojection error; The planar manifold factor residual e p The construction method is as follows: extract the plane parameters of the camera coordinate system as ground plane information, and construct the plane manifold factor residual e based on the extracted camera coordinate system plane parameters. p ; The plane parameters of the camera coordinate system are denoted as: in δ represents the normal vector at time t; h Indicates the altitude at time t; The planar manifold factor residual e p Represented as: in Represents object pose The rotation; e2 represents the normal vector of the ground plane in the object's coordinate system, and e2 = [0 1 0]. T H p This represents the first and third values ​​extracted from the difference in the transformed normal vector, and... F h () indicates that the object height information is extracted from the transformation matrix, W p It is a diagonal weight matrix used to balance errors; The uniform velocity model residual e of the object v The construction method is as follows: the assumption of a uniform speed model for the vehicle is used as the prior information of the uniform speed model, and the residual e of the uniform speed model of the object is constructed. v , represented as: in F represents the velocity of the k-th object in the n-th frame; t () indicates the extraction of translation components from the transformation matrix; Let Lie algebra represent the relative rotation of the k-th object in the n-th frame; Represents object pose The inverse of the rotating part; W v This is the diagonal weight matrix for the uniform velocity model; This represents the velocity of the k-th object in the (n-1)-th frame; Let represent the Lie algebra of the relative rotation of the k-th object in the (n-1)-th frame.

8. The multi-object tracking and pose optimization method based on camera parameterization according to claim 7, characterized in that, The semantic constraint mentioned in step 4 is: the semantic prior residual e constructed by obtaining semantic prior information. d ; The semantic prior residual e d Represented as: Among them W d The diagonal weight matrix represents the semantic prior constraints. This represents the estimated pose of the k-th object detected in the n-th frame.

9. The multi-object tracking and pose optimization method based on camera parameterization according to claim 8, characterized in that, The loss function mentioned in step 4 is: Where N K This represents a set of image frames for a sliding window. n-1 M k This represents the map point set in the (n-1)th frame; This represents the normalized coordinates of the associated feature points of the m-th feature point of the k-th object in the sliding window at the n-th frame. This represents the pose of the k-th object in the n-th frame; This represents the pose of the k-th object in the (n-1)-th frame; n π represents the plane parameters estimated in the nth frame.

Images