A real-time pose estimation method, device, electronic device, and storage medium

Abstract

The present invention discloses a real-time pose estimation method, device, electronic device, and storage medium. The method includes: initializing and configuring the coordinate reference systems of a depth camera and a lidar; after fusing the three-dimensional point cloud data obtained by the lidar and the image information obtained by the depth camera, determining a target minimum bounding rectangle; selecting several groups of feature points from the target minimum bounding rectangle for pose calculation to determine the target pose information of the target minimum bounding rectangle; and determining the size of the overlapping area between the target minimum bounding rectangle and the true minimum bounding rectangle; finally, based on the size of the overlapping area, determining the similarity between the target minimum bounding rectangle and the true minimum bounding rectangle and the real-time pose estimation result. The present invention uses a lidar and a depth camera to obtain relevant pose information of a target object, realizes the estimation of the position and pose of the target object, can improve the positioning accuracy of the target object, and can be widely applied to the field of computer technology.

Description

Technical Field

[0001] The present invention relates to the field of computer technology, and in particular, to a real-time pose estimation method, device, electronic device, and storage medium. Background Art

[0002] The pose estimation ability of a robot is crucial for target tracking and motion control for the robot to achieve autonomous navigation, and is of great significance for improving the automation level of the robot.

[0003] The Simultaneous Localization and Mapping (SLAM) problem can be described as follows: A robot explores a path from an unknown position in an unknown environment, realizes its own positioning according to position information during movement, and constructs an incremental map on the basis of its own positioning to achieve autonomous positioning and navigation of the robot. The main principle of SLAM is to detect the surrounding environment through sensors on the robot and construct an environmental map while estimating the pose of the robot. Currently, SLAM systems are mainly divided into two types: liDAR-SLAM and Visual-SLAM. Current SLAM methods usually only rely on a single sensor to implement. However, if only a single sensor is used for SLAM, problems such as poor target pose estimation accuracy or poor mapping accuracy caused by sensor failure, error accumulation, or occlusion cannot be solved. Summary of the Invention

[0004] In view of this, embodiments of the present invention provide a real-time pose estimation method, device, electronic device, and storage medium with high accuracy.

[0005] One aspect of embodiments of the present invention provides a real-time pose estimation method, including:

[0006] Initializing and configuring the coordinate reference systems of a depth camera and a lidar;

[0007] Based on the coordinate reference systems of the depth camera and the lidar, obtaining three-dimensional point cloud data of a target object through the lidar, and obtaining image information of the target object through the depth camera;

[0008] After fusing the three-dimensional point cloud data and the image information, determining a target minimum bounding rectangle through a two-dimensional image edge detection algorithm;

[0009] Selecting several groups of feature points from the target minimum bounding rectangle for pose calculation to determine the target pose information of the target minimum bounding rectangle;

[0010] Determine the size of the overlapping area between the target minimum bounding rectangle and the true minimum bounding rectangle according to the target pose information of the target minimum bounding rectangle and the true pose information of the true minimum bounding rectangle;

[0011] Determine the similarity between the target minimum bounding rectangle and the true minimum bounding rectangle according to the size of the overlapping area, and determine the real-time pose estimation result of the target object.

[0012] Optionally, the initialization of the coordinate reference systems of the depth camera and the lidar includes:

[0013] Construct the first coordinate system of the depth camera and the second coordinate system of the lidar;

[0014] Determine the transformation relationship of the target detection points in different coordinate systems according to the geometric relationship between the first coordinate system and the second coordinate system;

[0015] Convert the coordinate information of the target detection points collected by the depth camera to the second coordinate system according to the transformation relationship;

[0016] Determine the rotation and translation relationship between the first coordinate system and the second coordinate system according to the coordinate information of the target detection points in different coordinate systems, complete the joint calibration of the coordinate systems according to the rotation and translation relationship, and determine the transformation relationship between the first coordinate system and the second coordinate system.

[0017] Optionally, based on the coordinate reference systems of the depth camera and the lidar, obtaining the three-dimensional point cloud data of the target object through the lidar and obtaining the image information of the target object through the depth camera includes:

[0018] Obtain the three-dimensional point cloud data obtained by the lidar and the RGB image data obtained by the depth camera;

[0019] Generate a BEV image according to the three-dimensional point cloud data, generate an RGB-D image according to the RGB image data, and project the height information and depth information in the BEV image onto the RGB image plane through multi-sensor joint calibration and coordinate transformation;

[0020] Among them, for the BEV image, slice the image plane based on the point cloud, and then identify the attributes of the pixel points by calculating the density feature and height feature; for the RGB-D image, embed the height information projected by the point cloud into the original RGB image.

