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

Fix IMU example

release/4.3a0
Frank Dellaert 2016-06-09 00:19:54 -07:00
parent 853d327725
commit b30a7685db
3 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
python/gtsam_examples/ImuFactorExample.py
View File

@ -69,7 +69,7 @@ class ImuFactorExample(PreintegrationExample):
# get measurements and add them to PIM # get measurements and add them to PIM
measuredOmega = self.runner.measuredAngularVelocity(t) measuredOmega = self.runner.measuredAngularVelocity(t)
measuredAcc = self.runner.measuredSpecificForce(t) measuredAcc = self.runner.measuredSpecificForce(t)
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt, H9, H9) pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
# Plot IMU many times # Plot IMU many times
if k % 10 == 0: if k % 10 == 0:

2
python/gtsam_examples/PreintegrationExample.py
View File

@ -101,7 +101,7 @@ class PreintegrationExample(object):
actualPose = self.scenario.pose(t) actualPose = self.scenario.pose(t)
plotPose3(POSES_FIG, actualPose, 0.3) plotPose3(POSES_FIG, actualPose, 0.3)
t = actualPose.translation() t = actualPose.translation()
self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), self.maxDim]) self.maxDim = max([abs(t[0]), abs(t[1]), abs(t[2]), self.maxDim])
ax = plt.gca() ax = plt.gca()
ax.set_xlim3d(-self.maxDim, self.maxDim) ax.set_xlim3d(-self.maxDim, self.maxDim)
ax.set_ylim3d(-self.maxDim, self.maxDim) ax.set_ylim3d(-self.maxDim, self.maxDim)

2
python/gtsam_utils/plot.py
View File

@ -33,7 +33,7 @@ def plot3DPoints(fignum, values, linespec, marginals=None):
def plotPose3OnAxes(ax, pose, axisLength=0.1): def plotPose3OnAxes(ax, pose, axisLength=0.1):
# get rotation and translation (center) # get rotation and translation (center)
gRp = pose.rotation().matrix() # rotation from pose to global gRp = pose.rotation().matrix() # rotation from pose to global
C = pose.translation().vector() C = pose.translation()
# draw the camera axes # draw the camera axes
xAxis = C + gRp[:, 0] * axisLength xAxis = C + gRp[:, 0] * axisLength