Position and posture estimation method for battery replacement robot based on point cloud component segmentation and registration

Abstract

A point cloud component segmentation and registration-based battery replacement robot pose estimation method comprises the following steps: 1, placing a 3D visual sensor below an electric vehicle chassis to shoot a to-be-replaced battery pack, and obtaining an RGB color image and a depth image containing a battery pack locking and unlocking device in a scene; 2, acquiring a locking and unlocking instance segmentation mask of the acquired color image by using a convolutional neural network, and projecting a locking and unlocking instance into a locking and unlocking point cloud in combination with the acquired depth map and camera internal reference; 3, statistical filtering and voxel filtering are carried out on the locking and unlocking point cloud, point cloud noisy points are removed, downsampling is carried out, and the point cloud data quality is improved; 4, segmenting a point cloud of the locking and unlocking lock through a Point Net point cloud segmentation network; and 5, solving the important part of the locking and unlocking lock point cloud by using a principal component analysis method, calculating the FPFH feature of the important part, searching the corresponding relationship between the scene lock point cloud and the template lock point cloud through an RANSAC method to complete coarse registration, and finally, optimizing the registration result by using ICP to obtain the estimated pose of locking and unlocking.

Description

Technical Field

[0001] The present invention belongs to the field of intelligent battery replacement for electric vehicles, and specifically is a method for estimating the posture of a battery replacement robot based on point cloud component segmentation and alignment. Background Art

[0002] As the number of new energy vehicles in my country continues to increase, solving the problem of rapid energy replenishment for new energy vehicles plays an important role in alleviating users' mileage anxiety and accelerating the cultivation and development of the new energy vehicle industry. At present, most electric vehicles use fixed charging piles for charging and recharging. This method is flexible and convenient, but it takes a long time to charge, and is prone to two extreme situations: zombie piles and charging piles in short supply. Compared with the charging method, directly replacing the battery pack can complete the rapid energy replenishment of new energy vehicles.

[0003] During the battery replacement process of new energy vehicles, providing accurate spatial position information of battery pack unlocking and unlocking to the battery replacement robot is a key step in realizing the removal and installation of the battery pack. Mechanical positioning uses a flexible parking platform, fixed car, battery pack, unlocking and unlocking dimensions to accurately calculate the spatial position of unlocking and unlocking. With the increase in new energy vehicle brands, the problem of mechanical positioning's poor compatibility with battery packs and unlocking and unlocking of different brands has gradually become prominent.

[0004] Visual positioning collects environmental information through visual sensors, and realizes the pose estimation of locking and unlocking through image processing, point cloud processing and corresponding pose estimation algorithms. It has strong adaptability to the environment, battery pack type and locking and unlocking structure. Therefore, a vision-based battery locking and unlocking positioning method is urgently needed to make up for the defects of mechanical positioning and improve the compatibility and accuracy of battery pack locking and unlocking positioning.

[0005] Technical comparison with patent CN112561998B "A robot positioning and autonomous charging method based on three-dimensional point cloud registration"

[0006] In patent CN112561998B, the scene point cloud obtained by laser radar is cropped using the method of constraining the point cloud coordinates to obtain point cloud data that effectively contains the charging room. We use RGBD cameras to collect color images and depth images, first use a deep neural network to locate the scene target in the color image, and then project the depth data corresponding to the scene target part to obtain the target point cloud. Because the distance between the robot and the target is constantly changing, the point cloud cropping method based on image target recognition and instance segmentation is more adaptive and flexible than the method of cropping the target point cloud in a fixed range.

[0007] Patent CN112561998B first uses the Normal Distributions Transform (NDT) method to roughly align with the template point cloud. We use the Random Sample Consensus (RANSAC) method to roughly align with the template point cloud, which is more suitable for some scenes that do not require high initial alignment between the scene point cloud and the template point cloud and require fast processing.

[0008] Technical comparison with patent CN115690209A "A three-dimensional positioning and identification method, device, medium, and equipment for distribution transformer terminal"

[0009] In patent CN115690209A, a 3D scanner is used to obtain the scene point cloud, and then the target point cloud in the scene point cloud is extracted by the straight-through filtering method. We use RGBD cameras to collect color images and depth images, first use a deep neural network to locate the scene target in the color image, and then project the depth data corresponding to the scene target part to obtain the target point cloud. Since the straight-through filtering requires the fixed coordinate range of the three directions of the point cloud, which limits the extraction of target point clouds at different distances and angles, we rely on deep learning methods to make the extraction process of target point clouds more intelligent and flexible.

