PoseRTV.cpp
Go to the documentation of this file.
1 
7 #include <gtsam/geometry/Pose2.h>
8 #include <gtsam/base/Vector.h>
9 
10 #include <cassert>
11 
12 namespace gtsam {
13 
14 using namespace std;
15 
16 static const Vector kGravity = Vector::Unit(3,2)*9.81;
17 
18 /* ************************************************************************* */
19 double bound(double a, double min, double max) {
20  if (a < min) return min;
21  else if (a > max) return max;
22  else return a;
23 }
24 
25 /* ************************************************************************* */
26 PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y,
27  double z, double vx, double vy, double vz) :
28  Base(Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)),
29  Velocity3(vx, vy, vz)) {
30 }
31 
32 /* ************************************************************************* */
33 PoseRTV::PoseRTV(const Vector& rtv) :
34  Base(Pose3(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))),
35  Velocity3(rtv.tail(3))) {
36 }
37 
38 /* ************************************************************************* */
40  Vector rtv(9);
41  rtv.head(3) = rotation().xyz();
42  rtv.segment(3,3) = translation();
43  rtv.tail(3) = velocity();
44  return rtv;
45 }
46 
47 /* ************************************************************************* */
48 bool PoseRTV::equals(const PoseRTV& other, double tol) const {
49  return pose().equals(other.pose(), tol)
50  && equal_with_abs_tol(velocity(), other.velocity(), tol);
51 }
52 
53 /* ************************************************************************* */
54 void PoseRTV::print(const string& s) const {
55  cout << s << ":" << endl;
56  gtsam::print((Vector)R().xyz(), " R:rpy");
57  cout << " T" << t().transpose() << endl;
58  gtsam::print((Vector)velocity(), " V");
59 }
60 
61 /* ************************************************************************* */
62 PoseRTV PoseRTV::planarDynamics(double vel_rate, double heading_rate,
63  double max_accel, double dt) const {
64 
65  // split out initial state
66  const Rot3& r1 = R();
67  const Velocity3& v1 = v();
68 
69  // Update vehicle heading
70  Rot3 r2 = r1.retract((Vector(3) << 0.0, 0.0, heading_rate * dt).finished());
71  const double yaw2 = r2.ypr()(0);
72 
73  // Update vehicle position
74  const double mag_v1 = v1.norm();
75 
76  // FIXME: this doesn't account for direction in velocity bounds
77  double dv = bound(vel_rate - mag_v1, - (max_accel * dt), max_accel * dt);
78  double mag_v2 = mag_v1 + dv;
79  Velocity3 v2 = mag_v2 * Velocity3(cos(yaw2), sin(yaw2), 0.0);
80 
82 
83  return PoseRTV(r2, t2, v2);
84 }
85 
86 /* ************************************************************************* */
88  double pitch_rate, double heading_rate, double lift_control, double dt) const {
89  // split out initial state
90  const Rot3& r1 = R();
91  const Velocity3& v1 = v();
92 
93  // Update vehicle heading (and normalise yaw)
94  Vector rot_rates = (Vector(3) << 0.0, pitch_rate, heading_rate).finished();
95  Rot3 r2 = r1.retract(rot_rates*dt);
96 
97  // Work out dynamics on platform
98  const double thrust = 50.0;
99  const double lift = 50.0;
100  const double drag = 0.1;
101  double yaw2 = r2.yaw();
102  double pitch2 = r2.pitch();
103  double forward_accel = -thrust * sin(pitch2); // r2, pitch (in global frame?) controls forward force
104  double loss_lift = lift*std::abs(sin(pitch2));
105  Rot3 yaw_correction_bn = Rot3::Yaw(yaw2);
106  Point3 forward(forward_accel, 0.0, 0.0);
107  Vector Acc_n =
108  yaw_correction_bn.rotate(forward) // applies locally forward force in the global frame
109  - drag * (Vector(3) << v1.x(), v1.y(), 0.0).finished() // drag term dependent on v1
110  + Vector::Unit(3,2)*(loss_lift - lift_control); // falling due to lift lost from pitch
111 
112  // Update Vehicle Position and Velocity
113  Velocity3 v2 = v1 + Velocity3(Acc_n * dt);
115 
116  return PoseRTV(r2, t2, v2);
117 }
118 
119 /* ************************************************************************* */
121  const Vector& accel, const Vector& gyro, double dt) const {
122  // Integrate Attitude Equations
123  Rot3 r2 = rotation().retract(gyro * dt);
124 
125  // Integrate Velocity Equations
126  Velocity3 v2 = velocity() + Velocity3(dt * (r2.matrix() * accel + kGravity));
127 
128  // Integrate Position Equations
130 
131  return PoseRTV(t2, r2, v2);
132 }
133 
134 /* ************************************************************************* */
135 Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
136  // split out states
137  const Rot3 &r1 = R(), &r2 = x2.R();
138  const Velocity3 &v1 = v(), &v2 = x2.v();
139 
140  Vector6 imu;
141 
142  // acceleration
143  Vector3 accel = (v2-v1) / dt;
144  imu.head<3>() = r2.transpose() * (accel - kGravity);
145 
146  // rotation rates
147  // just using euler angles based on matlab code
148  // FIXME: this is silly - we shouldn't use differences in Euler angles
149  Matrix Enb = RRTMnb(r1);
150  Vector3 euler1 = r1.xyz(), euler2 = r2.xyz();
151  Vector3 dR = euler2 - euler1;
152 
153  // normalize yaw in difference (as per Mitch's code)
154  dR(2) = Rot2::fromAngle(dR(2)).theta();
155  dR /= dt;
156  imu.tail<3>() = Enb * dR;
157 // imu.tail(3) = r1.transpose() * dR;
158 
159  return imu;
160 }
161 
162 /* ************************************************************************* */
163 Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const {
164  // predict point for constraint
165  // NOTE: uses simple Euler approach for prediction
166  Point3 pred_t2 = t() + Point3(v2 * dt);
167  return pred_t2;
168 }
169 
170 /* ************************************************************************* */
173  Matrix36 D_t1_pose, D_t2_other;
174  const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0);
175  const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0);
176  Matrix13 D_d_t1, D_d_t2;
177  double d = distance3(t1, t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0);
178  if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0;
179  if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0;
180  return d;
181 }
182 
183 /* ************************************************************************* */
185  OptionalJacobian<9, 6> Dtrans) const {
186 
187  // Pose3 transform is just compose
188  Matrix6 D_newpose_trans, D_newpose_pose;
189  Pose3 newpose = trans.compose(pose(), D_newpose_trans, D_newpose_pose);
190 
191  // Note that we rotate the velocity
192  Matrix3 D_newvel_R, D_newvel_v;
193  Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v);
194 
195  if (Dglobal) {
196  Dglobal->setZero();
197  Dglobal->topLeftCorner<6,6>() = D_newpose_pose;
198  Dglobal->bottomRightCorner<3,3>() = D_newvel_v;
199  }
200 
201  if (Dtrans) {
202  Dtrans->setZero();
203  Dtrans->topLeftCorner<6,6>() = D_newpose_trans;
204  Dtrans->bottomLeftCorner<3,3>() = D_newvel_R;
205  }
206  return PoseRTV(newpose, newvel);
207 }
208 
209 /* ************************************************************************* */
211  assert(euler.size() == 3);
212  const double s1 = sin(euler.x()), c1 = cos(euler.x());
213  const double t2 = tan(euler.y()), c2 = cos(euler.y());
214  Matrix Ebn(3,3);
215  Ebn << 1.0, s1 * t2, c1 * t2,
216  0.0, c1, -s1,
217  0.0, s1 / c2, c1 / c2;
218  return Ebn;
219 }
220 
221 /* ************************************************************************* */
223  return PoseRTV::RRTMbn(att.rpy());
224 }
225 
226 /* ************************************************************************* */
228  Matrix Enb(3,3);
229  const double s1 = sin(euler.x()), c1 = cos(euler.x());
230  const double s2 = sin(euler.y()), c2 = cos(euler.y());
231  Enb << 1.0, 0.0, -s2,
232  0.0, c1, s1*c2,
233  0.0, -s1, c1*c2;
234  return Enb;
235 }
236 
237 /* ************************************************************************* */
239  return PoseRTV::RRTMnb(att.rpy());
240 }
241 
242 /* ************************************************************************* */
243 } // \namespace gtsam
gtsam::Rot3::xyz
Vector3 xyz(OptionalJacobian< 3, 3 > H={}) const
Definition: Rot3.cpp:161
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:48
gtsam::PoseRTV::transformed_from
PoseRTV transformed_from(const Pose3 &trans, ChartJacobian Dglobal={}, OptionalJacobian< 9, 6 > Dtrans={}) const
Definition: PoseRTV.cpp:184
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:192
gtsam::PoseRTV::RRTMbn
static Matrix RRTMbn(const Vector3 &euler)
Definition: PoseRTV.cpp:210
gtsam::bound
double bound(double a, double min, double max)
Definition: PoseRTV.cpp:19
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:178
gtsam::Rot2::theta
double theta() const
Definition: Rot2.h:186
gtsam::PoseRTV::vector
Vector vector() const
Definition: PoseRTV.cpp:39
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:120
gtsam::PoseRTV::rotation
const Rot3 & rotation() const
Definition: PoseRTV.h:63
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
r1
static const double r1
Definition: testSmartRangeFactor.cpp:32
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
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:163
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:135
gtsam::kGravity
static const Vector kGravity
Definition: PoseRTV.cpp:16
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:156
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:54
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:155
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:310
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:62
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:227
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:87
gtsam::PoseRTV::range
double range(const PoseRTV &other, OptionalJacobian< 1, 9 > H1={}, OptionalJacobian< 1, 9 > H2={}) const
Definition: PoseRTV.cpp:171
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 Sun Dec 22 2024 04:12:45