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 *

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

@ -1,9 +1,9 @@
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)
@ -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);
@ -31,7 +31,7 @@ def plot3DPoints(fignum, values, linespec, marginals=None):
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)
@ -40,15 +40,15 @@ def plotPose3(fignum, pose, axisLength=0.1):
# 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