[0010] Technical comparison with patent CN115272655A "Visual positioning method and system device for multi-type battery packs of battery swapping robots"

[0011] Patent CN115272655A uses HSV color space transformation to convert the color of the chassis battery pack point cloud from RGB space to HSV space, and identifies and unlocks the hole based on the color features of the HSV space and the circular feature of the lock hole. We use deep learning-based target detection and instance segmentation technology to locate and unlock, which has stronger robustness and adaptability.

[0012] Patent CN115272655A uses the method of fitting the center and normal vector of the keyhole point cloud to obtain the pose of the keyhole. We use a method based on point cloud feature description and registration. Affected by the shooting angle, the point cloud of the object will have a certain deformation. The feature description method does not rely on the overall contour of the point cloud, so the pose estimation result is more accurate.

[0013] Technical comparison with patent CN117635699A "Power battery pose estimation method based on machine vision point cloud segmentation"

[0014] Patent CN117635699A uses the RANSAC method to fit the point cloud of the battery pack plane, and then extracts the lock point cloud based on the distance from each point in the point cloud to the plane. We use a point cloud component segmentation method based on deep learning to directly segment the lock point cloud from the lock and unlock point cloud, effectively overcoming the problem of inaccurate segmentation results caused by selecting point clouds at a fixed distance.

[0015] Patent CN117635699A uses the method of obtaining the overall FPFH features of the lock point cloud, and then uses the RANSAC method to roughly align it with the template point cloud, and then uses ICP (Iterative Closest Point, ICP) to accurately align it to obtain the lock and unlock posture. We use the principal component analysis method to extract the main part of the lock point cloud, and then extract the features of this part of the point cloud for RANSAC rough alignment, and then use the original overall point cloud for ICP precise alignment to obtain the lock and unlock posture, which can effectively overcome the problem of weak feature expression caused by the point cloud holes that are easy to appear on the side of the object away from the camera. Summary of the invention

[0016] In order to solve the above technical problems, the present invention proposes a posture estimation method for a battery swapping robot based on point cloud component segmentation and registration, so as to improve the compatibility of the battery swapping robot in unlocking and positioning battery packs of different vehicle models and different structural sizes.

[0017] To achieve the above object, the technical solution adopted by the present invention is:

[0018] A method for estimating the posture of a battery-swapping robot based on point cloud component segmentation and registration, the specific steps of the method are as follows:

[0019] (1) 3D vision sensor installation and working method;

[0020] Use 3D vision sensors to collect color and depth images of the vehicle chassis battery pack in the smart battery swap station;

[0021] (2) Obtain the original point cloud that is encrypted and unlocked;

[0022] Apply the image instance segmentation technology to the collected color image, output the unlocked instance segmentation mask, locate the unlocked region of interest in the image, combine the instance segmentation mask and the collected depth map, and project the unlocked region of interest to obtain the scene point cloud P0;

[0023] (3) Encryption and unlocking point cloud preprocessing;

[0024] The point cloud P0 is filtered using point cloud statistical filtering to remove outliers and noise points in P0 to obtain point cloud P1. The point cloud P1 is downsampled using voxel filtering to reduce the number of point clouds and retain the fine geometric structure of the point cloud to obtain point cloud P2.

[0025] (4) Lock and unlock point cloud segmentation;

[0026] Train the PointNet point cloud segmentation network, input the point cloud P2 into the trained PointNet network for component segmentation, and obtain the lock point cloud P3;

[0027] (5) Enabling and unlocking the lock point cloud registration;

[0028] The principal component analysis method is used to calculate the important part P of the lock point cloud P3 m , and calculate P m The FPFH local feature F(P m ), use the same method to obtain the important part Q of the template point cloud Q m and important part features F(Q m ), and use the RANSAC method to find F(P m ) and F(Q m ) completes the corresponding relationship P m and Q m Then use ICP to complete the precise registration of P3 and Q to obtain the estimated pose T of the unlocked e .

[0029] Furthermore, the 3D vision sensor in step (1) is installed and works as follows:

