add XYZ axis labels + camera names to 3D plot

ethz-asl · Nov 29, 2023 · edbe492 · edbe492
1 parent c4a1d89
commit edbe492
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 3 deletions.
diff --git a/Schweizer-Messer/sm_python/python/sm/plotCoordinateFrame.py b/Schweizer-Messer/sm_python/python/sm/plotCoordinateFrame.py
@@ -3,7 +3,7 @@
 
 import matplotlib.axes as axes
 
-def plotCoordinateFrame(axis, T_0f, size=1, linewidth=3):
+def plotCoordinateFrame(axis, T_0f, size=1, linewidth=3, name=None):
     """Plot a coordinate frame on a 3d axis. In the resulting plot,
     x = red, y = green, z = blue.
     
@@ -43,4 +43,9 @@ def plotCoordinateFrame(axis, T_0f, size=1, linewidth=3):
     axis.plot3D(X[:,0],X[:,1],X[:,2],'r-', linewidth=linewidth)
     axis.plot3D(Y[:,0],Y[:,1],Y[:,2],'g-', linewidth=linewidth)
     axis.plot3D(Z[:,0],Z[:,1],Z[:,2],'b-', linewidth=linewidth)
+
+
+
+    if name is not None:
+        axis.text(X[0,0],X[0,1],X[0,2],name, zdir='x')
 
diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraUtils.py b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraUtils.py
@@ -604,8 +604,8 @@ def plotCameraRig(baselines, fno=1, clearFigure=True, title=""):
 
     #plot each frame in coordinates of first camera
     a3d = f.add_subplot(111, projection='3d')
-    for camera_frame in camera_frames:
-        sm.plotCoordinateFrame(a3d, camera_frame.T(), size=size)
+    for i, camera_frame in enumerate(camera_frames):
+        sm.plotCoordinateFrame(a3d, camera_frame.T(), size=size, name="cam{0}".format(i))
 
     #axis equal
     box_sizes=list()
@@ -614,6 +614,9 @@ def plotCameraRig(baselines, fno=1, clearFigure=True, title=""):
     box_size=1.25*np.max(box_sizes)+size
 
     a3d.auto_scale_xyz([-box_size, box_size], [-box_size, box_size], [-box_size, box_size])
+    a3d.set_xlabel('x')
+    a3d.set_ylabel('y')
+    a3d.set_zlabel('z')
 
 
 def exportPoses(cself, filename):