1/**
2 * @file PoseRTV.h
3 * @brief Pose3 with translational velocity
4 * @author Alex Cunningham
5 */
6
7#pragma once
8
9#include <gtsam_unstable/dllexport.h>
10#include <gtsam/geometry/Pose3.h>
11#include <gtsam/base/ProductLieGroup.h>
12
13namespace gtsam {
14
15/// Syntactic sugar to clarify components
16typedef Vector3 Velocity3;
17
18/**
19 * Robot state for use with IMU measurements
20 * - contains translation, translational velocity and rotation
21 * TODO(frank): Alex should deprecate/move to project
22 */
23class GTSAM_UNSTABLE_EXPORT PoseRTV : public ProductLieGroup<Pose3,Velocity3> {
24protected:
25
26 typedef ProductLieGroup<Pose3,Velocity3> Base;
27 typedef OptionalJacobian<9, 9> ChartJacobian;
28
29public:
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
48 /** build from components - useful for data files */
49 PoseRTV(double roll, double pitch, double yaw, double x, double y, double z,
50 double vx, double vy, double vz);
51
52 /** build from single vector - useful for Matlab - in RtV format */
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
76 /// @name Manifold
77 /// @{
78 using Base::dimension;
79 using Base::dim;
80 using Base::Dim;
81 using Base::retract;
82 using Base::localCoordinates;
83 using Base::LocalCoordinates;
84 /// @}
85
86 /// @name measurement functions
87 /// @{
88
89 /** range between translations */
90 double range(const PoseRTV& other,
91 OptionalJacobian<1,9> H1={},
92 OptionalJacobian<1,9> H2={}) const;
93 /// @}
94
95 /// @name IMU-specific
96 /// @{
97
98 /// Dynamics integrator for ground robots
99 /// Always move from time 1 to time 2
100 PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const;
101
102 /// Simulates flying robot with simple flight model
103 /// Integrates state x1 -> x2 given controls
104 /// x1 = {p1, r1, v1}, x2 = {p2, r2, v2}, all in global coordinates
105 /// @return x2
106 PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const;
107
108 /// General Dynamics update - supply control inputs in body frame
109 PoseRTV generalDynamics(const Vector& accel, const Vector& gyro, double dt) const;
110
111 /// Dynamics predictor for both ground and flying robots, given states at 1 and 2
112 /// Always move from time 1 to time 2
113 /// @return imu measurement, as [accel, gyro]
114 Vector6 imuPrediction(const PoseRTV& x2, double dt) const;
115
116 /// predict measurement and where Point3 for x2 should be, as a way
117 /// of enforcing a velocity constraint
118 /// This version splits out the rotation and velocity for x2
119 Point3 translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const;
120
121 /// predict measurement and where Point3 for x2 should be, as a way
122 /// of enforcing a velocity constraint
123 /// This version takes a full PoseRTV, but ignores the existing translation for x2
124 inline Point3 translationIntegration(const PoseRTV& x2, double dt) const {
125 return translationIntegration(r2: x2.rotation(), v2: x2.velocity(), dt);
126 }
127
128 /// @return a vector for Matlab compatibility
129 inline Vector translationIntegrationVec(const PoseRTV& x2, double dt) const {
130 return translationIntegration(x2, dt);
131 }
132
133 /**
134 * Apply transform to this pose, with optional derivatives
135 * equivalent to:
136 * local = trans.transformFrom(global, Dtrans, Dglobal)
137 *
138 * Note: the transform jacobian convention is flipped
139 */
140 PoseRTV transformed_from(const Pose3& trans,
141 ChartJacobian Dglobal = {},
142 OptionalJacobian<9, 6> Dtrans = {}) const;
143
144 /// @}
145 /// @name Utility functions
146 /// @{
147
148 /// RRTMbn - Function computes the rotation rate transformation matrix from
149 /// body axis rates to euler angle (global) rates
150 static Matrix RRTMbn(const Vector3& euler);
151 static Matrix RRTMbn(const Rot3& att);
152
153 /// RRTMnb - Function computes the rotation rate transformation matrix from
154 /// euler angle rates to body axis rates
155 static Matrix RRTMnb(const Vector3& euler);
156 static Matrix RRTMnb(const Rot3& att);
157 /// @}
158
159private:
160#if GTSAM_ENABLE_BOOST_SERIALIZATION
161 /** Serialization function */
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
172template<>
173struct traits<PoseRTV> : public internal::LieGroup<PoseRTV> {};
174
175// Define Range functor specializations that are used in RangeFactor
176template <typename A1, typename A2> struct Range;
177
178template<>
179struct Range<PoseRTV, PoseRTV> : HasRange<PoseRTV, PoseRTV, double> {};
180
181} // \namespace gtsam
182