From edbe492b43d504dde11e007ca83412ce2a5de26f Mon Sep 17 00:00:00 2001 From: Jakub Mandula Date: Wed, 29 Nov 2023 16:37:38 +0100 Subject: [PATCH 1/2] add XYZ axis labels + camera names to 3D plot --- .../sm_python/python/sm/plotCoordinateFrame.py | 7 ++++++- .../kalibr/python/kalibr_camera_calibration/CameraUtils.py | 7 +++++-- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/Schweizer-Messer/sm_python/python/sm/plotCoordinateFrame.py b/Schweizer-Messer/sm_python/python/sm/plotCoordinateFrame.py index bb9ebba18..d970fffca 100755 --- 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 index 1ce1747db..67b14362b 100644 --- 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): From a27ac691b9bf19622226f36d91b44749b8bb34d8 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 15 Jan 2024 22:46:59 -0800 Subject: [PATCH 2/2] small formating changes --- .../sm_python/python/sm/plotCoordinateFrame.py | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/Schweizer-Messer/sm_python/python/sm/plotCoordinateFrame.py b/Schweizer-Messer/sm_python/python/sm/plotCoordinateFrame.py index d970fffca..6269a7026 100755 --- a/Schweizer-Messer/sm_python/python/sm/plotCoordinateFrame.py +++ b/Schweizer-Messer/sm_python/python/sm/plotCoordinateFrame.py @@ -35,17 +35,14 @@ def plotCoordinateFrame(axis, T_0f, size=1, linewidth=3, name=None): 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') + axis.text(X[0,0],X[0,1],X[0,2], name, zdir='x')