[0021] Optionally, selecting several groups of feature points from the target minimum bounding rectangle for pose calculation to determine the target pose information of the target minimum bounding rectangle includes:

[0022] Construct the problem of pose calculation as a non - linear least - squares problem defined algebraically, and solve for the optimal solution of the camera pose;

[0023] According to the constructed least - squares problem, combined with the constructed error function, calculate the least - squares problem and the Jacobian function; wherein, the error function is used to determine the direction of the next optimal iterative estimate of the position and pose increment;

[0024] According to the calculation results of the least - squares problem and the Jacobian function, determine the target pose information of the target minimum bounding rectangle.

[0025] Optionally, determining the size of the overlapping area between the target minimum bounding rectangle and the true minimum bounding rectangle according to the target pose information of the target minimum bounding rectangle and the true pose information of the true minimum bounding rectangle includes:

[0026] According to the target pose information of the target minimum bounding rectangle and the true pose information of the true minimum bounding rectangle, determine the first area of the target minimum bounding rectangle and the second area of the true minimum bounding rectangle;

[0027] Calculate the intersection information and union information between the first area and the second area;

[0028] According to the intersection information and the union information, calculate the intersection - over - union ratio between the target minimum bounding rectangle and the true minimum bounding rectangle, and determine the size of the overlapping area between the target minimum bounding rectangle and the true minimum bounding rectangle according to the intersection - over - union ratio.

[0029] Optionally, the calculation formula of the intersection - over - union ratio is:

[0030]

[0031] wherein, IOU represents the intersection - over - union ratio between the target minimum bounding rectangle and the true minimum bounding rectangle; S dp represents the first area of the target minimum bounding rectangle; S gp represents the second area of the true minimum bounding rectangle; ∩ represents the intersection; ∪ represents the union.

[0032] Another aspect of the embodiments of the present invention also provides a real - time pose estimation device, including:

[0033] A first module for initializing and configuring the coordinate reference systems of the depth camera and the lidar;

[0034] A second module, configured to obtain three-dimensional point cloud data of a target object through the lidar based on the coordinate reference systems of the depth camera and the lidar, and obtain image information of the target object through the depth camera;

[0035] A third module, configured to determine a target minimum bounding rectangle through a two-dimensional image edge detection algorithm after fusing the three-dimensional point cloud data and the image information;

[0036] A fourth module, configured to select several groups of feature points from the target minimum bounding rectangle for pose calculation to determine the target pose information of the target minimum bounding rectangle;

[0037] A fifth module, configured to determine the size of the overlapping area between the target minimum bounding rectangle and the true minimum bounding rectangle according to the target pose information of the target minimum bounding rectangle and the true pose information of the true minimum bounding rectangle;

[0038] A sixth module, configured to determine the similarity between the target minimum bounding rectangle and the true minimum bounding rectangle according to the size of the overlapping area, and determine the real-time pose estimation result of the target object.

[0039] Another aspect of the embodiments of the present invention further provides an electronic device, including a processor and a memory;

[0040] The memory is used to store a program;

[0041] The processor executes the program to implement the method as described above.

[0042] Another aspect of the embodiments of the present invention further provides a computer-readable storage medium, where the storage medium stores a program, and the program is executed by a processor to implement the method as described above.

[0043] The embodiments of the present invention also disclose a computer program product or a computer program, the computer program product or the computer program includes computer instructions, and the computer instructions are stored in a computer-readable storage medium. The processor of the computer device can read the computer instructions from the computer-readable storage medium, and the processor executes the computer instructions, so that the computer device executes the method as described above.

[0044] In an embodiment of the present invention, the coordinate reference systems of the depth camera and the lidar are initialized and configured first. Then, based on the coordinate reference systems of the depth camera and the lidar, the 3D point cloud data of the target object is obtained through the lidar, and the image information of the target object is obtained through the depth camera. After fusing the 3D point cloud data and the image information, the target minimum bounding rectangle is determined by a 2D image edge detection algorithm. Then, several groups of feature points are selected from the target minimum bounding rectangle for pose calculation to determine the target pose information of the target minimum bounding rectangle. And according to the target pose information of the target minimum bounding rectangle and the true pose information of the true minimum bounding rectangle, the size of the overlapping area between the target minimum bounding rectangle and the true minimum bounding rectangle is determined. Finally, according to the size of the overlapping area, the similarity between the target minimum bounding rectangle and the true minimum bounding rectangle is determined, and the real-time pose estimation result of the target object is determined. The present invention uses a lidar and a depth camera to obtain the relevant pose information of the target object, realizes the estimation of the position and pose of the target object, and can improve the positioning accuracy of the target object. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following briefly introduces the drawings required for the description of the embodiments. Obviously, the following drawings are only some embodiments of the present application. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0046] Figure 1 It is a flowchart of the overall steps provided by the embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0047] In order to make the objectives, technical solutions and advantages of the present application more clear, the present application will be further described in detail below with reference to the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0048] In view of the problems existing in the prior art, the embodiment of the present invention provides a real-time pose estimation method, as Figure 1 shown, the method of the present invention generally includes the following steps:

