Vehicle attitude estimation method and system based on depth camera and point cloud processing

Abstract

The invention relates to the field of artificial intelligence, in particular to a vehicle attitude estimation method and system based on a depth camera and point cloud processing, and the method comprises the steps: calibrating internal and external parameters of the depth camera, collecting and preprocessing point cloud data of a vehicle and a surrounding environment under different visual angles, reconstructing dense point cloud of the vehicle, and carrying out the registration of the dense point cloud and a vehicle perfect model. The precise three-dimensional position, attitude information and the like of the vehicle are output based on the ICP registration result; compared with the prior art, the method has the advantages that through combination of the depth camera and point cloud processing, the problems of noise and environmental sensitivity existing in a traditional method are solved, and high-precision vehicle attitude estimation is achieved; the method can provide high-precision vehicle attitude estimation for an automatic spraying system, is suitable for spraying robots or automatic spraying devices driven by artificial intelligence, and has wide application prospects in the fields of intelligent manufacturing, automobile coating and the like.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of artificial intelligence, and in particular to a vehicle pose estimation method and system based on a depth camera and point cloud processing. BACKGROUND

[0002] Vehicle pose estimation is one of the key technologies in the fields of autonomous driving, intelligent manufacturing, and embodied intelligence, aiming to obtain the position, orientation, speed, and other information of a vehicle through sensors. It assists in achieving high-precision automation control, ensuring the safety and efficiency of production and control processes, and promoting the progress of industrial robots, autonomous driving technology, and intelligent heavy industrial vehicles. Traditional vehicle pose estimation methods mainly rely on vision, lidar, and inertial measurement units (IMU), but these methods have certain limitations. For example, monocular vision-based methods are relatively low-cost, but they require high camera calibration and environmental lighting conditions, are easily affected by environmental textures and lighting changes, and lack depth information, making it difficult to accurately estimate the distance and pose of the vehicle. Lidar-based methods can provide high-precision point cloud data, but high-precision lidars are expensive and bulky, making them difficult to popularize in ordinary vehicles, while low-precision lidars are less expensive but their point cloud data is sparse, making it difficult to meet the needs of high-precision pose estimation. In addition, IMU-based methods can provide vehicle pose information, but they have cumulative error problems, and the accuracy will decrease significantly after long-term use.

[0003] There are already many vehicle registration and positioning solutions on the market, such as the invention patent "Unmanned vehicle GNSS positioning reliability evaluation method based on point cloud registration" (CN111751852A) applied by Beijing United University, which is used for real-time positioning of unmanned vehicles. However, it relies on laser radar to collect point cloud data, which is different from our pure visual solution. Depth cameras can obtain real-time depth information of the scene at a lower cost and generate dense point cloud data, which provides new possibilities for vehicle pose estimation. By combining point cloud processing technology and deep learning algorithms, the geometric features of the vehicle can be extracted from the point cloud data, and high-precision pose estimation can be achieved. Traditional methods have deficiencies in cost, accuracy, and real-time performance. The method based on depth cameras and point cloud processing combines the low-cost advantage of depth cameras and the high-precision potential of point cloud processing, providing a new solution for vehicle pose estimation. SUMMARY

[0004] I. Technical problems solved

[0005] In order to solve the technical problems of the existing vehicle posture estimation, the application provides a vehicle posture estimation method and system based on a depth camera and point cloud processing, which can obtain high-precision position and posture information of the vehicle in space according to multi-view vehicle point cloud data collected by the depth camera.

[0006] II. Technical solutions

[0007] To solve the above technical problems, the application provides a technical solution: a vehicle posture estimation method and system based on a depth camera and point cloud processing, comprising the following steps:

[0008] Step 1: Calibrate the depth camera, calibrate the internal and external parameters of the depth camera using a known calibration board or standard object, eliminate the distortion of the camera, and obtain the spatial position information of the camera through hand-eye calibration to ensure the accuracy of the subsequent point cloud data.

