Merge pull request #653 from mandulaj/master

add XYZ axis labels + camera names to 3D plot

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.
     
@@ -35,12 +35,14 @@ def plotCoordinateFrame(axis, T_0f, size=1, linewidth=3):
 
     p_f = numpy.array([ [ 0,0,0,1], [size,0,0,1], [0,size,0,1], [0,0,size,1]]).T;
     p_0 = numpy.dot(T_0f,p_f)
-    # X-axis
-
-    X = numpy.append( [p_0[:,0].T] , [p_0[:,1].T], axis=0 )
-    Y = numpy.append( [p_0[:,0].T] , [p_0[:,2].T], axis=0 )
-    Z = numpy.append( [p_0[:,0].T] , [p_0[:,3].T], axis=0 )
-    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)
+
+    X = numpy.append([p_0[:,0].T], [p_0[:,1].T], axis=0 )
+    Y = numpy.append([p_0[:,0].T], [p_0[:,2].T], axis=0 )
+    Z = numpy.append([p_0[:,0].T], [p_0[:,3].T], axis=0 )
+    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')
 

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