Robot Pose Estimation Method Based on Image and Point Cloud Fusion with Dynamic Feature Elimination

Lei Zhang; Xiaobin Xu; Chenfei Cao; Jia He; Yngying Ran; Zhiying Tan; Minzhou Luo

doi:10.3788/CJL202249.0610001

Chinese Journal of Lasers, Volume. 49, Issue 6, 0610001(2022)

Robot Pose Estimation Method Based on Image and Point Cloud Fusion with Dynamic Feature Elimination

Lei Zhang^1,2, Xiaobin Xu^1,2,3,4、*, Chenfei Cao^1,2, Jia He^1,2, Yngying Ran^1,2, Zhiying Tan^1,2, and Minzhou Luo^1,2

Author Affiliations

¹College of Mechanical & Electrical Engineering, Hohai University, Changzhou, Jiangsu 213022, China

²Jiangsu Key Laboratory of Special Robot Technology, Hohai University, Changzhou, Jiangsu 213022, China

³College of Mechanical and Electrical Engineering, Nanjing University of Aeronautics and Astronautics, Nanjing, Jiangsu 210016, China

⁴Changzhou Changgong Electronic Technology Co., Ltd., Changzhou, Jiangsu 213001, China

show less

Abstract Get PDF(in Chinese)

Figures & Tables(13)

Fig. 1. Overall framework of pose estimation

Download full size

Fig. 2. Dynamic experimental scenes. (a) 05 sequence experiment scene; (b) 08 sequence experiment scene

Download full size

Fig. 3. Elimination of dynamic features. (a) Elimination of dynamic point clouds; (b) elimination of dynamic feature points

Download full size

Fig. 4. Box plots of pose error. (a) Angular error box plot; (b) displacement error box plot

Download full size

Fig. 5. Experimental platform and experimental scene. (a) Experimental platform; (b) experimental scene

Download full size

Fig. 6. Candidate frames extraction by deep learning. (a) Image candidate frame; (b) point cloud candidate frame

Download full size

Fig. 7. Running time box plot

Download full size

Table 1. Nonlinear optimization algorithm based on RANSAC

View table

Table 1. Nonlinear optimization algorithm based on RANSAC

Input:P_last;p_current;Camera internal referenceK; Jacobian matrix of error function for poseJ

Output: Camera pose of the current frameT

1：Use EPNP to initially solve the camera poseT₀

2：　T＝T₀

3：Sum of reprojection errors e_total＝0

4：　fori＝1→The maximum number of iterations n　do

5:　forP_j∈P_lastp_j∈p_currentdo

6：Reprojection error e_j＝|p_j－P_jKT/s_i|

7：ife_j>Error threshold t₀then

8：recordP_j andp_j

9：continue

10：end if

11：e_total＝e_total＋e_j

12：Calculate the Hessian matrixH＝JJ^T，以及g＝－Je_j

13：Calculate the iteration value ΔT＝H^－1g

14：T＝ΔTT

15：end for

16：Eliminate the recordedP_j andp_j

17：　end for

Table 2. Mean error of comparison algorithms in different scenarios

View table

Table 2. Mean error of comparison algorithms in different scenarios

Sequence	Algorithm	Pitch angle /(°)	Yaw angle /(°)	Roll angle /(°)	x /m	y /m	z /m
05	BA	0.0669	0.0391	0.0363	0.0197	0.0104	0.0136
	Visual	0.0302	0.0273	0.0380	0.0126	0.0087	0.0192
	LOAM	0.0376	0.0138	0.0377	0.0160	0.0091	0.0093
	LIDAR	0.0374	0.0148	0.0383	0.0160	0.0094	0.0107
08	BA	0.0451	0.0359	0.0478	0.0318	0.0196	0.0225
	Visual	0.0106	0.0248	0.0135	0.0096	0.0031	0.0059
	LOAM	0.0052	0.0097	0.0049	0.0055	0.0016	0.0060
	LIDAR	0.0049	0.0080	0.0046	0.0052	0.0015	0.0073

Table 3. Error standard deviation of comparison algorithms in different scenarios

View table

Table 3. Error standard deviation of comparison algorithms in different scenarios

Sequence	Algorithm	Pitch angle /(°)	Yaw angle /(°)	Roll angle /(°)	x /m	y /m	z /m
05	BA	0.0930	0.0263	0.0321	0.0099	0.0080	0.0103
	Visual	0.0392	0.0216	0.0295	0.0112	0.0069	0.0131
	LOAM	0.0322	0.0075	0.0221	0.0043	0.0074	0.0054
	LIDAR	0.0330	0.0083	0.0215	0.0044	0.0074	0.0057
08	BA	0.0431	0.0305	0.0446	0.0350	0.0166	0.0306
	Visual	0.0129	0.0154	0.0115	0.0095	0.0014	0.0053
	LOAM	0.0046	0.0054	0.0029	0.0038	0.0010	0.0044
	LIDAR	0.0047	0.0071	0.0027	0.0036	0.0009	0.0041

Table 4. Mean error of different algorithms

View table

Table 4. Mean error of different algorithms

Sequence	Algorithm	Pitch angle /(°)	Yaw angle /(°)	Roll angle /(°)	x /m	y /m	z /m
05	BA^[29]	0.0669	0.0391	0.0363	0.0197	0.0104	0.0136
	ORBSLAM2^[19]	0.0307	0.0114	0.0283	0.0099	0.0223	0.0243
	LOAM^[23]	0.0376	0.0138	0.0377	0.0160	0.0091	0.0093
	Fusion	0.0332	0.0081	0.0221	0.0046	0.0070	0.0057
08	BA^[29]	0.0451	0.0359	0.0478	0.0318	0.0196	0.0225
	ORBSLAM2^[19]	0.0067	0.0177	0.0076	0.0046	0.0023	0.0068
	LOAM^[23]	0.0052	0.0097	0.0049	0.0055	0.0016	0.0060
	Fusion	0.0060	0.0086	0.0064	0.0050	0.0018	0.0050

Table 5. Comparison of experimental results of relative pose errors in indoor dynamic scenes

View table

Table 5. Comparison of experimental results of relative pose errors in indoor dynamic scenes

Algorithm	Pitch angle /(°)	Yaw angle /(°)	Roll angle /(°)	x /m	y /m	z /m
BA	0.0255	1.1481	0.0134	0.0123	0.0016	0.1018
ORBSLAM2	0.0323	0.0901	0.0416	0.0058	0.0038	0.0033
Visual	0.0084	0.3568	0.0106	0.0086	0.0026	0.0134
LOAM	0.0779	0.0953	0.2583	0.0071	0.0098	0.0019
LIDAR	0.0048	0.0198	0.1041	0.0058	0.0024	0.0052
Fusion	0.0035	0.0178	0.0926	0.0044	0.0024	0.0060

Table 6. Average running time of different algorithms
View table
Table 6. Average running time of different algorithms
Algorithm Average running time /s
BA 0.0317
ORBSLAM2 0.1462
LOAM 0.0242
Fusion 0.2974

Tools

Get Citation

Copy Citation Text

Lei Zhang, Xiaobin Xu, Chenfei Cao, Jia He, Yngying Ran, Zhiying Tan, Minzhou Luo. Robot Pose Estimation Method Based on Image and Point Cloud Fusion with Dynamic Feature Elimination[J]. Chinese Journal of Lasers, 2022, 49(6): 0610001

Download Citation

EndNote(RIS)BibTex Plain Text

Set citation alerts for article

Save article for my favorites