[0030] The visual sensor is placed at a fixed position on the battery swap robot at a distance of 0.4 to 0.7 m from the car chassis. The alignment relationship T0 of the 3D vision sensor's color camera and depth camera needs to be calibrated. During the movement of the battery swap robot, the visual sensor takes color and depth pictures of the car chassis including the battery pack unlocking and unlocking.

[0031] Furthermore, step (2) of obtaining the encrypted and unlocked original point cloud includes the following steps:

[0032] (2-1) Use a convolutional neural network to segment the unlocked instances in the image and output the unlocked instance mask. The output mask is the membership probability p of each image pixel to the foreground and background. All pixels with a membership probability p greater than 0.5 are selected as the segmentation mask of the unlocked target;

[0033] (2-2) Using the alignment relationship T0 between the color camera and the depth camera, the depth value corresponding to the unlocked area in the color image is obtained, and the depth value is projected into the unlocked original point cloud P0 using the intrinsic parameter K of the depth camera.

[0034] Furthermore, step (3) of unlocking the point cloud preprocessing includes the following steps:

[0035] (3-1) First, perform point cloud statistical filtering. For each point p in the point cloudi , calculate the mean μ and variance σ of the Euclidean distance of n points in the area of ​​the point. If p i A point in the neighborhood and p i If the Euclidean distance of is within the range of (μ-kσ,μ+kσ), the point is retained, otherwise it is removed. All points in the point cloud are traversed to obtain the point cloud P1 after statistical filtering of P0;

[0036] (3-2) Create a voxel grid for the point cloud P1, calculate the centroid of the point cloud in each voxel grid and use it to replace the voxel grid point, traverse all voxel grids, and obtain the point cloud P2 after voxel filtering.

[0037] Furthermore, step (4) of locking and unlocking the lock point cloud segmentation includes the following steps:

[0038] (4-1) Expand the collected point cloud dataset and manually label the point cloud dataset using CloudCompare software, where the lock part is labeled as 0 and the other parts are labeled as 1;

[0039] (4-2) Train the PoinNet network using the labeled point cloud dataset;

[0040] (4-3) Input the point cloud P2 into the trained PoinNet network to obtain the confidence score of each point in the point cloud belonging to the two labels 0 and 1. According to the confidence result, the points belonging to label 0 are segmented from the point cloud to obtain the unlocking and locking point cloud P3.

[0041] Furthermore, step (5) of adding and unlocking the lock point cloud registration includes the following steps:

[0042] (5-1) Extract the important parts P of the lock point cloud P3 and the template point cloud Q using principal component analysis m and Q m ;

[0043] (5-2) Calculate P m and Q m The FPFH characteristic F(P m ) and F(Q m );

[0044] (5-3) Use the RANSAC method to find F(P m ) and F(Q m ) to complete the coarse-step registration of P3 and Q, and finally use ICP to complete the precise registration of P3 and Q to obtain the unlocked estimated pose T e .

[0045] The benefits of this application are:

[0046] The present invention discloses a method and system for estimating the posture of a battery-swap robot based on point cloud component segmentation and registration. The method uses a visual sensor to obtain a color image and a depth image containing locking and unlocking in a scene, obtains a locking and unlocking lock point cloud through image instance segmentation and PointNet point cloud component segmentation; obtains an important part of the lock point cloud using the principal component analysis method, and extracts the FPFH features of this part; on this basis, the scene lock point cloud and the target lock point cloud are preliminarily registered using the RANSAC algorithm, and finally the locking and unlocking posture is obtained using the ICP point cloud precise registration algorithm. The posture estimation method utilizes the powerful nonlinear data feature learning ability of the neural network, and by extracting the important components of the point cloud, it can effectively avoid the influence of different angles and distances of the visual sensor on the locking and unlocking posture estimation results. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 is a flow chart of the method disclosed in the present invention;

[0048] Figure 2 It is the installation and working method of the 3D visual sensor of the battery swap positioning system disclosed in the present invention;

[0049] Figure 3 It is a structural diagram of the PointNet point cloud segmentation model of the present invention. DETAILED DESCRIPTION

[0050] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments:

[0051] The present invention discloses a method and system for estimating the posture of a battery-swapping robot based on point cloud component segmentation and registration. The implementation flow chart of the method is as follows: Figure 1 As shown, the following steps are included:

