gtsam  4.1.0
gtsam
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;
84 
87 
89  double range(const PoseRTV& other,
90  OptionalJacobian<1,9> H1=boost::none,
91  OptionalJacobian<1,9> H2=boost::none) const;
93 
96 
99  PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const;
100 
105  PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const;
106 
108  PoseRTV generalDynamics(const Vector& accel, const Vector& gyro, double dt) const;
109 
113  Vector6 imuPrediction(const PoseRTV& x2, double dt) const;
114 
118  Point3 translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const;
119 
123  inline Point3 translationIntegration(const PoseRTV& x2, double dt) const {
124  return translationIntegration(x2.rotation(), x2.velocity(), dt);
125  }
126 
128  inline Vector translationIntegrationVec(const PoseRTV& x2, double dt) const {
129  return translationIntegration(x2, dt);
130  }
131 
139  PoseRTV transformed_from(const Pose3& trans,
140  ChartJacobian Dglobal = boost::none,
141  OptionalJacobian<9, 6> Dtrans = boost::none) const;
142 
146 
149  static Matrix RRTMbn(const Vector3& euler);
150  static Matrix RRTMbn(const Rot3& att);
151 
154  static Matrix RRTMnb(const Vector3& euler);
155  static Matrix RRTMnb(const Rot3& att);
157 
158 private:
160  friend class boost::serialization::access;
161  template<class Archive>
162  void serialize(Archive & ar, const unsigned int /*version*/) {
163  ar & BOOST_SERIALIZATION_NVP(first);
164  ar & BOOST_SERIALIZATION_NVP(second);
165  }
166 };
167 
168 
169 template<>
170 struct traits<PoseRTV> : public internal::LieGroup<PoseRTV> {};
171 
172 // Define Range functor specializations that are used in RangeFactor
173 template <typename A1, typename A2> struct Range;
174 
175 template<>
176 struct Range<PoseRTV, PoseRTV> : HasRange<PoseRTV, PoseRTV, double> {};
177 
178 } // \namespace gtsam
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition: Pose3.cpp:259
gtsam::PoseRTV::translationIntegration
Point3 translationIntegration(const PoseRTV &x2, double dt) const
predict measurement and where Point3 for x2 should be, as a way of enforcing a velocity constraint Th...
Definition: PoseRTV.h:123
ProductLieGroup.h
Group product of two Lie Groups.
gtsam::HasRange
Definition: BearingRange.h:193
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition: Lie.h:228
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
Pose3.h
3D Pose
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Velocity3
Vector3 Velocity3
Velocity is currently typedef'd to Vector3.
Definition: NavState.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::Rot3
Definition: Rot3.h:59
gtsam::trans
Matrix trans(const Matrix &A)
static transpose function, just calls Eigen transpose member function
Definition: Matrix.h:245
gtsam::Range
Definition: BearingRange.h:39
gtsam::ProductLieGroup
Template to construct the product Lie group of two other Lie groups Assumes Lie group structure for G...
Definition: ProductLieGroup.h:29
gtsam::Pose3
Definition: Pose3.h:37
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:35
gtsam::PoseRTV
Robot state for use with IMU measurements.
Definition: PoseRTV.h:23
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition: Pose3.cpp:266
gtsam::PoseRTV::translationIntegrationVec
Vector translationIntegrationVec(const PoseRTV &x2, double dt) const
Definition: PoseRTV.h:128