[0009] Step 2: Use the depth camera to collect real-time point cloud data of the vehicle and the surrounding environment under multi-view. The point cloud data includes three-dimensional information of the vehicle itself and its surrounding environment, ensuring the acquisition of high-quality depth map and point cloud data.

[0010] Step 3: Preprocess the collected point cloud data, including point cloud denoising and segmentation, detect the vehicle area by using a depth target detection network, and crop the point cloud according to the target area to remove irrelevant background point clouds.

[0011] Step 4: Use multi-view point cloud data reconstruction technology to align point cloud data from different views to generate a more complete three-dimensional point cloud model. By splicing multi-view point clouds, complete environment and high-density point cloud reconstruction around the vehicle are realized.

[0012] Step 5: Register the reconstructed point cloud data with the perfect vehicle model, and use the ICP (Iterative Closest Point) algorithm for accurate alignment to ensure accurate registration of the vehicle reconstruction point cloud and the perfect vehicle model.

[0013] Step 6: According to the ICP registration result, calculate the rigid transformation matrix of the vehicle, and finally output the accurate three-dimensional position and posture information and local geometric features of the vehicle to ensure accurate positioning and spray path planning in the spraying operation.

[0014] As an improvement: the hand-eye calibration of the depth camera in step 1 is a key technology in robot vision systems, which is used to determine the relative position and attitude relationship between the camera and the mechanical arm. By fixing the camera on the mechanical arm or spraying device, and using known calibration objects (such as calibration boards or specific geometric objects) for registration, the relationship between the camera coordinate system and the spraying device coordinate system (mechanical arm coordinate system) is ensured.

[0015] The hand-eye calibration step includes:

[0016] Step 1-1: Camera and robot pose collection:

[0017] With the known position of the object, record the images of the camera at different angles and positions, and simultaneously collect the end effector position and pose of the robot (such as obtained through encoders or sensors).

[0018] Step 1-2: Coordinate system conversion matrix calculation:

[0019] By solving the conversion matrix between the camera coordinate system and the robot coordinate system, the relationship between the two is established, i.e. the hand-eye calibration matrix, which can convert the point cloud data in the robot coordinate system to the camera coordinate system.

[0020] Step 1-3: Calibration accuracy optimization:

[0021] By minimizing the error function (e.g. re-projection error or point cloud overlap), the hand-eye calibration matrix is optimized to ensure the accuracy of the coordinate conversion between the camera and the robot.

[0022] As an improvement: the point cloud information of the vehicle at different angles in Step 2 is obtained by controlling the depth camera of the robot to take pictures at specified points in space, and finally obtaining the depth map and color image at each angle point.

[0023] As an improvement: the point cloud data preprocessing in Step 3 includes the following steps:

[0024] Step 3-1: Based on the pre-trained target detection network YOLOv8, the target detection is performed on the color image collected at the current point, and the vehicle region Car box is detected. car ,

[0025]

[0026] Step 3-2: Preprocess the original collected depth map Depth origin , only consider the depth map data within the vehicle region Car box , and obtain the final depth map Depth final = mask car *Depth origin .

[0027] Step 3-3: Combine the calibrated camera internal and external parameters in S1 to convert the depth map Depth final into point cloud, and obtain the point cloud data of the vehicle at a single angle.

[0028] As an improvement: the step 4 multi-view point cloud data reconstruction comprises the following steps:

[0029] Step 4-1: Coordinate conversion is performed on each view point cloud data to convert it to a unified global coordinate system.

[0030] Step 4-2: Multi-view point cloud data is spliced and fused to generate a complete point cloud model.

[0031] Step 4-3: The fused point cloud is down-sampled and simplified to reduce data volume and improve subsequent processing efficiency.

[0032] As an improvement: Step 5: The multi-view reconstructed point cloud data is registered with the perfect vehicle model point cloud data.

[0033] As an improvement: Step 5: The step of point cloud registration based on ICP (Iterative Closest Point) algorithm.