[0052] The 3D vision sensor installation and working method in step (1) is as follows Figure 2 shown.

[0053] Place the visual sensor at a fixed position on the battery swap robot at a distance of 0.4 to 0.7 m from the vehicle chassis. The alignment relationship T0 of the color camera and depth camera of the 3D visual sensor needs to be calibrated. During the movement of the battery swap robot, the visual sensor takes color and depth images of the vehicle chassis including the unlocking of the battery pack.

[0054] Step (2) of obtaining the encrypted and unlocked original point cloud includes the following steps:

[0055] (2-1) Use a convolutional neural network to segment the unlocked instances in the image and output the unlocked instance mask. The network uses a multi-layer residual convolutional neural network to extract the multi-level features of the image, and fuses the features of different scales through the feature pyramid structure to obtain the multi-scale features of the image. Then, use the segmentation mask prediction layer to predict the membership probability p of each pixel between the foreground and the background to generate a pixel-level instance mask. Finally, select all pixels with a membership probability p greater than 0.5 as the instance segmentation mask of the unlocked target;

[0056] (2-2) Using the alignment relationship T0 between the color camera and the depth camera, the depth value corresponding to the unlocked area in the color image is obtained, and the depth value is projected into the unlocked original point cloud P0 using the intrinsic parameter K of the depth camera. The projection relationship is as follows:

[0057]

[0058] Where u, v are pixel coordinates, f x , f y , c x , c y is the internal parameter of the color camera, and z is the depth value.

[0059] Step (3) The preprocessing of the unlocked point cloud includes the following steps:

[0060] (3-1) For a point p in the point cloud P0 i , calculate the mean and variance of the Euclidean distance between it and the n points in its neighborhood:

[0061]

[0062] Where p is the point i The Euclidean distance to a point in its neighborhood. If p i A point in the neighborhood and p i If the Euclidean distance of is within the range of (μ-kσ,μ+kσ), the point is retained, otherwise the point is considered an outlier and removed. All points in P0 are traversed to complete statistical filtering and obtain point cloud P1.

[0063] (3-2) Create a voxel grid of point cloud P1 and set the size of the voxel grid to 1 mm. For a voxel grid containing m points, calculate the centroid of all points in the grid:

[0064]

[0065] Using the point (x v ,y v ,z v ) as the point after the voxel grid is filtered. All voxel grids in P1 are traversed to complete the point cloud voxel filtering and obtain point cloud P2.

[0066] Step (4) Enabling and unlocking the lock point cloud segmentation includes the following steps:

[0067] (4-1) Collect 200 locked and unlocked point clouds from different angles and distances, perform a small distance spatial pose transformation on each point cloud to increase the angle of the collected point cloud, and then add Gaussian noise to simulate the random jitter in the process of collecting point clouds to complete the expansion of the point cloud data. Use CloudCompare software to manually label the expanded point cloud data set, where the lock part is labeled as 0, and the other parts except the lock are labeled as 1;

[0068] (4-2) Store the xyz coordinate information of n points in the point cloud into a tensor Input, whose shape is (n,3). The main module of the PointNet network is the multi-layer perceptron mlp (c1,c2) , the maximum pooling layer max d , feature concatenation layer cat d [F1, F2]. MLP includes a convolution layer Conv, a normalization layer Bn and an activation layer ReLU. C1 and C2 represent the number of input and output channels of MLP respectively. D represents the dimension of the maximum pooling and feature concatenation operations. F1 and F2 represent the two features that need to be concatenated.

[0069] The process of forward propagation of the PointNet model is as follows: the shape of the two-dimensional tensor composed of the xyz coordinates of n points in the point cloud is (n,3), which is input into the mlp (3,64) Get the local feature f1, f1 input mlp (64,128) and mlp (128,1024) After the two mlp layers are connected in series, f2 is obtained. f2 is input into the max1 layer to obtain a global feature f3 with a shape of (1,1024). f3 and f1 are input into cat2(f3,f1) to output a feature f with both local information and global information. m , f m Enter mlp (1088,512) 、mlp (512,256) and mlp (256,128) After the three mlp layers are connected in series, f4 is obtained, and f4 is input into mlp (128,2) mlp(128,2) gets f5, and finally f5 is input into the Log-softmax layer to output a tensor Output with a shape of (n,2). The structural diagram of the model is as follows Figure 3 shown.

