Fix IMU example
parent
853d327725
commit
b30a7685db
|
@ -69,7 +69,7 @@ class ImuFactorExample(PreintegrationExample):
|
|||
# get measurements and add them to PIM
|
||||
measuredOmega = self.runner.measuredAngularVelocity(t)
|
||||
measuredAcc = self.runner.measuredSpecificForce(t)
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt, H9, H9)
|
||||
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
|
||||
|
||||
# Plot IMU many times
|
||||
if k % 10 == 0:
|
||||
|
|
|
@ -101,7 +101,7 @@ class PreintegrationExample(object):
|
|||
actualPose = self.scenario.pose(t)
|
||||
plotPose3(POSES_FIG, actualPose, 0.3)
|
||||
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.set_xlim3d(-self.maxDim, self.maxDim)
|
||||
ax.set_ylim3d(-self.maxDim, self.maxDim)
|
||||
|
|
|
@ -33,7 +33,7 @@ def plot3DPoints(fignum, values, linespec, marginals=None):
|
|||
def plotPose3OnAxes(ax, pose, axisLength=0.1):
|
||||
# get rotation and translation (center)
|
||||
gRp = pose.rotation().matrix() # rotation from pose to global
|
||||
C = pose.translation().vector()
|
||||
C = pose.translation()
|
||||
|
||||
# draw the camera axes
|
||||
xAxis = C + gRp[:, 0] * axisLength
|
||||
|
|
Loading…
Reference in New Issue