[0049] Initialize and configure the coordinate reference systems of the depth camera and the lidar;

[0050] Based on the coordinate reference systems of the depth camera and the lidar, obtain the 3D point cloud data of the target object through the lidar, and obtain the image information of the target object through the depth camera;

[0051] After fusing the three-dimensional point cloud data and the image information, determine the target minimum bounding rectangle through a two-dimensional image edge detection algorithm;

[0052] Select several groups of feature points from the target minimum bounding rectangle for pose calculation to determine the target pose information of the target minimum bounding rectangle;

[0053] According to the target pose information of the target minimum bounding rectangle and the true pose information of the true minimum bounding rectangle, determine the size of the overlapping area between the target minimum bounding rectangle and the true minimum bounding rectangle; wherein, the true minimum bounding rectangle is a reference value (true value) manually made by the producer of the dataset.

[0054] According to the size of the overlapping area, determine the similarity between the target minimum bounding rectangle and the true minimum bounding rectangle, and determine the real-time pose estimation result of the target object.

[0055] It can be understood that a depth camera can detect the depth distance of the shooting space. The distance of each point in the image from the camera, as well as the two-dimensional coordinates and color information of the point in the 2D image, can be obtained through the depth camera.

[0056] A lidar is a radar system that detects the position, speed and other characteristic quantities of a target by emitting laser beams. Its working principle is to emit a detection signal (laser beam) to the target, and then compare the received signal (target echo) reflected from the target with the emitted signal. After appropriate processing, relevant information of the target can be obtained, such as target distance, azimuth, altitude, speed, attitude, and even shape and other parameters, so as to detect, track and identify targets such as airplanes and missiles.

[0057] Optionally, the initializing and configuring the coordinate reference systems of the depth camera and the lidar includes:

[0058] Construct a first coordinate system of the depth camera and a second coordinate system of the lidar;

[0059] According to the geometric relationship between the first coordinate system and the second coordinate system, determine the transformation relationship of the target detection point in different coordinate systems;

[0060] According to the transformation relationship, convert the coordinate information of the target detection point collected by the depth camera to the second coordinate system;

[0061] According to the coordinate information of the target detection point in different coordinate systems, determine the rotation and translation relationship between the first coordinate system and the second coordinate system. According to the rotation and translation relationship, complete the joint calibration of the coordinate systems and determine the transformation relationship between the first coordinate system and the second coordinate system.

[0062] Optionally, based on the coordinate reference systems of the depth camera and the lidar, obtaining the three-dimensional point cloud data of the target object through the lidar and obtaining the image information of the target object through the depth camera, including:

[0063] Obtaining the three-dimensional point cloud data acquired by the lidar and the RGB image data acquired by the depth camera;

[0064] Generating a BEV image based on the three-dimensional point cloud data, generating an RGB-D image based on the RGB image data, and through multi-sensor joint calibration and coordinate transformation, projecting the height information and depth information in the BEV image onto the RGB image plane together;

[0065] Among them, for the BEV image, slicing the image plane based on the point cloud, and then identifying the attributes of pixel points by calculating density features and height features; for the RGB-D image, embedding the height information projected by the point cloud into the original RGB image.

[0066] It should be noted that the BEV image refers to a top view centered on vision.

[0067] The RGB-D image can be understood as two images: one is an ordinary RGB three-channel color image; the other is a Depth image. The Depth image is similar to a grayscale image, except that each of its pixel values is the actual distance from the sensor to the object. Usually, the RGB image and the Depth image are registered, so there is a one-to-one correspondence between pixel points.

[0068] Therefore, it can be concluded that RGB-D = RGB + Depth Map.

[0069] The RGB color model is a color standard in the industrial community. It obtains various colors through the changes of the three color channels of red (R), green (G), and blue (B) and their superposition with each other. RGB represents the colors of the three channels of red, green, and blue. This standard almost includes all colors that the human vision can perceive and is one of the most widely used color systems at present.

