PoseRTV.cpp
Go to the documentation of this file.
1 
7 #include <gtsam/geometry/Pose2.h>
8 #include <gtsam/base/Vector.h>
9 
10 namespace gtsam {
11 
12 using namespace std;
13 
14 static const Vector kGravity = Vector::Unit(3,2)*9.81;
15 
16 /* ************************************************************************* */
17 double bound(double a, double min, double max) {
18  if (a < min) return min;
19  else if (a > max) return max;
20  else return a;
21 }
22 
23 /* ************************************************************************* */
24 PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y,
25  double z, double vx, double vy, double vz) :
26  Base(Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)),
27  Velocity3(vx, vy, vz)) {
28 }
29 
30 /* ************************************************************************* */
31 PoseRTV::PoseRTV(const Vector& rtv) :
32  Base(Pose3(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))),
33  Velocity3(rtv.tail(3))) {
34 }
35 
36 /* ************************************************************************* */
38  Vector rtv(9);
39  rtv.head(3) = rotation().xyz();
40  rtv.segment(3,3) = translation();
41  rtv.tail(3) = velocity();
42  return rtv;
43 }
44 
45 /* ************************************************************************* */
46 bool PoseRTV::equals(const PoseRTV& other, double tol) const {
47  return pose().equals(other.pose(), tol)
48  && equal_with_abs_tol(velocity(), other.velocity(), tol);
49 }
50 
51 /* ************************************************************************* */
52 void PoseRTV::print(const string& s) const {
53  cout << s << ":" << endl;
54  gtsam::print((Vector)R().xyz(), " R:rpy");
55  cout << " T" << t().transpose() << endl;
56  gtsam::print((Vector)velocity(), " V");
57 }
58 
59 /* ************************************************************************* */
60 PoseRTV PoseRTV::planarDynamics(double vel_rate, double heading_rate,
61  double max_accel, double dt) const {
62 
63  // split out initial state
64  const Rot3& r1 = R();
65  const Velocity3& v1 = v();
66 
67  // Update vehicle heading
68  Rot3 r2 = r1.retract((Vector(3) << 0.0, 0.0, heading_rate * dt).finished());
69  const double yaw2 = r2.ypr()(0);
70 
71  // Update vehicle position
72  const double mag_v1 = v1.norm();
73 
74  // FIXME: this doesn't account for direction in velocity bounds
75  double dv = bound(vel_rate - mag_v1, - (max_accel * dt), max_accel * dt);
76  double mag_v2 = mag_v1 + dv;
77  Velocity3 v2 = mag_v2 * Velocity3(cos(yaw2), sin(yaw2), 0.0);
78 
80 
81  return PoseRTV(r2, t2, v2);
82 }
83 
84 /* ************************************************************************* */
86  double pitch_rate, double heading_rate, double lift_control, double dt) const {
87  // split out initial state
88  const Rot3& r1 = R();
89  const Velocity3& v1 = v();
90 
91  // Update vehicle heading (and normalise yaw)
92  Vector rot_rates = (Vector(3) << 0.0, pitch_rate, heading_rate).finished();
93  Rot3 r2 = r1.retract(rot_rates*dt);
94 
95  // Work out dynamics on platform
96  const double thrust = 50.0;
97  const double lift = 50.0;
98  const double drag = 0.1;
99  double yaw2 = r2.yaw();
100  double pitch2 = r2.pitch();
101  double forward_accel = -thrust * sin(pitch2); // r2, pitch (in global frame?) controls forward force
102  double loss_lift = lift*std::abs(sin(pitch2));
103  Rot3 yaw_correction_bn = Rot3::Yaw(yaw2);
104  Point3 forward(forward_accel, 0.0, 0.0);
105  Vector Acc_n =
106  yaw_correction_bn.rotate(forward) // applies locally forward force in the global frame
107  - drag * (Vector(3) << v1.x(), v1.y(), 0.0).finished() // drag term dependent on v1
108  + Vector::Unit(3,2)*(loss_lift - lift_control); // falling due to lift lost from pitch
109 
110  // Update Vehicle Position and Velocity
111  Velocity3 v2 = v1 + Velocity3(Acc_n * dt);
113 
114  return PoseRTV(r2, t2, v2);
115 }
116 
117 /* ************************************************************************* */
119  const Vector& accel, const Vector& gyro, double dt) const {
120  // Integrate Attitude Equations
121  Rot3 r2 = rotation().retract(gyro * dt);
122 
123  // Integrate Velocity Equations
124  Velocity3 v2 = velocity() + Velocity3(dt * (r2.matrix() * accel + kGravity));
125 
126  // Integrate Position Equations
128 
129  return PoseRTV(t2, r2, v2);
130 }
131 
132 /* ************************************************************************* */
133 Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
134  // split out states
135  const Rot3 &r1 = R(), &r2 = x2.R();
136  const Velocity3 &v1 = v(), &v2 = x2.v();
137 
138  Vector6 imu;
139 
140  // acceleration
141  Vector3 accel = (v2-v1) / dt;
142  imu.head<3>() = r2.transpose() * (accel - kGravity);
143 
144  // rotation rates
145  // just using euler angles based on matlab code
146  // FIXME: this is silly - we shouldn't use differences in Euler angles
147  Matrix Enb = RRTMnb(r1);
148  Vector3 euler1 = r1.xyz(), euler2 = r2.xyz();
149  Vector3 dR = euler2 - euler1;
150 
151  // normalize yaw in difference (as per Mitch's code)
152  dR(2) = Rot2::fromAngle(dR(2)).theta();
153  dR /= dt;
154  imu.tail<3>() = Enb * dR;
155 // imu.tail(3) = r1.transpose() * dR;
156 
157  return imu;
158 }
159 
160 /* ************************************************************************* */
161 Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const {
162  // predict point for constraint
163  // NOTE: uses simple Euler approach for prediction
164  Point3 pred_t2 = t() + Point3(v2 * dt);
165  return pred_t2;
166 }
167 
168 /* ************************************************************************* */
171  Matrix36 D_t1_pose, D_t2_other;
172  const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0);
173  const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0);
174  Matrix13 D_d_t1, D_d_t2;
175  double d = distance3(t1, t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0);
176  if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0;
177  if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0;
178  return d;
179 }
180 
181 /* ************************************************************************* */
183  OptionalJacobian<9, 6> Dtrans) const {
184 
185  // Pose3 transform is just compose
186  Matrix6 D_newpose_trans, D_newpose_pose;
187  Pose3 newpose = trans.compose(pose(), D_newpose_trans, D_newpose_pose);
188 
189  // Note that we rotate the velocity
190  Matrix3 D_newvel_R, D_newvel_v;
191  Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v);
192 
193  if (Dglobal) {
194  Dglobal->setZero();
195  Dglobal->topLeftCorner<6,6>() = D_newpose_pose;
196  Dglobal->bottomRightCorner<3,3>() = D_newvel_v;
197  }
198 
199  if (Dtrans) {
200  Dtrans->setZero();
201  Dtrans->topLeftCorner<6,6>() = D_newpose_trans;
202  Dtrans->bottomLeftCorner<3,3>() = D_newvel_R;
203  }
204  return PoseRTV(newpose, newvel);
205 }
206 
207 /* ************************************************************************* */
209  assert(euler.size() == 3);
210  const double s1 = sin(euler.x()), c1 = cos(euler.x());
211  const double t2 = tan(euler.y()), c2 = cos(euler.y());
212  Matrix Ebn(3,3);
213  Ebn << 1.0, s1 * t2, c1 * t2,
214  0.0, c1, -s1,
215  0.0, s1 / c2, c1 / c2;
216  return Ebn;
217 }
218 
219 /* ************************************************************************* */
221  return PoseRTV::RRTMbn(att.rpy());
222 }
223 
224 /* ************************************************************************* */
226  Matrix Enb(3,3);
227  const double s1 = sin(euler.x()), c1 = cos(euler.x());
228  const double s2 = sin(euler.y()), c2 = cos(euler.y());
229  Enb << 1.0, 0.0, -s2,
230  0.0, c1, s1*c2,
231  0.0, -s1, c1*c2;
232  return Enb;
233 }
234 
235 /* ************************************************************************* */
237  return PoseRTV::RRTMnb(att.rpy());
238 }
239 
240 /* ************************************************************************* */
241 } // \namespace gtsam
gtsam::Rot3::xyz
Vector3 xyz(OptionalJacobian< 3, 3 > H={}) const
Definition: Rot3.cpp:160
Pose2.h
2D Pose
tail
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE FixedSegmentReturnType< internal::get_fixed_value< NType >::value >::Type tail(NType n)
Definition: BlockMethods.h:1257
gtsam::PoseRTV::equals
bool equals(const PoseRTV &other, double tol=1e-6) const
Definition: PoseRTV.cpp:46
gtsam::PoseRTV::transformed_from
PoseRTV transformed_from(const Pose3 &trans, ChartJacobian Dglobal={}, OptionalJacobian< 9, 6 > Dtrans={}) const
Definition: PoseRTV.cpp:182
Vector.h
typedef and functions to augment Eigen's VectorXd
s
RealScalar s
Definition: level1_cplx_impl.h:126
d
static const double d[K][N]
Definition: igam.h:11
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
gtsam::Rot3::rpy
Vector3 rpy(OptionalJacobian< 3, 3 > H={}) const
Definition: Rot3.cpp:191
gtsam::PoseRTV::RRTMbn
static Matrix RRTMbn(const Vector3 &euler)
Definition: PoseRTV.cpp:208
gtsam::bound
double bound(double a, double min, double max)
Definition: PoseRTV.cpp:17
gtsam::Velocity3
Vector3 Velocity3
Velocity is currently typedef'd to Vector3.
Definition: NavState.h:28
r2
static const double r2
Definition: testSmartRangeFactor.cpp:32
x
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
Definition: gnuplot_common_settings.hh:12
dt
const double dt
Definition: testVelocityConstraint.cpp:15
gtsam::Rot3::Yaw
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition: Rot3.h:174
gtsam::Rot2::theta
double theta() const
Definition: Rot2.h:186
gtsam::PoseRTV::vector
Vector vector() const
Definition: PoseRTV.cpp:37
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::PoseRTV::generalDynamics
PoseRTV generalDynamics(const Vector &accel, const Vector &gyro, double dt) const
General Dynamics update - supply control inputs in body frame.
Definition: PoseRTV.cpp:118
gtsam::PoseRTV::rotation
const Rot3 & rotation() const
Definition: PoseRTV.h:63
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
r1
static const double r1
Definition: testSmartRangeFactor.cpp:32
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
gtsam::PoseRTV::translationIntegration
Point3 translationIntegration(const Rot3 &r2, const Velocity3 &v2, double dt) const
Definition: PoseRTV.cpp:161
c1
static double c1
Definition: airy.c:54
gtsam::PoseRTV::t
const Point3 & t() const
Definition: PoseRTV.h:58
gtsam::Rot2::fromAngle
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition: Rot2.h:61
gtsam::PoseRTV::imuPrediction
Vector6 imuPrediction(const PoseRTV &x2, double dt) const
Definition: PoseRTV.cpp:133
gtsam::kGravity
static const Vector kGravity
Definition: PoseRTV.cpp:14
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
segment
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE FixedSegmentReturnType< internal::get_fixed_value< NType >::value >::Type segment(Index start, NType n)
Definition: BlockMethods.h:1158
vy
StridedVectorType vy(make_vector(y, *n, std::abs(*incy)))
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: Rot3M.cpp:148
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
gtsam::PoseRTV::print
void print(const std::string &s="") const
Definition: PoseRTV.cpp:52
gtsam::PoseRTV::v
const Velocity3 & v() const
Definition: PoseRTV.h:57
gtsam::Pose3::equals
bool equals(const Pose3 &pose, double tol=1e-9) const
assert equality up to a tolerance
Definition: Pose3.cpp:157
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
PoseRTV.h
Pose3 with translational velocity.
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
head
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE FixedSegmentReturnType< internal::get_fixed_value< NType >::value >::Type head(NType n)
Definition: BlockMethods.h:1208
tan
const EIGEN_DEVICE_FUNC TanReturnType tan() const
Definition: ArrayCwiseUnaryOps.h:269
gtsam::PoseRTV::translation
const Point3 & translation() const
Definition: PoseRTV.h:62
y
Scalar * y
Definition: level1_cplx_impl.h:124
gtsam::PoseRTV
Definition: PoseRTV.h:23
gtsam::PoseRTV::planarDynamics
PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const
Definition: PoseRTV.cpp:60
gtsam::PoseRTV::velocity
const Velocity3 & velocity() const
Definition: PoseRTV.h:64
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
gtsam
traits
Definition: SFMdata.h:40
gtsam::ProductLieGroup< Pose3, Velocity3 >
forward
Definition: testScenarioRunner.cpp:79
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::trans
Matrix trans(const Matrix &A)
Definition: base/Matrix.h:241
std
Definition: BFloat16.h:88
gtsam::PoseRTV::R
const Rot3 & R() const
Definition: PoseRTV.h:59
v2
Vector v2
Definition: testSerializationBase.cpp:39
c2
static double c2
Definition: airy.c:55
gtsam::PoseRTV::RRTMnb
static Matrix RRTMnb(const Vector3 &euler)
Definition: PoseRTV.cpp:225
gtsam::PoseRTV::PoseRTV
PoseRTV()
Definition: PoseRTV.h:32
min
#define min(a, b)
Definition: datatypes.h:19
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
gtsam::PoseRTV::pose
const Pose3 & pose() const
Definition: PoseRTV.h:56
abs
#define abs(x)
Definition: datatypes.h:17
max
#define max(a, b)
Definition: datatypes.h:20
gtsam::distance3
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
distance between two points
Definition: Point3.cpp:27
gtsam::PoseRTV::flyingDynamics
PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const
Definition: PoseRTV.cpp:85
gtsam::PoseRTV::range
double range(const PoseRTV &other, OptionalJacobian< 1, 9 > H1={}, OptionalJacobian< 1, 9 > H2={}) const
Definition: PoseRTV.cpp:169
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
v1
Vector v1
Definition: testSerializationBase.cpp:38
vx
StridedVectorType vx(make_vector(x, *n, std::abs(*incx)))


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:24