hangpersonal/SLAM_Docker_Env
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add slam_in_autonomous_driving repo

Browse Source
This commit is contained in:
Hang Cui
2025-11-03 09:52:30 -08:00
parent 0e874755e4
commit df731f29fa
987 changed files with 752983 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
workspace/slam_in_autonomous_driving/scripts/all_path.py Executable file
View File

@@ -0,0 +1,58 @@
# coding=UTF-8
import sys
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def PlotPath(keyframes):
fig = plt.figure('All path 3d')
ax = Axes3D(fig)
# lidar pose
p00, = ax.plot(keyframes[:, 5], keyframes[:, 6], keyframes[:, 7], 'y-')
# gps pose
p01, = ax.plot(keyframes[:, 12], keyframes[:, 13], keyframes[:, 14], 'g-')
# opti1
p02, = ax.plot(keyframes[:, 19], keyframes[:, 20], keyframes[:, 21], 'b-')
# opti2
# p03, = ax.plot(keyframes[:, 26], keyframes[:, 27], keyframes[:, 28], 'r-')
plt.title("All path", fontsize=16)
plt.grid()
fig = plt.figure('All path 2d')
# lidar
p0, = plt.plot(keyframes[:, 5], keyframes[:, 6], 'y-')
# gps
p1, = plt.plot(keyframes[:, 12], keyframes[:, 13], 'g-')
# opti1
p2, = plt.plot(keyframes[:, 19], keyframes[:, 20], 'b-')
# opti2
# p3, = plt.plot(keyframes[:, 26], keyframes[:, 27], 'r-')
# plt.legend(['Lidar', 'RTK', 'Opti1', 'Opti2'])
plt.legend(['Lidar', 'RTK', 'Opti1'])
plt.show()
def LoadMappingTxt(filepath):
keyframes = np.loadtxt(filepath)
PlotPath(keyframes)
if __name__ == '__main__':
if len(sys.argv) != 2:
print('Please input vaild param !!!')
exit(1)
else:
path = sys.argv[1]
LoadMappingTxt(path)
exit(1)
exit(1)

25
workspace/slam_in_autonomous_driving/scripts/plot_ch3_gnss_2d.py Normal file
View File

@@ -0,0 +1,25 @@
# coding=UTF-8
import sys
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# 格式：t x y z qx qy qz qw
if __name__ == '__main__':
if len(sys.argv) != 2:
print('Please input valid file')
exit(1)
else:
path = sys.argv[1]
path_data = np.loadtxt(path)
plt.rcParams['figure.figsize'] = (16.0, 12.0)
# 2D轨迹
plt.scatter(path_data[:, 1], path_data[:, 2], s=2)
plt.xlabel('X')
plt.ylabel('Y')
plt.grid()
plt.title('2D trajectory')
plt.show()
exit(1)

20
workspace/slam_in_autonomous_driving/scripts/plot_ch3_gnss_3d.py Normal file
View File

@@ -0,0 +1,20 @@
# coding=UTF-8
import sys
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# 格式：t x y z qx qy qz qw
if __name__ == '__main__':
if len(sys.argv) != 2:
print('Please input valid file')
exit(1)
else:
path = sys.argv[1]
path_data = np.loadtxt(path)
fig = plt.figure()
ax = Axes3D(fig)
ax.plot(path_data[:, 1], path_data[:, 2], path_data[:, 3])
plt.title('3D path')
plt.show()
exit(1)

43
workspace/slam_in_autonomous_driving/scripts/plot_ch3_state.py Normal file
View File

@@ -0,0 +1,43 @@
# coding=UTF-8
import sys
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# 格式：t x y z qx qy qz qw vx vy vz
if __name__ == '__main__':
if len(sys.argv) != 2:
print('Please input valid file')
exit(1)
else:
path = sys.argv[1]
path_data = np.loadtxt(path)
plt.rcParams['figure.figsize'] = (16.0, 12.0)
# 轨迹
plt.subplot(121)
plt.scatter(path_data[:, 1], path_data[:, 2], s=2)
plt.xlabel('X')
plt.ylabel('Y')
plt.grid()
plt.title('2D trajectory')
# 姿态
plt.subplot(222)
plt.plot(path_data[:, 0], path_data[:, 4], 'r')
plt.plot(path_data[:, 0], path_data[:, 5], 'g')
plt.plot(path_data[:, 0], path_data[:, 6], 'b')
plt.plot(path_data[:, 0], path_data[:, 7], 'k')
plt.title('q')
plt.legend(['qw', 'qx', 'qy', 'qz'])
# 速度
plt.subplot(224)
plt.plot(path_data[:, 0], path_data[:, 8], 'r')
plt.plot(path_data[:, 0], path_data[:, 9], 'g')
plt.plot(path_data[:, 0], path_data[:, 10], 'b')
plt.title('v')
plt.legend(['vx', 'vy', 'vz'])
plt.show()
exit(1)