[0070] Depth Map: In 3D computer graphics, a Depth Map (depth map) is an image or image channel that contains information related to the distance of the surface of scene objects from the viewpoint. Among them, the Depth Map is similar to a grayscale image, except that each of its pixel values is the actual distance from the sensor to the object. Usually, the RGB image and the Depth image are registered, so there is a one-to-one correspondence between pixel points.

[0071] The image depth refers to the number of bits used to store each pixel and is also used to measure the color resolution of an image. The image depth determines the number of colors that each pixel of a color image can have, or the number of gray levels that each pixel of a grayscale image can have. It determines the maximum number of colors that can appear in a color image or the maximum gray level in a grayscale image. For example, in a monochrome image, if each pixel has 8 bits, the maximum number of gray levels is 2 to the 8th power, which is 256.

[0072] For a color image, if the pixel bits of the RGB three channels are 4, 4, and 2 respectively, the maximum number of colors is 2 to the power of 4 + 4 + 2, which is 1024. That is to say, the depth of the pixel is 10 bits, and each pixel can be one of 1024 colors.

[0073] For example: The size of a picture is 1024 * 768 and the depth is 16, then its data volume is 1.5M.

[0074] The calculation is as follows:

[0075] 1024 × 768 × 16 bit = (1024 × 768 × 16) / 8 Byte = [(1024 × 768 × 16) / 8] / 1024 KB = 1536 KB = {[(1024 × 768 × 16) / 8] / 1024} / 1024 MB = 1.5 MB.

[0076] Optionally, selecting several groups of feature points from the target minimum bounding rectangle for pose calculation to determine the target pose information of the target minimum bounding rectangle includes:

[0077] Constructing the pose calculation problem as a non - linear least - squares problem defined algebraically and solving for the optimal solution of the camera pose;

[0078] According to the constructed least - squares problem, combined with the constructed error function, calculating the least - squares problem and the Jacobian function; wherein, the error function is used to determine the direction of the next optimal iterative estimate of the position and pose increment;

[0079] According to the calculation results of the least - squares problem and the Jacobian function, determining the target pose information of the target minimum bounding rectangle.

[0080] It should be noted that the non - linear least - squares method is a parameter - estimation method for estimating the parameters of a non - linear static model based on the criterion of minimizing the sum of the squares of the errors. Suppose the model of the non - linear system is y = f(x, θ), which is often used for sensor parameter setting. Here, y is the output of the system, x is the input, and θ is the parameter (they can be vectors). The non - linearity here refers to the non - linear model with respect to the parameter θ, excluding the relationship between the input - output variables changing with time. When estimating the parameters, the form f of the model is known. After N experiments, data (x1, y1), (x2, y2), …, (xn, yn) are obtained. The criterion for estimating the parameters (or the objective function) is selected as the sum of the squares of the errors of the model. The non - linear least - squares method is to find the parameter - estimation value that minimizes Q.

[0081] In vector calculus, the Jacobian matrix is a matrix formed by arranging the first - order partial derivatives in a certain way, and its determinant is called the Jacobian determinant. The importance of the Jacobian matrix lies in that it represents the best linear approximation of a differentiable equation at a given point. Therefore, the Jacobian matrix is similar to the derivative of a multivariate function.

[0082] Optionally, determining the size of the overlapping region between the target minimum bounding rectangle and the true minimum bounding rectangle according to the target pose information of the target minimum bounding rectangle and the true pose information of the true minimum bounding rectangle includes:

[0083] Determine the first area of the target minimum bounding rectangle and the second area of the true minimum bounding rectangle according to the target pose information of the target minimum bounding rectangle and the true pose information of the true minimum bounding rectangle;

[0084] Calculate the intersection information and union information between the first area and the second area;

[0085] According to the intersection information and the union information, calculate the intersection - over - union ratio between the target minimum bounding rectangle and the true minimum bounding rectangle, and determine the size of the overlapping region between the target minimum bounding rectangle and the true minimum bounding rectangle according to the intersection - over - union ratio.

[0086] Optionally, the calculation formula for the intersection - over - union ratio is:

[0087]

[0088] where IOU represents the intersection - over - union ratio between the target minimum bounding rectangle and the true minimum bounding rectangle; S dp represents the first area of the target minimum bounding rectangle; S gp represents the second area of the true minimum bounding rectangle; ∩ represents the intersection; ∪ represents the union.

[0089] Another aspect of the embodiments of the present invention further provides a real-time pose estimation device, including:

[0090] A first module for initializing and configuring the coordinate reference systems of a depth camera and a lidar;

[0091] A second module for obtaining three-dimensional point cloud data of a target object through the lidar and obtaining image information of the target object through the depth camera based on the coordinate reference systems of the depth camera and the lidar;