[0034] Step 5-1: Initialization:

[0035] The source point cloud is the reconstructed vehicle point cloud to be registered, and the target point cloud is the perfect vehicle model point cloud; during initialization, it is assumed that the relative transformation of the source point cloud and the target point cloud is a unit matrix (i.e., there is no rotation or translation).

[0036] Step 5-2: Nearest neighbor matching:

[0037] For each point in the source point cloud, find the point in the target point cloud that is closest to that point. The Euclidean distance is usually used to measure the distance between two points.

[0038] The corresponding point pair (p i , q i ) in the two point clouds, where p i is a point in the source point cloud and q i is a point in the target point cloud, and the Euclidean distance d i is calculated as follows:

[0039] d i = ||p i -q i ||

[0040] Where ||·|| is the Euclidean norm.

[0041] Step 5-3: Transformation calculation:

[0042] In point cloud registration, the goal is to transform the source point cloud P into the coordinate system of the target point cloud Q through a rotation matrix R and a translation vector t, so that the Euclidean distance between the corresponding point pairs is minimized, and the optimal rigid transformation (including the rotation matrix and the translation vector) is calculated according to the nearest neighbor matching point pair. The commonly used method is to minimize the point pair error between the source point cloud and the target point cloud, and to calculate by least squares method;

[0043] The objective function can be expressed as:

[0044]

[0045] Where r is a 3*3 rotation matrix, t is a 3*1 translation vector, p i is a point in the source point cloud, q i is the corresponding point in the target point cloud corresponding to p i , and ||·|| is the Euclidean norm.

[0046] Step 5-4: Apply the transformation:

[0047] Apply the calculated rotation matrix and translation vector to the source point cloud to obtain a new transformed source point cloud, and observe the difference between the transformed source point cloud and the target point cloud.

[0048] Step 5-5: Check the convergence condition:

[0049] Calculate the registration error of the current iteration (the sum of the Euclidean distances between all point pairs in the transformed source point cloud and the target point cloud), if the error is less than the preset threshold δ, or the maximum iteration number T max , it is considered that the registration process has converged, and the iteration is stopped; otherwise, return to S52 and continue the next iteration.

[0050] As an improvement: the step 6 is specifically: according to the ICP registration result, a rigid transformation matrix (including rotation and translation) of the vehicle is calculated, which can accurately align the source reconstructed vehicle point cloud to the target perfect vehicle model point cloud, and finally output the accurate three-dimensional position and attitude information of the vehicle in space.

[0051] III. Advantages

[0052] Compared with the prior art, the advantages of the present application are:

[0053] 1. The method proposed in the present application has excellent system stability. The prior knowledge of vehicle segmentation is introduced in the point cloud data preprocessing stage, effectively removing noise points and enhancing the robustness of the system in complex environments. The depth camera can work stably under various lighting conditions, and combined with point cloud preprocessing technology, it can effectively cope with the challenges of complex environments (such as night, weak light).

[0054] 2、The method disclosed by the application is completely realized based on a depth camera, does not need to depend on expensive laser radar or other sensors, greatly reduces the hardware cost, and through algorithm optimization of the multi-view point cloud fusion technology, the consumption of computing resources is reduced, high-density point cloud reconstruction is realized, and the balance between cost and performance is considered.

[0055] 3、The method disclosed by the application has the advantages of high precision, error control in the millimeter level, and application in the scene of industrial-grade automobile intelligent maintenance with high precision requirements. BRIEF DESCRIPTION OF DRAWINGS

[0056] Figure 1 A vehicle posture estimation method and system flow schematic diagram based on a depth camera and point cloud processing.

[0057] Figure 2 A camera hand-eye calibration calibration board schematic diagram.

[0058] Figure 3 A camera multi-view point cloud acquisition schematic diagram.

[0059] Figure 4 A vehicle region detection schematic diagram.

[0060] Figure 5 A point cloud registration flowchart. DETAILED DESCRIPTION