[0070] (4-3) Each point cloud is fed into the trained PointNet to output an Output tensor of shape (n,2), which contains the confidence scores of n points corresponding to label 0 and label 1. Points whose label 0 scores are greater than label 1 scores are segmented from P2 to obtain the locked and unlocked lock point cloud P3.

[0071] Step (5) Enabling and unlocking the lock point cloud registration includes the following steps:

[0072] (5-1) Use principal component analysis to extract the important components of the point cloud. First, point p of point cloud P3 is i (x i ,y i ,z i ) and the centroid p c (x c ,y c ,z c ) is projected onto the xoy plane and the following covariance matrix is ​​calculated:

[0073]

[0074] Solve the eigenvalues ​​of matrix C and arrange them from small to large λ1<λ2<λ3. Find the eigenvectors {v1,v2,v3} corresponding to the three eigenvalues ​​and establish the following principal component coordinate system:

[0075]

[0076] Set the coordinate system X p Y p Z p Move to the point cloud centroid p c At, calculate the X coordinate system of each point in P3 p The projection of the direction is divided into two parts according to the positive and negative projection, and the part with large number and low resolution is selected as the important part of the point cloud P m The same method is used to obtain the important part Q of the template point cloud Q m .

[0077] (5-2) A point p in the point cloud s The FPFH characteristic calculation formula is:

[0078]

[0079] where p j For p s A point in the neighborhood, ω j For p j and p s The Euclidean distance between s ) is to simplify the feature histogram, and its calculation process is as follows:

[0080] 1. Remember the point p s The normal vector is n s , establish p s and p j The local coordinate system (u,v,w):

[0081]

[0082] 2. According to the local coordinate system (u, v, w) and p j Normal vector calculation p s Local features:

[0083]

[0084] 3. Divide the value space of the feature quantities α, φ and θ into 11 equal parts, and count the p s Each point pair in the neighborhood (p s ,p j )’s features (α, φ, θ), and then concatenates these three histograms into a 33-dimensional vector representing p s SPFH characteristics.

[0085] Point cloud P m and point cloud Q m All points in the calculation of their FPFH features to obtain F(P m ) and F(Q m ).

[0086] (5-3) The process of using the RANSAC method to complete the point cloud coarse registration is described as follows:

[0087] 1. In the point cloud P m Three sampling points are randomly selected in the point cloud Q. To ensure the difference of FPFH features of the sampling points, the distance between the two sampling points should be greater than the predetermined distance threshold d. m Find the same as P m The three corresponding points with the most similar FPFH features among the sampling points.

[0088] 2. Based on the three selected matching points, the rigid body transformation matrix T between the corresponding points is calculated by SVD singular value decomposition SAC , according to T SAC Find point cloud P m and Q m The transformation error is:

[0089]

[0090] Where p i is a point in the scene point cloud, q i Point cloud Q m Middle distance pi The nearest point, h(·) is the Huber loss.

[0091] 3. Repeat steps (1) and (2) until the error T SAC When the value is less than the threshold or the number of iterations reaches the upper limit, the final transformation matrix T is obtained. SAC . SAC Applied to P3 to complete the coarse alignment of P3 and Q.

[0092] After the preliminary registration of P3 and Q, their spatial positions roughly overlap. In order to make the pose estimation result more accurate, the ICP algorithm is used to complete the precise registration of point clouds P3 and Q to obtain the pose transformation matrix T ICP The final output of the unlocked pose information is T e =T ICP ·T SAC .

[0093] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent change made based on the technical essence of the present invention still falls within the scope of protection required by the present invention.

Claims

