9 #include <gtsam_unstable/dllexport.h> 34 : Base(
Pose3(rot, t), vel) {}
36 : Base(
Pose3(rot, t), vel) {}
49 PoseRTV(
double roll,
double pitch,
double yaw,
double x,
double y,
double z,
50 double vx,
double vy,
double vz);
57 const Velocity3&
v()
const {
return second; }
64 const Velocity3&
velocity()
const {
return second; }
74 void print(
const std::string&
s=
"")
const;
78 using Base::dimension;
82 using Base::localCoordinates;
83 using Base::LocalCoordinates;
100 PoseRTV planarDynamics(
double vel_rate,
double heading_rate,
double max_accel,
double dt)
const;
106 PoseRTV flyingDynamics(
double pitch_rate,
double heading_rate,
double lift_control,
double dt)
const;
114 Vector6 imuPrediction(
const PoseRTV&
x2,
double dt)
const;
119 Point3 translationIntegration(
const Rot3&
r2,
const Velocity3&
v2,
double dt)
const;
130 return translationIntegration(x2, dt);
141 ChartJacobian Dglobal = {},
160 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION 162 friend class boost::serialization::access;
163 template<
class Archive>
164 void serialize(Archive & ar,
const unsigned int ) {
165 ar & BOOST_SERIALIZATION_NVP(first);
166 ar & BOOST_SERIALIZATION_NVP(second);
176 template <
typename A1,
typename A2>
struct Range;
void print(const Matrix &A, const string &s, ostream &stream)
int EIGEN_BLAS_FUNC() rot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar *pc, RealScalar *ps)
StridedVectorType vy(make_vector(y, *n, std::abs(*incy)))
const Point3 & translation() const
Vector translationVec() const
const Velocity3 & velocity() const
std::string serialize(const T &input)
serializes to a string
PoseRTV(const Point3 &t, const Rot3 &rot, const Velocity3 &vel)
static const Velocity3 vel(0.4, 0.5, 0.6)
Vector3 Velocity3
Velocity is currently typedef'd to Vector3.
PoseRTV(const Pose3 &pose, const Velocity3 &vel)
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
const Pose3 & pose() const
Group product of two Lie Groups.
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Matrix trans(const Matrix &A)
PoseRTV(const Base &base)
Vector translationIntegrationVec(const PoseRTV &x2, double dt) const
const Velocity3 & velocityVec() const
StridedVectorType vx(make_vector(x, *n, std::abs(*incx)))
Array< int, Dynamic, 1 > v
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Velocity3_ velocity(const NavState_ &X)
PoseRTV(const Rot3 &rot, const Point3 &t, const Velocity3 &vel)
ProductLieGroup< Pose3, Velocity3 > Base
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
PoseRTV(const Pose3 &pose)
const Rot3 & rotation() const
Both LieGroupTraits and Testable.
Double_ range(const Point2_ &p, const Point2_ &q)
OptionalJacobian< 9, 9 > ChartJacobian
Point3 translationIntegration(const PoseRTV &x2, double dt) const
Annotation indicating that a class derives from another given type.
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
const Velocity3 & v() const