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

Integrate the IMU, plot the prediction

release/4.3a0
Frank 2016-01-26 18:13:28 -08:00
parent 5f491ac52f
commit ae867e8d6e
2 changed files with 28 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
python/gtsam_examples/ImuFactorExample.py
View File

@ -36,7 +36,7 @@ class ImuFactorExample(object):
W = np.array([0, -w, 0]) W = np.array([0, -w, 0])
V = np.array([v, 0, 0]) V = np.array([v, 0, 0])
self.scenario = gtsam.ConstantTwistScenario(W, V) self.scenario = gtsam.ConstantTwistScenario(W, V)
self.dt = 0.5 self.dt = 0.25
self.realTimeFactor = 10.0 self.realTimeFactor = 10.0
# Calculate time to do 1 loop # Calculate time to do 1 loop
@ -50,6 +50,7 @@ class ImuFactorExample(object):
self.g = 10 # simple gravity constant self.g = 10 # simple gravity constant
self.params = self.defaultParams(self.g) self.params = self.defaultParams(self.g)
self.runner = gtsam.ScenarioRunner(gtsam.ScenarioPointer(self.scenario), self.params, dt) self.runner = gtsam.ScenarioRunner(gtsam.ScenarioPointer(self.scenario), self.params, dt)
self.estimatedBias = gtsam.ConstantBias()
def plot(self, t, measuredOmega, measuredAcc): def plot(self, t, measuredOmega, measuredAcc):
# plot angular velocity # plot angular velocity
@ -77,9 +78,12 @@ class ImuFactorExample(object):
plt.scatter(t, acceleration_b[i], color=color, marker='.') plt.scatter(t, acceleration_b[i], color=color, marker='.')
plt.xlabel(label) plt.xlabel(label)
# plot ground truth pose # plot ground truth pose, as well as prediction from integrated IMU measurements
pose = self.scenario.pose(t) actualPose = self.scenario.pose(t)
plotPose3(4, pose, 1.0) plotPose3(4, actualPose, 1.0)
pim = self.runner.integrate(t, self.estimatedBias, False)
predictedNavState = self.runner.predict(pim, self.estimatedBias)
plotPose3(4, predictedNavState.pose(), 1.0)
ax = plt.gca() ax = plt.gca()
ax.set_xlim3d(-self.radius, self.radius) ax.set_xlim3d(-self.radius, self.radius)
ax.set_ylim3d(-self.radius, self.radius) ax.set_ylim3d(-self.radius, self.radius)

21
python/handwritten/navigation/Scenario.cpp
View File

@ -67,11 +67,30 @@ void exportScenario() {
.def("MakeSharedU", &PreintegrationParams::MakeSharedU) .def("MakeSharedU", &PreintegrationParams::MakeSharedU)
.staticmethod("MakeSharedU"); .staticmethod("MakeSharedU");
class_<PreintegratedImuMeasurements>(
"PreintegratedImuMeasurements",
init<const boost::shared_ptr<PreintegrationParams>&,
const imuBias::ConstantBias&>());
class_<NavState>("NavState", init<>())
// TODO(frank): overload with jacobians
// .def("attitude", &NavState::attitude)
// .def("position", &NavState::position)
// .def("velocity", &NavState::velocity)
.def("pose", &NavState::pose);
class_<imuBias::ConstantBias>("ConstantBias", init<>());
class_<ScenarioRunner>( class_<ScenarioRunner>(
"ScenarioRunner", "ScenarioRunner",
init<const Scenario*, const boost::shared_ptr<PreintegrationParams>&, init<const Scenario*, const boost::shared_ptr<PreintegrationParams>&,
double>()) double>())
.def("actualSpecificForce", &ScenarioRunner::actualSpecificForce) .def("actualSpecificForce", &ScenarioRunner::actualSpecificForce)
.def("measuredAngularVelocity", &ScenarioRunner::measuredAngularVelocity) .def("measuredAngularVelocity", &ScenarioRunner::measuredAngularVelocity)
.def("measuredSpecificForce", &ScenarioRunner::measuredSpecificForce); .def("measuredSpecificForce", &ScenarioRunner::measuredSpecificForce)
.def("imuSampleTime", &ScenarioRunner::imuSampleTime,
return_value_policy<copy_const_reference>())
.def("integrate", &ScenarioRunner::integrate)
.def("predict", &ScenarioRunner::predict)
.def("estimateCovariance", &ScenarioRunner::estimateCovariance);
} }