12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Cleaned up plot

release/4.3a0
dellaert 2016-01-28 00:57:24 -08:00
parent 85e231bea5
commit 9dbe61a05e
2 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
python/gtsam_utils/__init__.py
View File

@ -1 +1 @@
from ._plot import * from .plot import *

34
python/gtsam_utils/_plot.py → python/gtsam_utils/plot.py
View File

@ -1,11 +1,11 @@
import numpy as _np import numpy as np
import matplotlib.pyplot as _plt import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D as _Axes3D from mpl_toolkits.mplot3d import Axes3D as _Axes3D
def plotPoint3(fignum, point, linespec): def plotPoint3(fignum, point, linespec):
fig = _plt.figure(fignum) fig = plt.figure(fignum)
ax = fig.gca(projection='3d') ax = fig.gca(projection='3d')
ax.plot([point.x()],[point.y()],[point.z()], linespec) ax.plot([point.x()], [point.y()], [point.z()], linespec)
def plot3DPoints(fignum, values, linespec, marginals=None): def plot3DPoints(fignum, values, linespec, marginals=None):
@ -19,7 +19,7 @@ def plot3DPoints(fignum, values, linespec, marginals=None):
# Plot points and covariance matrices # Plot points and covariance matrices
for key in keys: for key in keys:
try: try:
p = values.point3_at(key); p = values.atPoint3(key);
# if haveMarginals # if haveMarginals
# P = marginals.marginalCovariance(key); # P = marginals.marginalCovariance(key);
# gtsam.plotPoint3(p, linespec, P); # gtsam.plotPoint3(p, linespec, P);
@ -27,11 +27,11 @@ def plot3DPoints(fignum, values, linespec, marginals=None):
plotPoint3(fignum, p, linespec); plotPoint3(fignum, p, linespec);
except RuntimeError: except RuntimeError:
continue continue
#I guess it's not a Point3 # I guess it's not a Point3
def plotPose3(fignum, pose, axisLength=0.1): def plotPose3(fignum, pose, axisLength=0.1):
# get figure object # get figure object
fig = _plt.figure(fignum) fig = plt.figure(fignum)
ax = fig.gca(projection='3d') ax = fig.gca(projection='3d')
# get rotation and translation (center) # get rotation and translation (center)
@ -39,21 +39,21 @@ def plotPose3(fignum, pose, axisLength=0.1):
C = pose.translation().vector() C = pose.translation().vector()
# draw the camera axes # draw the camera axes
xAxis = C+gRp[:,0]*axisLength xAxis = C + gRp[:, 0] * axisLength
L = _np.append(C[_np.newaxis], xAxis[_np.newaxis], axis=0) L = np.append(C[np.newaxis], xAxis[np.newaxis], axis=0)
ax.plot(L[:,0],L[:,1],L[:,2],'r-') ax.plot(L[:, 0], L[:, 1], L[:, 2], 'r-')
yAxis = C+gRp[:,1]*axisLength yAxis = C + gRp[:, 1] * axisLength
L = _np.append(C[_np.newaxis], yAxis[_np.newaxis], axis=0) L = np.append(C[np.newaxis], yAxis[np.newaxis], axis=0)
ax.plot(L[:,0],L[:,1],L[:,2],'g-') ax.plot(L[:, 0], L[:, 1], L[:, 2], 'g-')
zAxis = C+gRp[:,2]*axisLength zAxis = C + gRp[:, 2] * axisLength
L = _np.append(C[_np.newaxis], zAxis[_np.newaxis], axis=0) L = np.append(C[np.newaxis], zAxis[np.newaxis], axis=0)
ax.plot(L[:,0],L[:,1],L[:,2],'b-') ax.plot(L[:, 0], L[:, 1], L[:, 2], 'b-')
# # plot the covariance # # plot the covariance
# if (nargin>2) && (~isempty(P)) # if (nargin>2) && (~isempty(P))
# pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame # pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame
# gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame # gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
# gtsam.covarianceEllipse3D(C,gPp); # gtsam.covarianceEllipse3D(C,gPp);
# end # end