[0061] The application will be further described in detail below with reference to the drawings.

[0062] To solve the above technical problems, the technical scheme provided by the application is as follows: Figure 1 As shown in the figure, a vehicle posture estimation method and system based on a depth camera and point cloud processing are specifically divided into the following six steps:

[0063] Step 1, control the mechanical arm to perform hand-eye calibration of the depth camera, and acquire accurate spatial three-dimensional information of the depth camera;

[0064] In this embodiment, the camera is on the mechanical arm, that is, the eye is on the hand (eye on hand), in this calibration mode, the camera is fixed on the end flange of the mechanical arm, and the relative position of the camera and the mechanical arm flange is fixed, as shown in the figure, the calibration board used for calibration is a chessboard with 8 rows and 11 columns, and each grid is a square with a side length of 15mm. Figure 2

[0065] The hand-eye calibration of the depth camera in this embodiment is specifically as follows:

[0066] ​Step 1-1, Camera and mechanical arm pose collection: After placing the calibration board, collect the images of the calibration board taken by the camera at each pose, ensure that the calibration board is clearly visible in the image, and simultaneously collect the end effector position and pose of the mechanical arm (such as through encoder or sensor acquisition);

[0067] Step 1-2, Corner point detection: Use the corner point detection tool of OpenCV to detect the corner points of the calibration board, and calculate the pose of the calibration board relative to the camera according to the corner points;

[0068] Step 1-3, Calibration accuracy optimization: Define the coordinate systems of the mechanical arm base, end, camera and calibration board, minimize the re-projection error, specifically, project the 3D points of the calibration board onto the image, calculate the pixel error with the detected points, optimize the hand-eye calibration matrix, and ensure the coordinate transformation accuracy between the camera and the mechanical arm;

[0069] Step 2, as shown in Figure 3 In this embodiment, after the vehicle is placed in a reasonable area indoors, we plan 8 points for multi-view data collection, and the 8 views in the figure basically surround the vehicle for omnidirectional information collection, which can stably represent the complete information of the vehicle. Specifically, the depth camera controlled by the mechanical arm takes pictures at designated points in space in turn, and finally obtains the depth map and color image at each view point;

[0070] Step 3, preprocess the obtained vehicle point cloud data, segment the point cloud data based on the vehicle region prior obtained by color image semantic segmentation, and filter the point cloud data to eliminate abnormal information interference;

[0071] Target detection algorithms based on deep learning are widely used in the industry due to their efficiency and accuracy. YOLOv8 is the latest stable version of the YOLO (You Only Look Once) series iteration, which has significant advantages in target detection accuracy and speed, and is suitable for real-time vehicle detection tasks;

[0072] Optionally, a deep network or large model such as Transformer, DETR, etc. can also be used to construct a target detector;

[0073] In this embodiment, we use the YOLOv8 model pre-trained on the COCO dataset to detect the target in the collected color image, as shown in Figure 4 The input color image has a resolution of 1280*960, and the output class is the vehicle region detection rectangle Car box , thus obtaining the vehicle mask mask car :

[0074]

[0075] In this embodiment, the collected depth image Depth origin The size of the vehicle mask mask car The size of the 0-1 matrix is (1280, 960), and the dot product operation is directly performed on the two, and the depth image considering only the vehicle part is obtained: Depth final = mask car * depth origin ;

[0076] Step 4, obtain the vehicle point cloud data of each view, and splice and align the point cloud data of each view to obtain a complete vehicle point cloud;

[0077] Specifically, the following steps are included:

[0078] Step 4-1, coordinate conversion is performed on the point cloud data of each view to convert it to a unified global coordinate system;

[0079] Step 4-2, multi-view point cloud data is spliced and fused to generate a complete point cloud model;

[0080] Step 4-3, the fused point cloud is down-sampled and simplified to reduce the data amount and improve the subsequent processing efficiency;

