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

Acceleration now specified in nav frame, allow angular velocity

release/4.3a0
Frank Dellaert 2015-12-23 14:29:42 -08:00
parent 9eb7e38cb8
commit 31335608a8
3 changed files with 28 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/navigation/Scenario.h
View File

@ -67,24 +67,26 @@ class ExpmapScenario : public Scenario {
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
}; };
/// Accelerating from an arbitrary initial state /// Accelerating from an arbitrary initial state, with optional rotation
class AcceleratingScenario : public Scenario { class AcceleratingScenario : public Scenario {
public: public:
/// Construct scenario with constant twist [w,v] /// Construct scenario with constant acceleration in navigation frame and
/// optional angular velocity in body: rotating while translating...
AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0, AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0,
const Vector3& accelerationInBody) const Vector3& a_n,
: nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(nRb_ * accelerationInBody) {} const Vector3& omega_b = Vector3::Zero())
: nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(a_n), omega_b_(omega_b) {}
Pose3 pose(double t) const { Pose3 pose(double t) const {
return Pose3(nRb_, p0_ + v0_ * t + a_n_ * t * t / 2.0); return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0);
} }
Vector3 omega_b(double t) const { return Vector3::Zero(); } Vector3 omega_b(double t) const { return omega_b_; }
Vector3 velocity_n(double t) const { return v0_ + a_n_ * t; } Vector3 velocity_n(double t) const { return v0_ + a_n_ * t; }
Vector3 acceleration_n(double t) const { return a_n_; } Vector3 acceleration_n(double t) const { return a_n_; }
private: private:
const Rot3 nRb_; const Rot3 nRb_;
const Vector3 p0_, v0_, a_n_; const Vector3 p0_, v0_, a_n_, omega_b_;
}; };
} // namespace gtsam } // namespace gtsam

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

@ -16,7 +16,10 @@
*/ */
#include <gtsam/navigation/Scenario.h> #include <gtsam/navigation/Scenario.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <cmath> #include <cmath>
using namespace std; using namespace std;
@ -87,18 +90,25 @@ TEST(Scenario, Accelerating) {
const Point3 P0(10, 20, 0); const Point3 P0(10, 20, 0);
const Vector3 V0(50, 0, 0); const Vector3 V0(50, 0, 0);
const double a_b = 0.2; // m/s^2 const double a = 0.2; // m/s^2
const AcceleratingScenario scenario(nRb, P0, V0, Vector3(a_b, 0, 0)); const Vector3 A(0, a, 0), W(0.1, 0.2, 0.3);
const AcceleratingScenario scenario(nRb, P0, V0, A, W);
const double T = 3; const double T = 3;
const Vector3 A = nRb * Vector3(a_b, 0, 0); EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
EXPECT(assert_equal(Vector3(0, 0, 0), scenario.omega_b(T), 1e-9));
EXPECT(assert_equal(Vector3(V0 + T * A), scenario.velocity_n(T), 1e-9)); EXPECT(assert_equal(Vector3(V0 + T * A), scenario.velocity_n(T), 1e-9));
EXPECT(assert_equal(A, scenario.acceleration_n(T), 1e-9)); EXPECT(assert_equal(A, scenario.acceleration_n(T), 1e-9));
{
// Check acceleration in nav
Matrix expected = numericalDerivative11<Vector3, double>(
boost::bind(&Scenario::velocity_n, scenario, _1), T);
EXPECT(assert_equal(Vector3(expected), scenario.acceleration_n(T), 1e-9));
}
const Pose3 T3 = scenario.pose(3); const Pose3 T3 = scenario.pose(3);
EXPECT(assert_equal(nRb, T3.rotation(), 1e-9)); EXPECT(assert_equal(nRb.expmap(T * W), T3.rotation(), 1e-9));
EXPECT(assert_equal(Point3(10 + T * 50, 20 + a_b * T * T / 2, 0), EXPECT(assert_equal(Point3(10 + T * 50, 20 + a * T * T / 2, 0),
T3.translation(), 1e-9)); T3.translation(), 1e-9));
} }

5
gtsam/navigation/tests/testScenarioRunner.cpp
View File

@ -83,8 +83,9 @@ TEST(ScenarioRunner, Accelerating) {
const Point3 P0(10, 20, 0); const Point3 P0(10, 20, 0);
const Vector3 V0(50, 0, 0); const Vector3 V0(50, 0, 0);
const double a_b = 0.2; // m/s^2 const double a = 0.2; // m/s^2
const AcceleratingScenario scenario(nRb, P0, V0, Vector3(a_b, 0, 0)); const Vector3 A(0, a, 0);
const AcceleratingScenario scenario(nRb, P0, V0, A);
const double T = 3; // seconds const double T = 3; // seconds
ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma); ScenarioRunner runner(&scenario, T / 10, kGyroSigma, kAccelerometerSigma);