PoseRTV.h
Go to the documentation of this file.
1 
7 #pragma once
8 
9 #include <gtsam_unstable/dllexport.h>
10 #include <gtsam/geometry/Pose3.h>
12 
13 namespace gtsam {
14 
16 typedef Vector3 Velocity3;
17 
23 class GTSAM_UNSTABLE_EXPORT PoseRTV : public ProductLieGroup<Pose3,Velocity3> {
24 protected:
25 
28 
29 public:
30 
31  // constructors - with partial versions
32  PoseRTV() {}
33  PoseRTV(const Point3& t, const Rot3& rot, const Velocity3& vel)
34  : Base(Pose3(rot, t), vel) {}
35  PoseRTV(const Rot3& rot, const Point3& t, const Velocity3& vel)
36  : Base(Pose3(rot, t), vel) {}
37  explicit PoseRTV(const Point3& t)
38  : Base(Pose3(Rot3(), t),Vector3::Zero()) {}
39  PoseRTV(const Pose3& pose, const Velocity3& vel)
40  : Base(pose, vel) {}
41  explicit PoseRTV(const Pose3& pose)
42  : Base(pose,Vector3::Zero()) {}
43 
44  // Construct from Base
45  PoseRTV(const Base& base)
46  : Base(base) {}
47 
49  PoseRTV(double roll, double pitch, double yaw, double x, double y, double z,
50  double vx, double vy, double vz);
51 
53  explicit PoseRTV(const Vector& v);
54 
55  // access
56  const Pose3& pose() const { return first; }
57  const Velocity3& v() const { return second; }
58  const Point3& t() const { return pose().translation(); }
59  const Rot3& R() const { return pose().rotation(); }
60 
61  // longer function names
62  const Point3& translation() const { return pose().translation(); }
63  const Rot3& rotation() const { return pose().rotation(); }
64  const Velocity3& velocity() const { return second; }
65 
66  // Access to vector for ease of use with Matlab
67  // and avoidance of Point3
68  Vector vector() const;
69  Vector translationVec() const { return pose().translation(); }
70  const Velocity3& velocityVec() const { return velocity(); }
71 
72  // testable
73  bool equals(const PoseRTV& other, double tol=1e-6) const;
74  void print(const std::string& s="") const;
75 
78  using Base::dimension;
79  using Base::dim;
80  using Base::Dim;
81  using Base::retract;
82  using Base::localCoordinates;
83  using Base::LocalCoordinates;
85 
88 
90  double range(const PoseRTV& other,
92  OptionalJacobian<1,9> H2={}) const;
94 
97 
100  PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const;
101 
106  PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const;
107 
109  PoseRTV generalDynamics(const Vector& accel, const Vector& gyro, double dt) const;
110 
114  Vector6 imuPrediction(const PoseRTV& x2, double dt) const;
115 
119  Point3 translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const;
120 
124  inline Point3 translationIntegration(const PoseRTV& x2, double dt) const {
125  return translationIntegration(x2.rotation(), x2.velocity(), dt);
126  }
127 
129  inline Vector translationIntegrationVec(const PoseRTV& x2, double dt) const {
130  return translationIntegration(x2, dt);
131  }
132 
140  PoseRTV transformed_from(const Pose3& trans,
141  ChartJacobian Dglobal = {},
142  OptionalJacobian<9, 6> Dtrans = {}) const;
143 
147 
150  static Matrix RRTMbn(const Vector3& euler);
151  static Matrix RRTMbn(const Rot3& att);
152 
155  static Matrix RRTMnb(const Vector3& euler);
156  static Matrix RRTMnb(const Rot3& att);
158 
159 private:
160 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
161 
162  friend class boost::serialization::access;
163  template<class Archive>
164  void serialize(Archive & ar, const unsigned int /*version*/) {
165  ar & BOOST_SERIALIZATION_NVP(first);
166  ar & BOOST_SERIALIZATION_NVP(second);
167  }
168 #endif
169 };
170 
171 
172 template<>
173 struct traits<PoseRTV> : public internal::LieGroup<PoseRTV> {};
174 
175 // Define Range functor specializations that are used in RangeFactor
176 template <typename A1, typename A2> struct Range;
177 
178 template<>
179 struct Range<PoseRTV, PoseRTV> : HasRange<PoseRTV, PoseRTV, double> {};
180 
181 } // \namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
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
Definition: PoseRTV.h:62
Scalar * y
Vector v2
Vector translationVec() const
Definition: PoseRTV.h:69
Eigen::Vector3d Vector3
Definition: Vector.h:43
const Velocity3 & velocity() const
Definition: PoseRTV.h:64
std::string serialize(const T &input)
serializes to a string
PoseRTV(const Point3 &t, const Rot3 &rot, const Velocity3 &vel)
Definition: PoseRTV.h:33
static const Velocity3 vel(0.4, 0.5, 0.6)
Vector3 Velocity3
Velocity is currently typedef&#39;d to Vector3.
Definition: NavState.h:28
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
const Rot3 & R() const
Definition: PoseRTV.h:59
PoseRTV(const Pose3 &pose, const Velocity3 &vel)
Definition: PoseRTV.h:39
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
const Pose3 & pose() const
Definition: PoseRTV.h:56
Group product of two Lie Groups.
PoseRTV(const Point3 &t)
Definition: PoseRTV.h:37
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Matrix trans(const Matrix &A)
Definition: base/Matrix.h:241
PoseRTV(const Base &base)
Definition: PoseRTV.h:45
Vector translationIntegrationVec(const PoseRTV &x2, double dt) const
Definition: PoseRTV.h:129
const double dt
Eigen::VectorXd Vector
Definition: Vector.h:38
const Velocity3 & velocityVec() const
Definition: PoseRTV.h:70
StridedVectorType vx(make_vector(x, *n, std::abs(*incx)))
Array< int, Dynamic, 1 > v
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
static const double r2
Velocity3_ velocity(const NavState_ &X)
traits
Definition: chartTesting.h:28
PoseRTV(const Rot3 &rot, const Point3 &t, const Velocity3 &vel)
Definition: PoseRTV.h:35
ProductLieGroup< Pose3, Velocity3 > Base
Definition: PoseRTV.h:26
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
PoseRTV(const Pose3 &pose)
Definition: PoseRTV.h:41
const Rot3 & rotation() const
Definition: PoseRTV.h:63
Both LieGroupTraits and Testable.
Definition: Lie.h:229
Double_ range(const Point2_ &p, const Point2_ &q)
OptionalJacobian< 9, 9 > ChartJacobian
Definition: PoseRTV.h:27
const Point3 & t() const
Definition: PoseRTV.h:58
Point3 translationIntegration(const PoseRTV &x2, double dt) const
Definition: PoseRTV.h:124
const G double tol
Definition: Group.h:86
Annotation indicating that a class derives from another given type.
Definition: attr.h:61
Vector3 Point3
Definition: Point3.h:38
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
3D Pose
const Velocity3 & v() const
Definition: PoseRTV.h:57
Point2 t(10, 10)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:15