[0081] Since the reconstructed complete point cloud contains all the feature information of multiple views, the point cloud quantity is large, and there are a large number of redundant point clouds, which need to be down-sampled. In this embodiment, uniform sampling is used to down-sample the reconstructed point cloud. The point cloud space is divided into uniform grids by uniform sampling, and the desired point cloud density, i.e., the number of points in each cubic cell, is set. In this embodiment, the point cloud density is set to 1000, and the total number of reconstructed point clouds is finally controlled to about 100000. After multiple experiments, the reconstructed point cloud of this size can balance the registration accuracy and registration efficiency of the subsequent registration;

[0082] Step 5, register the reconstructed point cloud data with the perfect vehicle model, and use the ICP (Iterative Closest Point) algorithm for accurate alignment to ensure accurate registration of the vehicle reconstructed point cloud and the perfect vehicle model;

[0083] The specific process of this embodiment is shown in Figure 5 In the point cloud registration process, the goal is to transform the source point cloud P to the coordinate system of the target point cloud Q through the rotation matrix R and the translation vector t, so that the Euclidean distance between the corresponding point pairs is minimized. According to the nearest neighbor matching point pair, the optimal rigid transformation (including the rotation matrix R and the translation vector t) is calculated. The commonly used method is to minimize the point pair error between the source point cloud and the target point cloud, and the least squares method is used for calculation;

[0084] The objective function can be expressed as:

[0085]

[0086] In the formula, R is a 3*3 rotation matrix, t is a 3*1 translation vector, p i is a point in the source point cloud, q i is a point in the target point cloud corresponding to p i , and ||·|| is the Euclidean norm.

[0087] In this embodiment, according to multiple experiments, the convergence number of ICP (Iterative Closest Point) algorithm iteration is set to 10, that is, the ICP registration is terminated after at most 10 iterations. At the same time, the convergence error of registration is set to 1, that is, the iteration is stopped when the Euclidean distance between the point pairs is less than 1;

[0088] Step 6: According to the ICP registration result in step 5, the rigid transformation matrix of the vehicle is obtained from the vehicle point cloud reconstructed in step 4, specifically, the rotation matrix R and the translation vector t, which can accurately align the source reconstructed vehicle point cloud to the target perfect vehicle model point cloud, and finally output the accurate three-dimensional position and attitude information of the vehicle in space.

[0089] Corresponding to the foregoing embodiment of vehicle attitude estimation based on a depth camera and point cloud processing, the application also provides an embodiment of a vehicle attitude estimation system based on a depth camera and point cloud processing.

[0090] The embodiment of the application provides a vehicle attitude estimation system based on a depth camera and point cloud processing, which comprises a memory and one or more processors, and the memory stores executable code.

[0091] The embodiment of the application provides a vehicle attitude estimation system based on a depth camera and point cloud processing, which comprises a memory and one or more processors, and the memory stores executable code.

[0092] The device embodiment can be realized by software, hardware or a combination of software and hardware. Taking software realization as an example, as a logical device, it is formed by reading the corresponding computer program instructions in the non-volatile memory into the memory and running by the processor of the device.

[0093] The implementation process of the functions and roles of each unit in the above device is specifically described in the implementation process of the corresponding steps in the above method, which will not be repeated here.

[0094] For the device embodiment, since it basically corresponds to the method embodiment, the relevant part can refer to the part of the method embodiment. The above described device embodiment is only schematic, and the units described as separate components can or can not be physically separated, and the components displayed as units can or can not be physical units, that is, they can be located in one place, or can be distributed on multiple network units. Some or all of the modules can be selected to achieve the purpose of the present application according to actual needs. Those skilled in the art can understand and implement it without creative labor.

[0095] The embodiment of the application also provides a computer readable storage medium, which stores a program, and the program is executed by a processor to realize the vehicle pose estimation method based on the depth camera and the point cloud processing in the above embodiment.

