frame_tf.h
Go to the documentation of this file.
1 
11 /*
12  * Copyright 2016,2017 Vladimir Ermakov.
13  * Copyright 2017,2018 Nuno Marques.
14  *
15  * This file is part of the mavros package and subject to the license terms
16  * in the top-level LICENSE file of the mavros repository.
17  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
18  */
19 
20 #pragma once
21 
22 #include <array>
23 #include <Eigen/Eigen>
24 #include <Eigen/Geometry>
25 #include <ros/assert.h>
26 
27 // for Covariance types
28 #include <sensor_msgs/Imu.h>
29 #include <geometry_msgs/Point.h>
30 #include <geometry_msgs/Vector3.h>
31 #include <geometry_msgs/Quaternion.h>
32 #include <geometry_msgs/PoseWithCovariance.h>
33 
34 namespace mavros {
35 namespace ftf {
37 using Covariance3d = sensor_msgs::Imu::_angular_velocity_covariance_type;
38 
40 using Covariance6d = geometry_msgs::PoseWithCovariance::_covariance_type;
41 
43 using Covariance9d = boost::array<double, 81>;
44 
46 using EigenMapCovariance3d = Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> >;
47 using EigenMapConstCovariance3d = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> >;
48 
50 using EigenMapCovariance6d = Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor> >;
51 using EigenMapConstCovariance6d = Eigen::Map<const Eigen::Matrix<double, 6, 6, Eigen::RowMajor> >;
52 
54 using EigenMapCovariance9d = Eigen::Map<Eigen::Matrix<double, 9, 9, Eigen::RowMajor> >;
55 using EigenMapConstCovariance9d = Eigen::Map<const Eigen::Matrix<double, 9, 9, Eigen::RowMajor> >;
56 
60 enum class StaticTF {
61  NED_TO_ENU,
62  ENU_TO_NED,
67 };
68 
72 enum class StaticEcefTF {
73  ECEF_TO_ENU,
75 };
76 
77 namespace detail {
85 Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond &q, const StaticTF transform);
86 
92 Eigen::Vector3d transform_frame(const Eigen::Vector3d &vec, const Eigen::Quaterniond &q);
93 
99 Covariance3d transform_frame(const Covariance3d &cov, const Eigen::Quaterniond &q);
100 
106 Covariance6d transform_frame(const Covariance6d &cov, const Eigen::Quaterniond &q);
107 
113 Covariance9d transform_frame(const Covariance9d &cov, const Eigen::Quaterniond &q);
114 
120 Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const StaticTF transform);
121 
127 Covariance3d transform_static_frame(const Covariance3d &cov, const StaticTF transform);
128 
134 Covariance6d transform_static_frame(const Covariance6d &cov, const StaticTF transform);
135 
141 Covariance9d transform_static_frame(const Covariance9d &cov, const StaticTF transform);
142 
149 Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const Eigen::Vector3d &map_origin, const StaticEcefTF transform);
150 
151 } // namespace detail
152 
153 // -*- frame tf -*-
154 
159 template<class T>
160 inline T transform_orientation_ned_enu(const T &in) {
162 }
163 
168 template<class T>
169 inline T transform_orientation_enu_ned(const T &in) {
171 }
172 
177 template<class T>
180 }
181 
186 template<class T>
189 }
190 
196 template<class T>
199 }
200 
206 template<class T>
209 }
210 
214 template<class T>
215 inline T transform_frame_ned_enu(const T &in) {
217 }
218 
223 template<class T>
224 inline T transform_frame_enu_ned(const T &in) {
226 }
227 
232 template<class T>
233 inline T transform_frame_aircraft_baselink(const T &in) {
235 }
236 
241 template<class T>
242 inline T transform_frame_baselink_aircraft(const T &in) {
244 }
245 
253 template<class T>
254 inline T transform_frame_ecef_enu(const T &in, const T &map_origin) {
256 }
257 
265 template<class T>
266 inline T transform_frame_enu_ecef(const T &in, const T &map_origin) {
268 }
269 
274 template<class T>
275 inline T transform_frame_aircraft_ned(const T &in, const Eigen::Quaterniond &q) {
276  return detail::transform_frame(in, q);
277 }
278 
283 template<class T>
284 inline T transform_frame_ned_aircraft(const T &in, const Eigen::Quaterniond &q) {
285  return detail::transform_frame(in, q);
286 }
287 
292 template<class T>
293 inline T transform_frame_aircraft_enu(const T &in, const Eigen::Quaterniond &q) {
294  return detail::transform_frame(in, q);
295 }
296 
301 template<class T>
302 inline T transform_frame_enu_aircraft(const T &in, const Eigen::Quaterniond &q) {
303  return detail::transform_frame(in, q);
304 }
305 
310 template<class T>
311 inline T transform_frame_enu_baselink(const T &in, const Eigen::Quaterniond &q) {
312  return detail::transform_frame(in, q);
313 }
314 
319 template<class T>
320 inline T transform_frame_baselink_enu(const T &in, const Eigen::Quaterniond &q) {
321  return detail::transform_frame(in, q);
322 }
323 
324 // -*- utils -*-
325 
326 
330 Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy);
331 
337 inline Eigen::Quaterniond quaternion_from_rpy(const double roll, const double pitch, const double yaw) {
338  return quaternion_from_rpy(Eigen::Vector3d(roll, pitch, yaw));
339 }
340 
346 Eigen::Vector3d quaternion_to_rpy(const Eigen::Quaterniond &q);
347 
351 inline void quaternion_to_rpy(const Eigen::Quaterniond &q, double &roll, double &pitch, double &yaw)
352 {
353  const auto rpy = quaternion_to_rpy(q);
354  roll = rpy.x();
355  pitch = rpy.y();
356  yaw = rpy.z();
357 }
358 
364 double quaternion_get_yaw(const Eigen::Quaterniond &q);
365 
372 template <typename _Scalar, typename std::enable_if<std::is_floating_point<_Scalar>::value, bool>::type = true>
373 inline void quaternion_to_mavlink(const Eigen::Quaternion<_Scalar> &q, std::array<float, 4> &qmsg)
374 {
375  qmsg[0] = q.w();
376  qmsg[1] = q.x();
377  qmsg[2] = q.y();
378  qmsg[3] = q.z();
379 }
380 
384 inline Eigen::Quaterniond mavlink_to_quaternion(const std::array<float, 4> &q)
385 {
386  return Eigen::Quaterniond(q[0], q[1], q[2], q[3]);
387 }
388 
392 template<class T, std::size_t SIZE>
393 inline void covariance_to_mavlink(const T &cov, std::array<float, SIZE> &covmsg)
394 {
395  std::copy(cov.cbegin(), cov.cend(), covmsg.begin());
396 }
397 
401 template<class T, std::size_t ARR_SIZE>
402 inline void covariance_urt_to_mavlink(const T &covmap, std::array<float, ARR_SIZE> &covmsg)
403 {
404  auto m = covmap;
405  std::size_t COV_SIZE = m.rows() * (m.rows() + 1) / 2;
406  ROS_ASSERT_MSG(COV_SIZE == ARR_SIZE,
407  "frame_tf: covariance matrix URT size (%lu) is different from Mavlink msg covariance field size (%lu)",
408  COV_SIZE, ARR_SIZE);
409 
410  auto out = covmsg.begin();
411 
412  for (size_t x = 0; x < m.cols(); x++) {
413  for (size_t y = x; y < m.rows(); y++)
414  *out++ = m(y, x);
415  }
416 }
417 
422 template<class T, std::size_t ARR_SIZE>
423 inline void mavlink_urt_to_covariance_matrix(const std::array<float, ARR_SIZE> &covmsg, T &covmat)
424 {
425  std::size_t COV_SIZE = covmat.rows() * (covmat.rows() + 1) / 2;
426  ROS_ASSERT_MSG(COV_SIZE == ARR_SIZE,
427  "frame_tf: covariance matrix URT size (%lu) is different from Mavlink msg covariance field size (%lu)",
428  COV_SIZE, ARR_SIZE);
429 
430  auto in = covmsg.begin();
431 
432  for (size_t x = 0; x < covmat.cols(); x++) {
433  for (size_t y = x; y < covmat.rows(); y++) {
434  covmat(x, y) = static_cast<double>(*in++);
435  covmat(y, x) = covmat(x, y);
436  }
437  }
438 }
439 
440 // [[[cog:
441 // def make_to_eigen(te, tr, fields):
442 // cog.outl("""//! @brief Helper to convert common ROS geometry_msgs::{tr} to Eigen::{te}""".format(**locals()))
443 // cog.outl("""inline Eigen::{te} to_eigen(const geometry_msgs::{tr} r) {{""".format(**locals()))
444 // cog.outl("""\treturn Eigen::{te}({fl});""".format(te=te, fl=", ".join(["r." + f for f in fields])))
445 // cog.outl("""}""")
446 //
447 // make_to_eigen("Vector3d", "Point", "xyz")
448 // make_to_eigen("Vector3d", "Vector3", "xyz")
449 // make_to_eigen("Quaterniond", "Quaternion", "wxyz")
450 // ]]]
452 inline Eigen::Vector3d to_eigen(const geometry_msgs::Point r) {
453  return Eigen::Vector3d(r.x, r.y, r.z);
454 }
456 inline Eigen::Vector3d to_eigen(const geometry_msgs::Vector3 r) {
457  return Eigen::Vector3d(r.x, r.y, r.z);
458 }
460 inline Eigen::Quaterniond to_eigen(const geometry_msgs::Quaternion r) {
461  return Eigen::Quaterniond(r.w, r.x, r.y, r.z);
462 }
463 // [[[end]]] (checksum: 1b3ada1c4245d4e31dcae9768779b952)
464 } // namespace ftf
465 } // namespace mavros
change orientation from being expressed in baselink frame to aircraft frame in an absolute frame of r...
change from expressed WRT ENU frame to WRT ECEF frame
will change orientation from being expressed WRT NED frame to WRT ENU frame
StaticEcefTF
Orientation transform options when applying rotations to data, for ECEF.
Definition: frame_tf.h:72
Eigen::Quaterniond mavlink_to_quaternion(const std::array< float, 4 > &q)
Convert Mavlink float[4] quaternion to Eigen.
Definition: frame_tf.h:384
StaticTF
Orientation transform options when applying rotations to data.
Definition: frame_tf.h:60
Eigen::Map< Eigen::Matrix< double, 6, 6, Eigen::RowMajor > > EigenMapCovariance6d
Eigen::Map for Covariance6d.
Definition: frame_tf.h:50
Eigen::Map< const Eigen::Matrix< double, 3, 3, Eigen::RowMajor > > EigenMapConstCovariance3d
Definition: frame_tf.h:47
void quaternion_to_mavlink(const Eigen::Quaternion< _Scalar > &q, std::array< float, 4 > &qmsg)
Store Quaternion to MAVLink float[4] format.
Definition: frame_tf.h:373
Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond &q, const StaticTF transform)
Transform representation of attitude from 1 frame to another (e.g. transfrom attitude from representi...
T transform_orientation_enu_ned(const T &in)
Transform from attitude represented WRT ENU frame to attitude represented WRT NED frame...
Definition: frame_tf.h:169
void mavlink_urt_to_covariance_matrix(const std::array< float, ARR_SIZE > &covmsg, T &covmat)
Convert MAVLink float[n] format (upper right triangular of a covariance matrix) to Eigen::MatrixXd<n...
Definition: frame_tf.h:423
Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const StaticTF transform)
Transform data expressed in one frame to another frame.
T transform_frame_enu_aircraft(const T &in, const Eigen::Quaterniond &q)
Transform data expressed in ENU to aircraft frame. Assumes quaternion represents rotation from ENU to...
Definition: frame_tf.h:302
Eigen::Vector3d transform_frame(const Eigen::Vector3d &vec, const Eigen::Quaterniond &q)
Transform data expressed in one frame to another frame.
T transform_frame_enu_baselink(const T &in, const Eigen::Quaterniond &q)
Transform data expressed in ENU to base_link frame. Assumes quaternion represents rotation from ENU t...
Definition: frame_tf.h:311
T transform_orientation_absolute_frame_aircraft_baselink(const T &in)
Transform from attitude represented WRT aircraft frame to attitude represented WRT base_link frame...
Definition: frame_tf.h:197
T transform_frame_aircraft_baselink(const T &in)
Transform data expressed in Aircraft frame to Baselink frame.
Definition: frame_tf.h:233
change from expressed WRT aircraft frame to WRT to baselink frame
Eigen::Vector3d quaternion_to_rpy(const Eigen::Quaterniond &q)
Convert quaternion to euler angles.
#define ROS_ASSERT_MSG(cond,...)
Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const Eigen::Vector3d &map_origin, const StaticEcefTF transform)
Transform data expressed in one frame to another frame with additional map origin parameter...
T transform_frame_ned_aircraft(const T &in, const Eigen::Quaterniond &q)
Transform data expressed in NED to aircraft frame. Assumes quaternion represents rotation from NED to...
Definition: frame_tf.h:284
Eigen::Map< Eigen::Matrix< double, 9, 9, Eigen::RowMajor > > EigenMapCovariance9d
Eigen::Map for Covariance9d.
Definition: frame_tf.h:54
T transform_frame_aircraft_enu(const T &in, const Eigen::Quaterniond &q)
Transform data expressed in aircraft frame to ENU frame. Assumes quaternion represents rotation from ...
Definition: frame_tf.h:293
T transform_frame_ned_enu(const T &in)
Transform data expressed in NED to ENU frame.
Definition: frame_tf.h:215
Eigen::Vector3d to_eigen(const geometry_msgs::Point r)
Helper to convert common ROS geometry_msgs::Point to Eigen::Vector3d.
Definition: frame_tf.h:452
void covariance_urt_to_mavlink(const T &covmap, std::array< float, ARR_SIZE > &covmsg)
Convert upper right triangular of a covariance matrix to MAVLink float[n] format. ...
Definition: frame_tf.h:402
geometry_msgs::PoseWithCovariance::_covariance_type Covariance6d
Type matching rosmsg for 6x6 covariance matrix.
Definition: frame_tf.h:40
T transform_frame_baselink_aircraft(const T &in)
Transform data expressed in Baselink frame to Aircraft frame.
Definition: frame_tf.h:242
boost::array< double, 81 > Covariance9d
Type matching rosmsg for 9x9 covariance matrix.
Definition: frame_tf.h:43
change orientation from being expressed in aircraft frame to baselink frame in an absolute frame of r...
Eigen::Map< Eigen::Matrix< double, 3, 3, Eigen::RowMajor > > EigenMapCovariance3d
Eigen::Map for Covariance3d.
Definition: frame_tf.h:46
T transform_frame_enu_ecef(const T &in, const T &map_origin)
Transform data expressed in ENU frame to ECEF frame.
Definition: frame_tf.h:266
T transform_orientation_absolute_frame_baselink_aircraft(const T &in)
Transform from attitude represented WRT baselink frame to attitude represented WRT body frame...
Definition: frame_tf.h:207
T transform_frame_enu_ned(const T &in)
Transform data expressed in ENU to NED frame.
Definition: frame_tf.h:224
sensor_msgs::Imu::_angular_velocity_covariance_type Covariance3d
Type matching rosmsg for 3x3 covariance matrix.
Definition: frame_tf.h:37
Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy)
Convert euler angles to quaternion.
change from expressed WRT baselnk to WRT aircraft
Covariance9d transform_frame(const Covariance9d &cov, const Eigen::Quaterniond &q)
Transform 9x9 convariance expressed in one frame to another.
double quaternion_get_yaw(const Eigen::Quaterniond &q)
Get Yaw angle from quaternion.
T transform_frame_ecef_enu(const T &in, const T &map_origin)
Transform data expressed in ECEF frame to ENU frame.
Definition: frame_tf.h:254
T transform_frame_aircraft_ned(const T &in, const Eigen::Quaterniond &q)
Transform data expressed in aircraft frame to NED frame. Assumes quaternion represents rotation from ...
Definition: frame_tf.h:275
T transform_orientation_ned_enu(const T &in)
Transform from attitude represented WRT NED frame to attitude represented WRT ENU frame...
Definition: frame_tf.h:160
change from expressed WRT ECEF frame to WRT ENU frame
T transform_orientation_baselink_aircraft(const T &in)
Transform from attitude represented WRT baselink frame to attitude represented WRT body frame...
Definition: frame_tf.h:187
void covariance_to_mavlink(const T &cov, std::array< float, SIZE > &covmsg)
Convert covariance matrix to MAVLink float[n] format.
Definition: frame_tf.h:393
T transform_orientation_aircraft_baselink(const T &in)
Transform from attitude represented WRT aircraft frame to attitude represented WRT base_link frame...
Definition: frame_tf.h:178
Eigen::Map< const Eigen::Matrix< double, 9, 9, Eigen::RowMajor > > EigenMapConstCovariance9d
Definition: frame_tf.h:55
Eigen::Map< const Eigen::Matrix< double, 6, 6, Eigen::RowMajor > > EigenMapConstCovariance6d
Definition: frame_tf.h:51
change from expressed WRT ENU frame to WRT NED frame
T transform_frame_baselink_enu(const T &in, const Eigen::Quaterniond &q)
Transform data expressed in baselink to ENU frame. Assumes quaternion represents rotation from basel_...
Definition: frame_tf.h:320


mavros
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 1 2021 02:36:26