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

Added optional initial pose in ConstantTwistScenario

release/4.3a0
Frank Dellaert 2018-10-16 16:14:24 -04:00
parent d751c65fab
commit 4868a36b6c
2 changed files with 31 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/navigation/Scenario.h
View File

@ -57,10 +57,13 @@ class Scenario {
class ConstantTwistScenario : public Scenario { class ConstantTwistScenario : public Scenario {
public: public:
/// Construct scenario with constant twist [w,v] /// Construct scenario with constant twist [w,v]
ConstantTwistScenario(const Vector3& w, const Vector3& v) ConstantTwistScenario(const Vector3& w, const Vector3& v,
: twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)) {} const Pose3& nTb0 = Pose3())
: twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)), nTb0_(nTb0) {}
Pose3 pose(double t) const override { return Pose3::Expmap(twist_ * t); } Pose3 pose(double t) const override {
return nTb0_ * Pose3::Expmap(twist_ * t);
}
Vector3 omega_b(double t) const override { return twist_.head<3>(); } Vector3 omega_b(double t) const override { return twist_.head<3>(); }
Vector3 velocity_n(double t) const override { Vector3 velocity_n(double t) const override {
return rotation(t).matrix() * twist_.tail<3>(); return rotation(t).matrix() * twist_.tail<3>();
@ -70,6 +73,7 @@ class ConstantTwistScenario : public Scenario {
private: private:
const Vector6 twist_; const Vector6 twist_;
const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b
const Pose3 nTb0_;
}; };
/// Accelerating from an arbitrary initial state, with optional rotation /// Accelerating from an arbitrary initial state, with optional rotation

25
gtsam/navigation/tests/testScenario.cpp
View File

@ -15,8 +15,8 @@
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include <gtsam/navigation/Scenario.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/navigation/Scenario.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp> #include <boost/bind.hpp>
@ -99,6 +99,29 @@ TEST(Scenario, Loop) {
EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9)); EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9));
} }
/* ************************************************************************* */
TEST(Scenario, LoopWithInitialPose) {
// Forward velocity 2m/s
// Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
const double v = 2, w = 6 * kDegree;
const Vector3 W(0, -w, 0), V(v, 0, 0);
const Rot3 nRb0 = Rot3::yaw(M_PI);
const Pose3 nTb0(nRb0, Point3(1, 2, 3));
const ConstantTwistScenario scenario(W, V, nTb0);
const double T = 30;
EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
// R = v/w, so test if loop crests at 2*R
const double R = v / w;
const Pose3 T30 = scenario.pose(30);
EXPECT(
assert_equal(nRb0 * Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9));
EXPECT(assert_equal(Point3(1, 2, 3 + 2 * R), T30.translation(), 1e-9));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Scenario, Accelerating) { TEST(Scenario, Accelerating) {
// Set up body pointing towards y axis, and start at 10,20,0 with velocity // Set up body pointing towards y axis, and start at 10,20,0 with velocity