53
workspace/slam_in_autonomous_driving/scripts/plot_ch4_path.py Normal file
View File

@@ -0,0 +1,53 @@
# coding=UTF-8
import sys
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
if __name__ == '__main__':
if len(sys.argv) != 2:
print('Please input valid file')
exit(1)
else:
path = sys.argv[1]
path_data = np.loadtxt(path)
plt.rcParams['figure.figsize'] = (16.0, 12.0)
# 轨迹
plt.subplot(121)
plt.scatter(path_data[:, 1], path_data[:, 2], s=2)
plt.xlabel('X')
plt.ylabel('Y')
plt.grid()
plt.title('2D trajectory')
# 速度
plt.subplot(322)
plt.plot(path_data[:, 0], path_data[:, 8], 'r')
plt.plot(path_data[:, 0], path_data[:, 9], 'g')
plt.plot(path_data[:, 0], path_data[:, 10], 'b')
plt.title('v')
plt.legend(['vx', 'vy', 'vz'])
# 零偏
plt.subplot(324)
plt.plot(path_data[:, 0], path_data[:, 11], 'r')
plt.plot(path_data[:, 0], path_data[:, 12], 'g')
plt.plot(path_data[:, 0], path_data[:, 13], 'b')
plt.title('bias gyro')
plt.legend(['bg_x', 'bg_y', 'bg_z'])
plt.subplot(326)
plt.plot(path_data[:, 0], path_data[:, 14], 'r')
plt.plot(path_data[:, 0], path_data[:, 15], 'g')
plt.plot(path_data[:, 0], path_data[:, 16], 'b')
plt.title('bias acce')
plt.legend(['ba_x', 'ba_y', 'ba_z'])
# 3D轨迹
fig = plt.figure()
ax = Axes3D(fig)
ax.scatter(path_data[:, 1], path_data[:, 2], path_data[:, 3], s=2)
plt.show()
exit(1)

18
workspace/slam_in_autonomous_driving/scripts/plot_ch5_results.py Normal file
View File

@@ -0,0 +1,18 @@
# coding=UTF-8
import sys
import numpy as np
import matplotlib.pyplot as plt
method_names = ['Brute-force', 'Brute-force MT', '2D Grid8', '2D Grid8 MT', '3D Grid', '3D Grid MT', 'KD Tree',
'KD Tree MT',
'KD Tree PCL', 'Octo Tree', 'Octo Tree MT']
time_usage = [821.109, 102.074, 7.6908, 0.7044, 3.1735, 0.3916, 9.6234, 2.0651, 11.8725, 31.9253, 4.7698]
fig, ax = plt.subplots()
plt.bar(method_names, time_usage, color='rgbky')
plt.title('NN methods')
ax.set_yscale('log')
# plt.xlabel("method name")
plt.ylabel("runtime(s)")
plt.grid()
plt.show()

39
workspace/slam_in_autonomous_driving/scripts/plot_ch7_align_results.py Normal file
View File

@@ -0,0 +1,39 @@
# coding=UTF-8
import sys
import numpy as np
import matplotlib.pyplot as plt
method_names = ['Point-Point ICP', 'Point-Plane ICP', 'Point-Line ICP', 'NDT', 'PCL ICP', 'PCL NDT']
time_usage = [399.808, 334.409, 492.718, 142.278, 2774.16, 255.947]
res_icp_point = [0.211044, 0.155513, 0.114218, 0.0837873, 0.0611407, 0.0444493, 0.0320967, 0.0229813]
res_icp_plane = [0.0946098, 0.00664127, 0.000288226]
res_icp_line = [0.20137, 0.138774, 0.0931768, 0.0618093, 0.0410299, 0.0276539, 0.0192892]
res_ndt = [0.148351, 0.0597357, 0.0227735, 0.0100937, 0.00690136]
res_icp_pcl = [0.0483]
res_ndt_pcl = [0.1603]
# 用时
plt.figure()
plt.bar(method_names, time_usage, color='rgbkyc')
plt.title('Align Time Usage')
plt.xlabel("method name")
plt.ylabel("runtime(ms)")
plt.grid()
plt.show()
# 收敛曲线
plt.figure()
plt.plot(range(0, len(res_icp_point)), res_icp_point, color='r', marker='o')
plt.plot(range(0, len(res_icp_plane)), res_icp_plane, color='g', marker='o')
plt.plot(range(0, len(res_icp_line)), res_icp_line, color='b', marker='o')
plt.plot(range(0, len(res_ndt)), res_ndt, color='k', marker='o')
plt.axhline(res_icp_pcl, color='y')
plt.axhline(res_ndt_pcl, color='c')
# plt.yscale('log')
plt.title('Pose Convergence Speed')
plt.xlabel("iteration")
plt.ylabel("Pose error")
plt.legend(method_names)
plt.grid()
plt.show()