Merge pull request #653 from mandulaj/master

add XYZ axis labels camera names to 3D plot
ethz-asl · Jan 16, 2024 · 28e31ae · 28e31ae
2 parents 10ad876 a27ac69
commit 28e31ae
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 11 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.
     
@@ -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')
 
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):