Skip to content

Commit

Permalink
Merge pull request #653 from mandulaj/master
Browse files Browse the repository at this point in the history
add XYZ axis labels + camera names to 3D plot
  • Loading branch information
goldbattle authored Jan 16, 2024
2 parents 10ad876 + a27ac69 commit 28e31ae
Show file tree
Hide file tree
Showing 2 changed files with 16 additions and 11 deletions.
20 changes: 11 additions & 9 deletions Schweizer-Messer/sm_python/python/sm/plotCoordinateFrame.py
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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')

Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand All @@ -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):
Expand Down

0 comments on commit 28e31ae

Please sign in to comment.