[0096] The computer readable storage medium can be an internal storage unit of any data processing capable device, such as a hard disk or a memory, and the computer readable storage medium can also be an external storage device of any data processing capable device.

[0097] Further, the computer readable storage medium can include both the internal storage unit of any data processing capable device and the external storage device, and the computer readable storage medium is used to store the computer program and other programs and data required by the data processing capable device, and can also be used to temporarily store data that has been output or will be output.

[0098] The above describes the present application and its embodiments, which is not restrictive, and the embodiment shown in the drawings is only one of the embodiments of the present application, and the actual structure is not limited thereto. In general, if a person skilled in the art is inspired, without departing from the purpose of the present application, without creative design, similar structure and embodiments of the technical solution can be designed, which should belong to the protection scope of the present application.

Claims

1. A method and system for vehicle pose estimation based on depth camera and point cloud processing, characterized in that: Includes the following steps: Step 1: Perform depth camera calibration. Use a known calibration board or standard object to calibrate the intrinsic and extrinsic parameters of the depth camera to eliminate the influence of camera distortion. Obtain the camera's spatial position information through hand-eye calibration to ensure the accuracy of subsequent point cloud data. Step 2: Use a depth camera to acquire point cloud data of the vehicle and its surrounding environment in real time from multiple perspectives. This point cloud data includes 3D information of the vehicle itself and its surrounding environment, ensuring the acquisition of high-quality depth maps and point cloud data; Step 3: Preprocess the collected point cloud data, including point cloud noise reduction and segmentation. Use a deep object detection network to detect vehicle areas and crop the point cloud according to the target area to remove irrelevant background point clouds. Step 4: Employ multi-view point cloud data reconstruction technology to align point cloud data from different perspectives, generating a more complete 3D point cloud model. By stitching together multi-view point clouds, a complete high-density point cloud reconstruction of the environment and the area around the vehicle can be achieved. Step 5: Register the reconstructed point cloud data with the perfect vehicle model, and use the ICP (Iterative Closest Point) algorithm for precise alignment to ensure accurate registration between the reconstructed vehicle point cloud and the perfect vehicle model; Step 6: Based on the ICP registration results, calculate the rigid body transformation matrix of the vehicle, and finally output the vehicle's precise three-dimensional position and attitude information and local geometric features to ensure accurate positioning and spraying path planning during the painting operation.

2. The vehicle pose estimation method and system based on depth camera and point cloud processing as described in claim 1, characterized in that: The hand-eye calibration of the depth camera described in step 1 is a key technology in robot vision systems. It is used to determine the relative position and posture relationship between the camera and the robotic arm. By fixing the camera on the robotic arm or the spraying device, and using known calibration objects (such as calibration boards or specific geometric objects) for registration, the relationship between the camera coordinate system and the spraying device coordinate system (robotic arm coordinate system) is ensured. The steps of hand-eye alignment include: Step 1-1: Camera and robotic arm pose acquisition: Given the position of the object, record images from the camera at different angles and positions, and simultaneously acquire the position and orientation of the end effector of the robotic arm (e.g., through an encoder or sensor). Step 1-2: Calculation of coordinate system transformation matrix: By solving the transformation matrix between the camera coordinate system and the robotic arm coordinate system, the relationship between the two is established, namely the hand-eye calibration matrix. This matrix can transform the point cloud data in the robotic arm coordinate system to the camera coordinate system. Steps 1-3: Calibration accuracy optimization: By minimizing error functions (e.g., reprojection error or point cloud overlap), the hand-eye calibration matrix is ​​optimized to ensure the accuracy of coordinate transformation between the camera and the robotic arm.

3. The vehicle pose estimation method and system based on depth camera and point cloud processing as described in claim 1, characterized in that: In step 2, the point cloud information of the vehicle from different perspectives is captured by a depth camera controlled by a robotic arm at a designated point in space, ultimately obtaining a depth map and color image for each perspective point.

