Make 3D graph correctly scaled

Author: Ryohei Sasaki

SLAM/PoseOptimizationSLAM3D/pose_optimization_slam_3d.py

@@ -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):