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

Fixed pose of accelerating trajectory

release/4.3a0
Frank Dellaert 2015-12-23 11:58:41 -08:00
parent dc2bac5a9e
commit ccef2faa95
2 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/navigation/Scenario.h
View File

@ -73,7 +73,9 @@ class AcceleratingScenario : public Scenario {
const Vector3& accelerationInBody)
: nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(nRb_ * accelerationInBody) {}
Pose3 pose(double t) const { return Pose3(nRb_, p0_ + a_n_ * t * t / 2.0); }
Pose3 pose(double t) const {
return Pose3(nRb_, p0_ + v0_ * t + a_n_ * t * t / 2.0);
}
Vector3 omega_b(double t) const { return Vector3::Zero(); }
Vector3 velocity_n(double t) const { return v0_ + a_n_ * t; }
Vector3 acceleration_n(double t) const { return a_n_; }

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

@ -98,8 +98,8 @@ TEST(Scenario, Accelerating) {
const Pose3 T3 = scenario.pose(3);
EXPECT(assert_equal(nRb, T3.rotation(), 1e-9));
EXPECT(assert_equal(Point3(P0.vector() + T * T * A / 2.0), T3.translation(),
1e-9));
EXPECT(assert_equal(Point3(10 + T * 50, 20 + a_b * T * T / 2, 0),
T3.translation(), 1e-9));
}
/* ************************************************************************* */