[0092] A third module for determining a target minimum bounding rectangle through a two-dimensional image edge detection algorithm after fusing the three-dimensional point cloud data and the image information;

[0093] A fourth module for selecting several groups of feature points from the target minimum bounding rectangle for pose calculation to determine the target pose information of the target minimum bounding rectangle;

[0094] A fifth module for determining the size of the overlapping area between the target minimum bounding rectangle and the true minimum bounding rectangle according to the target pose information of the target minimum bounding rectangle and the true pose information of the true minimum bounding rectangle;

[0095] A sixth module for determining the similarity between the target minimum bounding rectangle and the true minimum bounding rectangle according to the size of the overlapping area and determining the real-time pose estimation result of the target object.

[0096] Another aspect of the embodiments of the present invention further provides an electronic device, including a processor and a memory;

[0097] The memory is used for storing programs;

[0098] The processor executes the program to implement the method as described above.

[0099] Another aspect of the embodiments of the present invention further provides a computer-readable storage medium, where the storage medium stores a program, and the program is executed by a processor to implement the method as described above.

[0100] The embodiments of the present invention also disclose a computer program product or a computer program. The computer program product or the computer program includes computer instructions, and the computer instructions are stored in a computer-readable storage medium. The processor of the computer device can read the computer instructions from the computer-readable storage medium, and the processor executes the computer instructions to enable the computer device to execute the method as described above.

[0101] The following details the specific implementation process of the real-time pose estimation method of the present invention:

[0102] First of all, it should be noted that positioning and navigation are key technologies for autonomous mobile service robots, and building positioning and mapping is considered to be a necessary foundation for realizing this function. The main principle of SLAM is to detect the surrounding environment through sensors on the robot and build an environmental map while estimating the pose of the robot. Currently, SLAM systems are mainly divided into two types: LiDAR-SLAM and Visual-SLAM. If only a single sensor is used for SLAM, the positioning accuracy is low.

[0103] Simultaneous Localization and Mapping (SLAM), also known as CML (Concurrent Mapping and Localization), is a technical term. The SLAM problem can be described as follows: The robot starts moving from an unknown position in an unknown environment, locates itself based on its position and the map during movement, and simultaneously builds an incremental map based on its self-localization to achieve the robot's autonomous positioning and navigation.

[0104] Aiming at the problems existing in the prior art, the present invention provides a method for real-time pose estimation. In a specific application scenario, a complementary system solution is constructed based on a lidar and supplemented by multi-sensors such as a depth camera RGB-D and an IMU, including the following steps:

[0105] S1: Initialize the coordinate reference systems of the RGB-D and the lidar. Specifically, define a world (global) coordinate system. For the sake of simplifying the calculation, assume that the reference system of the mobile robot is the same as that of the lidar. LiDAR and the camera detect objects in different data forms. Assume that point P is the target point. The coordinates of point P in the lidar coordinate system (O L , X L , Y L , Z L ) are (X L , Y L , Z L ), the coordinates of point P in the camera coordinate system (Oc, Xc, Yc, Zc) are (Xc, Yc, Zc), and the projected coordinates of point P in the plane coordinate system (X p O p Y P ) are P, (u, v). The coordinates of point P collected by the lidar are not (xL, γL, zL), but the distance r and the angle α; the coordinates of point P collected by the camera are not (xc, γc, zc), but the projected coordinates (u, v) and the corresponding depth information (Zc).

[0106] According to the LiDAR coordinate system (O L , XL , Y L , Z L ) With the geometric relationship with the camera coordinate system (Oc, Xc, Yc, Zc), the transformation relationship of point P in different coordinate systems can be obtained, as shown in Equation 1.

[0107]

[0108] Among them, R is the rotation array and T is the translation array. For the classical model of the camera, RGB-D can collect the coordinate data P, (u, v) and the depth information Zc of the camera. Among them, P is the projection of point P on the image plane. The relationship between the coordinate system of the camera and the data u, v, z collected by the LiDAR is shown in Equation 2.

[0109]

[0110] Among them, in Equation (2), f x , f y are the equivalent focal lengths (in pixel units) on the x-axis and y-axis respectively. c x , c y are the reference points on the x-axis and y-axis respectively, and they both belong to the intrinsic parameters of the camera; (u, v) is the projection point of the target point P on the image plane. According to Equations (1) and (2), this embodiment can convert the coordinate information collected by the camera into the coordinates in the LiDAR coordinate system. Here, it is necessary to keep the origin of coordinates of the LiDAR and the camera on the Y-axis. Let the vertical height difference be h, as shown in Equation 3.

[0111] z L＝r cosa x L = r sina

[0112]