4. The vehicle pose estimation method and system based on depth camera and point cloud processing as described in claim 1, characterized in that: Step 3, the point cloud data preprocessing, includes the following steps: Step 3-1: Based on the pre-trained object detection network YOLOv8, perform object detection on the color image acquired at the current location, and detect the vehicle region (Car). box This yields the vehicle mask. car , Step 3-2: Process the original depth map. origin Preprocessing, only considering the vehicle area (Car). box The depth map data within the area is used to obtain the final depth map. final =mask car *Depth origin ; Step 3-3: Combining the camera intrinsic and extrinsic parameters calibrated in S1, generate the depth map. final The data is converted into point clouds to obtain point cloud data of the vehicle from a single viewpoint.

5. The vehicle pose estimation method and system based on depth camera and point cloud processing as described in claim 1, characterized in that: Step 4, multi-view point cloud data reconstruction, includes the following steps: Step 4-1: Perform coordinate transformation on the point cloud data from each viewpoint to convert it to a unified global coordinate system; Step 4-2: Multi-view point cloud data are stitched and fused to generate a complete point cloud model; Step 4-3: Downsample and simplify the fused point cloud to reduce the amount of data and improve the efficiency of subsequent processing.

6. The vehicle pose estimation method and system based on depth camera and point cloud processing as described in claim 1, characterized in that: Step 5: Perform point cloud registration between the multi-view reconstructed point cloud data and the perfect car model point cloud data.

7. The vehicle pose estimation method and system based on depth camera and point cloud processing as described in claim 6, characterized in that: Step 5 describes the point cloud registration process based on the ICP (Iterative Closest Point) algorithm: Step 5-1: Initialization: The source point cloud is the reconstructed vehicle point cloud to be registered, and the target point cloud is the perfect vehicle model point cloud; during initialization, it is assumed that the relative transformation between the source point cloud and the target point cloud is an identity matrix (i.e., there is no rotation or translation). Step 5-2: Nearest Neighbor Matching: For each point in the source point cloud, find the point in the target point cloud that is closest to that point. Euclidean distance is typically used to measure the distance between the two points. Corresponding point pairs (p) in two point clouds i ,q i ), where p i It is a point in the source point cloud, q i It is a point in the target point cloud, and its Euclidean distance d is... i The calculation formula is: d i =||p i -q i || Where ||·|| is the Euclidean norm. Step 5-3: Transformation Calculation: In point cloud registration, the goal is to transform the source point cloud P into the target point cloud Q coordinate system using a rotation matrix R and a translation vector t, so as to minimize the Euclidean distance between corresponding point pairs. Based on the nearest neighbor matching point pairs, the optimal rigid transformation (including the rotation matrix and translation vector) is calculated. A common method is to minimize the point pair error between the source point cloud and the target point cloud, which is calculated using the least squares method. The objective function can be expressed as: Where R is a 3x3 rotation matrix, t is a 3x1 translation vector, and p i It is a point in the source point cloud, q i It is the target point cloud and p i For the corresponding points, ||·|| is the Euclidean norm; Step 5-4: Apply the transformation: The calculated rotation matrix and translation vector are applied to the source point cloud to obtain a new transformed source point cloud. The difference between the transformed source point cloud and the target point cloud is then observed. Step 5-5: Check convergence conditions: Calculate the registration error of the current iteration (the sum of Euclidean distances between all point pairs in the transformed source and target point clouds). If the error is less than a preset threshold δ, or if the maximum number of iterations T is reached... max If the registration process is considered to have converged, the iteration stops; otherwise, return to S52 and continue to the next round of iteration.

8. The vehicle pose estimation method and system based on depth camera and point cloud processing as described in claim 1, characterized in that: Step 6 specifically involves: calculating the rigid body transformation matrix (including rotation and translation) of the vehicle based on the ICP registration results. This transformation matrix can accurately align the source reconstructed vehicle point cloud to the target perfect vehicle model point cloud, and finally output the precise three-dimensional position and attitude information of the vehicle in space.

Images