1. A method for estimating the pose of a battery-swapping robot based on point cloud component segmentation and registration, characterized in that: The specific steps for use are: (1) 3D vision sensor installation and working method; Use 3D vision sensors to collect color and depth images of the vehicle chassis battery pack in the smart battery swap station; (2) Obtain the original point cloud that is encrypted and unlocked; Apply the image instance segmentation technology to the collected color image, output the unlocked instance segmentation mask, locate the unlocked region of interest in the image, combine the instance segmentation mask and the collected depth map, and project the unlocked region of interest to obtain the scene point cloud P0; (3) Encryption and unlocking point cloud preprocessing; The point cloud P0 is filtered using point cloud statistical filtering to remove outliers and noise points in P0 to obtain point cloud P1. The point cloud P1 is downsampled using voxel filtering to reduce the number of point clouds and retain the fine geometric structure of the point cloud to obtain point cloud P2. (4) Lock and unlock point cloud segmentation; Train the PointNet point cloud segmentation network, input the point cloud P2 into the trained PointNet network for component segmentation, and obtain the lock point cloud P3; (5) Enabling and unlocking the lock point cloud registration; The principal component analysis method is used to calculate the important part P of the lock point cloud P3 m , and calculate P m The FPFH local feature F(P m ), use the same method to obtain the important part Q of the template point cloud Q m and important part features F(Q m ), and use the RANSAC method to find F(P m ) and F(Q m ) completes the corresponding relationship P m and Q m Then use ICP to complete the precise registration of P3 and Q to obtain the estimated pose T of the unlocked e .

2. According to claim 1, a method for estimating the posture of a battery-swapping robot based on point cloud component segmentation and registration is characterized in that: The installation and working method of the 3D vision sensor in step (1) is as follows: The visual sensor is placed at a fixed position on the battery swap robot at a distance of 0.4 to 0.7 m from the car chassis. The alignment relationship T0 of the 3D vision sensor's color camera and depth camera needs to be calibrated. During the movement of the battery swap robot, the visual sensor takes color and depth pictures of the car chassis including the battery pack unlocking and unlocking.

3. According to claim 1, a method for estimating the posture of a battery-swapping robot based on point cloud component segmentation and registration is characterized in that: Step (2) of obtaining the encrypted and unlocked original point cloud includes the following steps: (2-1) Use a convolutional neural network to segment the unlocked instances in the image and output the unlocked instance mask. The output mask is the membership probability p of each image pixel to the foreground and background. All pixels with a membership probability p greater than 0.5 are selected as the segmentation mask of the unlocked target; (2-2) Using the alignment relationship T0 between the color camera and the depth camera, the depth value corresponding to the unlocked area in the color image is obtained, and the depth value is projected into the unlocked original point cloud P0 using the intrinsic parameter K of the depth camera.

4. A method for estimating the posture of a battery-swapping robot based on point cloud component segmentation and registration according to claim 1, characterized in that: Step (3) The preprocessing of the unlocked point cloud includes the following steps: (3-1) First, perform point cloud statistical filtering. For each point p in the point cloud i , calculate the mean μ and variance σ of the Euclidean distance of n points in the area of ​​the point. If p i A point in the neighborhood and p i If the Euclidean distance of is within the range of (μ-kσ,μ+kσ), the point is retained, otherwise it is removed. All points in the point cloud are traversed to obtain the point cloud P1 after statistical filtering of P0; (3-2) Create a voxel grid for the point cloud P1, calculate the centroid of the point cloud in each voxel grid and use it to replace the voxel grid point, traverse all voxel grids, and obtain the point cloud P2 after voxel filtering.

5. A method for estimating the posture of a battery-swapping robot based on point cloud component segmentation and registration according to claim 1, characterized in that: Step (4) Enabling and unlocking the lock point cloud segmentation includes the following steps: (4-1) Expand the collected point cloud dataset and manually label the point cloud dataset using CloudCompare software, where the lock part is labeled as 0 and the other parts are labeled as 1; (4-2) Train the PoinNet network using the labeled point cloud dataset; (4-3) Input the point cloud P2 into the trained PoinNet network to obtain the confidence score of each point in the point cloud belonging to the two labels 0 and 1. According to the confidence result, the points belonging to label 0 are segmented from the point cloud to obtain the unlocking and locking point cloud P3.

6. A method for estimating the posture of a battery-swapping robot based on point cloud component segmentation and registration according to claim 1, characterized in that: Step (5) Enabling and unlocking the lock point cloud registration includes the following steps: (5-1) Extract the important parts P of the lock point cloud P3 and the template point cloud Q using principal component analysis m and Q m ; (5-2) Calculate P m and Q m The FPFH characteristic F(P m ) and F(Q m ); (5-3) Use the RANSAC method to find F(P m ) and F(Q m ) to complete the coarse-step registration of P3 and Q, and finally use ICP to complete the precise registration of P3 and Q to obtain the unlocked estimated pose T e .

Images