[0113] After determining the camera parameters f x , f y , c x , c y ), take multiple camera data u, v, z, and the scanning radius r of the lidar, and α into Formula (3), where α is the angle with the x-axis and y-axis; by solving the linear equations, the matrices R and T are obtained, and then the rotation and translation relationship between the lidar and the camera coordinate systems is completed for joint calibration, and then the transformation relationship between the camera and the lidar coordinate systems is determined and calibration is completed.

[0114] S2: The lidar measures the shape and contour of the object to generate three-dimensional point cloud data. The camera acquires the image information of the object. The image and the point cloud data are fused, and then the minimum circumscribed rectangle of the target is obtained through the two-dimensional image edge detection algorithm, and several groups of feature points are selected in the polygon for pose calculation.

[0115] Specifically, in this embodiment, the lidar point cloud and the RGB image are used as input data. BEV and RGB-D images can be obtained. Through multi-sensor joint calibration and coordinate transformation, the height information of the three-dimensional lidar point cloud is projected onto the RGB image plane together with its depth information. Therefore, for BEV, the image plane is sliced based on the point cloud, and then the attributes of the pixel points (x, y, z) are identified by calculating the density feature and the height feature. For the RGB-D image, only the height information projected by the point cloud needs to be embedded into the original RGB image.

[0116] The whole process is divided into three steps:

[0117] First, map the point cloud (X, Y, Z) to the original image (W, H) plane, as shown in Equation 4.

[0118] (uv1) T = M(XYZ1) T

[0119]

[0120] where (u, v) are the image coordinates, P roj is the projection matrix, is the rotation matrix from LiDAR to camera, is the translation vector, and M is the homogeneous transformation matrix from LiDAR to camera. In this embodiment, the LiDAR point cloud coordinates (X, Y, Z) are mapped to the W×H image plane through formula (4) to solve the projection matrix M; where the T in (uv1) T is the transpose of the matrix.

[0121] Secondly, retain the points {(x, y, z)|x ∈ X, y ∈ Y, z ∈} that are within the image size W*H. At the same time, project the LiDAR points onto the camera coordinates, denoted as (x c , y c , z c ). As shown in Equation 5.

[0122] (x c y c z c ) T = M·(xyz1) T (5)

[0123] Finally, map z c to between 0 and 255, and then assign it to the corresponding image coordinates (u, v). The formula (5) in this embodiment is the derivation of formula (4), (xc, yc, zc) are the camera coordinates, and the point {(x, y, z)|x ∈ X, y ∈ Y, z ∈ Z} is a point coordinate of the point cloud.

[0124] S3. Construct the PnP problem as a non - linear least - squares problem defined algebraically to solve for the optimal solution of the camera pose, as shown in Equation 6.

[0125]

[0126] Where, δ is the Lie algebra; r(δ) is the residual (predicted value - observed value), is the predicted value, and u is the observed value.

[0127] S4. The constructed least - squares problem is as shown in Equation 7 to solve the least - squares problem and the Jacobian function.

[0128]

[0129] In the embodiment of the present invention, by constructing a least - squares problem and using the Jacobi function, the Jacobi expression J as shown in formula (8) can be obtained.

[0130] S5. The error function determines the direction of the next optimal iterative estimate of the position and pose increment. According to the above process of pose transformation, this embodiment can represent j0 using the chain rule of formula 8.

[0131]

[0132] It can be seen from the above calculations that the Jacobian matrices of the direct method and the feature - point method are only different in j0. The specific derivation steps can be seen in the error function of the pose Jacobian matrix during SLAM optimization. The result is as shown in Equation 9.

[0133]

[0134] S6: Use IOU to evaluate the detected polygon (S dp ) and the true minimum bounding rectangle (S gp ) divided by the overlapping area of the union (S dp ) and (S gp ). The calculation process is as shown in Equation 10.

[0135]

[0136] If the IoU between the detected polygon and the true minimum bounding rectangle is greater than 0.5, then the polygon is considered to be the same object; otherwise, it is not.

[0137] In summary, the real-time pose estimation method of the present invention combines the advantages of RGB-D and lidar, and is different from the existing two-stage framework or multi-stage pipeline methods. After fusing the image data and the original point cloud data, this solution uses the spatial anchors of the input 3D point cloud as key points to predict the pose between two consecutive frames. Then, it uses a CNN to identify and extract 3D bounding boxes, tracks the target object projected onto the RGB image to obtain the target MBR, and then calculates the rotation angle and translation distance of the target centroid using geometric methods. Finally, it combines with an IMU for joint optimization to achieve the estimation of position and pose, improving the positioning accuracy of the model.

