From 5b430da31601006aa83d1db438f959dfe15b1daa Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 26 Jan 2016 16:47:45 -0800 Subject: [PATCH] Simpler calculation of acceleration --- gtsam/navigation/Scenario.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index a6c005d7c..5544ae4b3 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -57,8 +57,7 @@ class ConstantTwistScenario : public Scenario { public: /// Construct scenario with constant twist [w,v] ConstantTwistScenario(const Vector3& w, const Vector3& v) - : twist_((Vector6() << w, v).finished()), - a_b_(twist_.head<3>().cross(twist_.tail<3>())) {} + : twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)) {} Pose3 pose(double t) const override { return Pose3::Expmap(twist_ * t); } Vector3 omega_b(double t) const override { return twist_.head<3>(); }