From d147df5ba77b7898f1aa8b6f68a3ae2abe0c8dbe Mon Sep 17 00:00:00 2001 From: Ryohei Sasaki Date: Sat, 7 Sep 2019 15:02:36 +0900 Subject: [PATCH] Make 3D graph correctly scaled --- .../pose_optimization_slam_3d.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/SLAM/PoseOptimizationSLAM3D/pose_optimization_slam_3d.py b/SLAM/PoseOptimizationSLAM3D/pose_optimization_slam_3d.py index 105201f8..c9b72e88 100644 --- a/SLAM/PoseOptimizationSLAM3D/pose_optimization_slam_3d.py +++ b/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):