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