[0138] In some alternative embodiments, the functions / operations mentioned in the block diagrams may not occur in the order shown in the operation diagrams. For example, depending on the functions / operations involved, two consecutive blocks shown may actually be executed substantially simultaneously or the blocks can sometimes be executed in the reverse order. In addition, the embodiments presented and described in the flowcharts of the present invention are provided by way of example for the purpose of providing a more comprehensive understanding of the technology. The disclosed methods are not limited to the operations and logical flows presented herein. Alternative embodiments are contemplated where the order of various operations is changed and where sub-operations described as part of a larger operation are executed independently.

[0139] Furthermore, although the present invention has been described in the context of functional modules, it should be understood that, unless otherwise stated to the contrary, one or more of the functions and / or features described may be integrated in a single physical device and / or software module, or one or more functions and / or features may be implemented in separate physical devices or software modules. It can also be understood that a detailed discussion of the actual implementation of each module is not necessary for understanding the present invention. Rather, considering the attributes, functions, and internal relationships of the various functional modules in the devices disclosed herein, the actual implementation of the modules will be understood within the ordinary skills of an engineer. Therefore, those skilled in the art can implement the present invention as set forth in the claims without undue experimentation. It can also be understood that the specific concepts disclosed are illustrative only and are not intended to limit the scope of the present invention, which is determined by the full scope of the appended claims and their equivalents.

[0140] If the above-mentioned functions are implemented in the form of software function units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or a part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in various embodiments of the present invention. The foregoing storage medium includes: various media that can store program codes, such as USB flash drives, mobile hard disks, read-only memories (ROMs, Read-Only Memories), random access memories (RAMs, Random Access Memories), magnetic disks, or optical discs.

[0141] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a definite sequence list of executable instructions for implementing logical functions, and can be specifically implemented in any computer-readable medium for use by an instruction execution system, apparatus, or device (such as a computer-based system, a system including a processor, or other systems that can fetch and execute instructions from the instruction execution system, apparatus, or device), or in combination with these instruction execution systems, apparatus, or devices. For the purposes of this specification, a "computer-readable medium" can be any device that can contain, store, communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device.

[0142] More specific examples (non-exhaustive list) of computer-readable media include the following: an electrical connection portion (electronic device) having one or more wirings, a portable computer diskette case (magnetic device), random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber device, and portable compact disc read-only memory (CDROM). Additionally, the computer-readable medium can even be paper or other suitable media on which the program can be printed, because the program can be obtained electronically, for example, by optically scanning the paper or other media, then editing, interpreting, or otherwise processing it as appropriate, and then storing it in a computer memory.

[0143] It should be understood that various parts of the present invention can be implemented by hardware, software, firmware, or a combination thereof. In the above embodiments, multiple steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented by hardware, as in another embodiment, any one or a combination of the following techniques well known in the art can be used: discrete logic circuits having logic gate circuits for implementing logic functions on data signals, application specific integrated circuits having appropriate combinational logic gate circuits, programmable gate arrays (PGAs), field programmable gate arrays (FPGAs), etc.

[0144] In the description of this specification, the descriptions referring to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples", etc. mean that the specific features, structures, materials, or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.

[0145] Although the embodiments of the present invention have been shown and described, those of ordinary skill in the art can understand that various changes, modifications, substitutions, and variations can be made to these embodiments without departing from the principles and spirit of the present invention, and the scope of the present invention is defined by the claims and their equivalents.

[0146] The above has specifically described the preferred embodiments of the present invention, but the present invention is not limited to the described embodiments. Those skilled in the art can also make various equivalent deformations or substitutions without departing from the spirit of the present invention, and these equivalent deformations or substitutions are all included within the scope defined by the claims of this application.

Claims

1. A real-time pose estimation method, characterized in that, Including: Initializing and configuring the coordinate reference systems of the depth camera and the lidar; Based on the coordinate reference systems of the depth camera and the lidar, obtaining the three-dimensional point cloud data of the target object through the lidar, and obtaining the image information of the target object through the depth camera; After fusing the three-dimensional point cloud data and the image information, determining the target minimum bounding rectangle through a two-dimensional image edge detection algorithm; Selecting several groups of feature points from the target minimum bounding rectangle for pose calculation to determine the target pose information of the target minimum bounding rectangle; According to the target pose information of the target minimum bounding rectangle and the true pose information of the true minimum bounding rectangle, determining the size of the overlapping area between the target minimum bounding rectangle and the true minimum bounding rectangle; According to the size of the overlapping area, determining the similarity between the target minimum bounding rectangle and the true minimum bounding rectangle, and determining the real-time pose estimation result of the target object.

