Loading...
Searching...
No Matches
Scenario.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/navigation/NavState.h>
namespace gtsam {
class Scenario {
public:
virtual ~Scenario() {}
// Quantities a Scenario needs to specify:
virtual Pose3 pose(double t) const = 0;
virtual Vector3 omega_b(double t) const = 0;
virtual Vector3 velocity_n(double t) const = 0;
virtual Vector3 acceleration_n(double t) const = 0;
// Derived quantities:
Rot3 rotation(double t) const { return pose(t).rotation(); }
NavState navState(double t) const { return NavState(pose(t), velocity_n(t)); }
Vector3 velocity_b(double t) const {
const Rot3 nRb = rotation(t);
return nRb.transpose() * velocity_n(t);
}
Vector3 acceleration_b(double t) const {
const Rot3 nRb = rotation(t);
return nRb.transpose() * acceleration_n(t);
}
};
class ConstantTwistScenario : public Scenario {
public:
ConstantTwistScenario(const Vector3& w, const Vector3& v,
const Pose3& nTb0 = Pose3())
: twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)), nTb0_(nTb0) {}
Pose3 pose(double t) const override {
return nTb0_ * Pose3::Expmap(twist_ * t);
}
Vector3 omega_b(double t) const override { return twist_.head<3>(); }
Vector3 velocity_n(double t) const override {
return rotation(t).matrix() * twist_.tail<3>();
}
Vector3 acceleration_n(double t) const override { return rotation(t) * a_b_; }
private:
const Vector6 twist_;
const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b
const Pose3 nTb0_;
};
class AcceleratingScenario : public Scenario {
public:
AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0,
const Vector3& a_n,
const Vector3& omega_b = Vector3::Zero())
: nRb_(nRb), p0_(p0), v0_(v0), a_n_(a_n), omega_b_(omega_b) {}
Pose3 pose(double t) const override {
return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0);
}
Vector3 omega_b(double t) const override { return omega_b_; }
Vector3 velocity_n(double t) const override { return v0_ + a_n_ * t; }
Vector3 acceleration_n(double t) const override { return a_n_; }
private:
const Rot3 nRb_;
const Vector3 p0_, v0_, a_n_, omega_b_;
};
} // namespace gtsam
NoiseModel.h
NavState.h
Navigation state composing of attitude, position, and velocity.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::cross
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition Point3.cpp:64
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::Pose3::Expmap
static Pose3 Expmap(const Vector6 &xi, OptionalJacobian< 6, 6 > Hxi=boost::none)
Exponential map at identity - create a rotation from canonical coordinates .
Definition Pose3.cpp:169
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition Pose3.cpp:315
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
gtsam::Scenario
Simple trajectory simulator.
Definition Scenario.h:25
gtsam::Scenario::pose
virtual Pose3 pose(double t) const =0
pose at time t
gtsam::Scenario::acceleration_n
virtual Vector3 acceleration_n(double t) const =0
acceleration in nav frame
gtsam::Scenario::velocity_n
virtual Vector3 velocity_n(double t) const =0
velocity at time t, in nav frame
gtsam::Scenario::~Scenario
virtual ~Scenario()
virtual destructor
Definition Scenario.h:28
gtsam::Scenario::omega_b
virtual Vector3 omega_b(double t) const =0
angular velocity in body frame
gtsam::ConstantTwistScenario::ConstantTwistScenario
ConstantTwistScenario(const Vector3 &w, const Vector3 &v, const Pose3 &nTb0=Pose3())
Construct scenario with constant twist [w,v].
Definition Scenario.h:63
gtsam::ConstantTwistScenario::acceleration_n
Vector3 acceleration_n(double t) const override
acceleration in nav frame
Definition Scenario.h:74
gtsam::ConstantTwistScenario::omega_b
Vector3 omega_b(double t) const override
angular velocity in body frame
Definition Scenario.h:70
gtsam::ConstantTwistScenario::pose
Pose3 pose(double t) const override
pose at time t
Definition Scenario.h:67
gtsam::ConstantTwistScenario::velocity_n
Vector3 velocity_n(double t) const override
velocity at time t, in nav frame
Definition Scenario.h:71
gtsam::AcceleratingScenario::pose
Pose3 pose(double t) const override
pose at time t
Definition Scenario.h:92
gtsam::AcceleratingScenario::omega_b
Vector3 omega_b(double t) const override
angular velocity in body frame
Definition Scenario.h:95
gtsam::AcceleratingScenario::AcceleratingScenario
AcceleratingScenario(const Rot3 &nRb, const Point3 &p0, const Vector3 &v0, const Vector3 &a_n, const Vector3 &omega_b=Vector3::Zero())
Construct scenario with constant acceleration in navigation frame and optional angular velocity in bo...
Definition Scenario.h:87
gtsam::AcceleratingScenario::acceleration_n
Vector3 acceleration_n(double t) const override
acceleration in nav frame
Definition Scenario.h:97
gtsam::AcceleratingScenario::velocity_n
Vector3 velocity_n(double t) const override
velocity at time t, in nav frame
Definition Scenario.h:96