github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-12 21:18:09 -05:00
Code Issues Packages Projects Releases Wiki Activity

Make 3D graph correctly scaled

Browse Source
This commit is contained in:
Ryohei Sasaki
2019-09-07 15:02:36 +09:00
committed by GitHub
parent 01a5bcb419
commit d147df5ba7
1 changed files with 10 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
SLAM/PoseOptimizationSLAM3D/pose_optimization_slam_3d.py
View File

@@ -434,6 +434,16 @@ def plot_nodes(nodes, ax, color ="-r", label = ""):
x, y, z = [], [], []
for n in nodes:
x.append(n.x); y.append(n.y); z.append(n.z)
x = np.array(x)
y = np.array(y)
z = np.array(z)
max_range = np.array([x.max()-x.min(), y.max()-y.min(), z.max()-z.min()]).max() / 2.0
mid_x = (x.max()+x.min()) * 0.5
mid_y = (y.max()+y.min()) * 0.5
mid_z = (z.max()+z.min()) * 0.5
ax.set_xlim(mid_x - max_range, mid_x + max_range)
ax.set_ylim(mid_y - max_range, mid_y + max_range)
ax.set_zlim(mid_z - max_range, mid_z + max_range)
ax.plot(x, y, z, color, label=label)
def load_data(fname):