2. The real-time pose estimation method according to claim 1, characterized in that The initializing and configuring the coordinate reference systems of the depth camera and the lidar includes: Constructing the first coordinate system of the depth camera and the second coordinate system of the lidar; According to the geometric relationship between the first coordinate system and the second coordinate system, determining the transformation relationship of the target detection point in different coordinate systems; According to the transformation relationship, converting the coordinate information of the target detection point collected by the depth camera to the second coordinate system; According to the coordinate information of the target detection point in different coordinate systems, determining the rotation and translation relationship between the first coordinate system and the second coordinate system, completing the joint calibration of the coordinate systems according to the rotation and translation relationship, and determining the transformation relationship between the first coordinate system and the second coordinate system.

3. A real-time pose estimation method according to claim 1, characterized in that, The obtaining the three-dimensional point cloud data of the target object through the lidar and obtaining the image information of the target object through the depth camera based on the coordinate reference systems of the depth camera and the lidar includes: Obtaining the three-dimensional point cloud data obtained by the lidar and the RGB image data obtained by the depth camera; Generating a BEV image according to the three-dimensional point cloud data, generating an RGB-D image according to the RGB image data, and projecting the height information and depth information in the BEV image onto the RGB image plane through multi-sensor joint calibration and coordinate transformation; Among them, for the BEV image, slicing the image plane based on the point cloud, and then identifying the attributes of the pixel points by calculating the density feature and the height feature; for the RGB-D image, embedding the height information projected by the point cloud into the original RGB image.

4. A real-time pose estimation method according to claim 1, characterized in that, The selecting several groups of feature points from the target minimum bounding rectangle for pose calculation to determine the target pose information of the target minimum bounding rectangle includes: Constructing the pose calculation problem as a non-linear least squares problem defined algebraically and solving the optimal solution of the camera pose; According to the constructed least squares problem, combining the constructed error function, calculating the least squares problem and the Jacobian function; wherein, the error function is used to determine the direction of the next optimal iterative estimate of the position and pose increment. Determine the target pose information of the target minimum bounding rectangle according to the calculation results of the least square problem and the Jacobi function.

5. A real-time pose estimation method according to claim 1, characterized in that The determining the size of the overlapping area between the target minimum bounding rectangle and the true minimum bounding rectangle according to the target pose information of the target minimum bounding rectangle and the true pose information of the true minimum bounding rectangle includes: Determine the first area of the target minimum bounding rectangle and the second area of the true minimum bounding rectangle according to the target pose information of the target minimum bounding rectangle and the true pose information of the true minimum bounding rectangle; Calculate the intersection information and union information between the first area and the second area; According to the intersection information and the union information, calculate the intersection over union between the target minimum bounding rectangle and the true minimum bounding rectangle, and determine the size of the overlapping area between the target minimum bounding rectangle and the true minimum bounding rectangle according to the intersection over union.

6. A real-time pose estimation method according to claim 5, wherein The calculation formula of the intersection over union is: Among them, IOU represents the intersection over union between the target minimum bounding rectangle and the ground truth minimum bounding rectangle; S dp represents the first area of the target minimum bounding rectangle; S gp represents the second area of the ground truth minimum bounding rectangle; ∩ represents the intersection; ∪ represents the union.

7. A real-time pose estimation device, characterized in that, including: A first module for initializing and configuring the coordinate reference systems of the depth camera and the lidar; A second module for obtaining the three-dimensional point cloud data of the target object through the lidar and obtaining the image information of the target object through the depth camera based on the coordinate reference systems of the depth camera and the lidar; A third module for determining the target minimum bounding rectangle through a two-dimensional image edge detection algorithm after fusing the three-dimensional point cloud data and the image information; A fourth module for selecting several groups of feature points from the target minimum bounding rectangle for pose calculation to determine the target pose information of the target minimum bounding rectangle; A fifth module for determining the size of the overlapping area between the target minimum bounding rectangle and the true minimum bounding rectangle according to the target pose information of the target minimum bounding rectangle and the true pose information of the true minimum bounding rectangle; A sixth module for determining the similarity between the target minimum bounding rectangle and the true minimum bounding rectangle according to the size of the overlapping area and determining the real-time pose estimation result of the target object.

8. An electronic device, characterized in that, including a processor and a memory; The memory is used for storing programs; The processor executes the program to implement the method according to any one of claims 1 to 6.

9. A computer-readable storage medium, characterized in that, The storage medium stores a program, and the program is executed by the processor to implement the method according to any one of claims 1 to 6.

10. A computer program product, comprising a computer program, characterized in that, The computer program, when executed by the processor, implements the method according to any one of claims 1 to 6.

Images