.. _program_listing_file_include_mavros_frame_tf.hpp:

Program Listing for File frame_tf.hpp
=====================================

|exhale_lsh| :ref:`Return to documentation for file <file_include_mavros_frame_tf.hpp>` (``include/mavros/frame_tf.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*
    * Copyright 2016,2017,2021 Vladimir Ermakov.
    * Copyright 2017,2018 Nuno Marques.
    *
    * This file is part of the mavros package and subject to the license terms
    * in the top-level LICENSE file of the mavros repository.
    * https://github.com/mavlink/mavros/tree/master/LICENSE.md
    */
   #pragma once
   
   #ifndef MAVROS__FRAME_TF_HPP_
   #define MAVROS__FRAME_TF_HPP_
   
   #include <array>
   #include <algorithm>
   #include <Eigen/Eigen>              // NOLINT
   #include <Eigen/Geometry>           // NOLINT
   #include <rcpputils/asserts.hpp>    // NOLINT
   
   // for Covariance types
   #include "sensor_msgs/msg/imu.hpp"
   #include "geometry_msgs/msg/point.hpp"
   #include "geometry_msgs/msg/vector3.hpp"
   #include "geometry_msgs/msg/quaternion.hpp"
   #include "geometry_msgs/msg/pose_with_covariance.hpp"
   
   namespace mavros
   {
   namespace ftf
   {
   using Covariance3d = sensor_msgs::msg::Imu::_angular_velocity_covariance_type;
   
   using Covariance6d = geometry_msgs::msg::PoseWithCovariance::_covariance_type;
   
   using Covariance9d = std::array<double, 81>;
   
   using EigenMapCovariance3d = Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>;
   using EigenMapConstCovariance3d = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>;
   
   using EigenMapCovariance6d = Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor>>;
   using EigenMapConstCovariance6d = Eigen::Map<const Eigen::Matrix<double, 6, 6, Eigen::RowMajor>>;
   
   using EigenMapCovariance9d = Eigen::Map<Eigen::Matrix<double, 9, 9, Eigen::RowMajor>>;
   using EigenMapConstCovariance9d = Eigen::Map<const Eigen::Matrix<double, 9, 9, Eigen::RowMajor>>;
   
   enum class StaticTF
   {
     NED_TO_ENU,
     ENU_TO_NED,
     AIRCRAFT_TO_BASELINK,
     BASELINK_TO_AIRCRAFT,
     //  baselink frame in an absolute frame of reference.
     ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK,
     //  aircraft frame in an absolute frame of reference
     ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT,
   };
   
   enum class StaticEcefTF
   {
     ECEF_TO_ENU,                  
     ENU_TO_ECEF                   
   };
   
   namespace detail
   {
   Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond & q, const StaticTF transform);
   
   Eigen::Vector3d transform_frame(const Eigen::Vector3d & vec, const Eigen::Quaterniond & q);
   
   Covariance3d transform_frame(const Covariance3d & cov, const Eigen::Quaterniond & q);
   
   Covariance6d transform_frame(const Covariance6d & cov, const Eigen::Quaterniond & q);
   
   Covariance9d transform_frame(const Covariance9d & cov, const Eigen::Quaterniond & q);
   
   Eigen::Vector3d transform_static_frame(const Eigen::Vector3d & vec, const StaticTF transform);
   
   Covariance3d transform_static_frame(const Covariance3d & cov, const StaticTF transform);
   
   Covariance6d transform_static_frame(const Covariance6d & cov, const StaticTF transform);
   
   Covariance9d transform_static_frame(const Covariance9d & cov, const StaticTF transform);
   
   Eigen::Vector3d transform_static_frame(
     const Eigen::Vector3d & vec,
     const Eigen::Vector3d & map_origin,
     const StaticEcefTF transform);
   
   }       // namespace detail
   
   // -*- frame tf -*-
   
   template<class T>
   inline T transform_orientation_ned_enu(const T & in)
   {
     return detail::transform_orientation(in, StaticTF::NED_TO_ENU);
   }
   
   template<class T>
   inline T transform_orientation_enu_ned(const T & in)
   {
     return detail::transform_orientation(in, StaticTF::ENU_TO_NED);
   }
   
   template<class T>
   inline T transform_orientation_aircraft_baselink(const T & in)
   {
     return detail::transform_orientation(in, StaticTF::AIRCRAFT_TO_BASELINK);
   }
   
   template<class T>
   inline T transform_orientation_baselink_aircraft(const T & in)
   {
     return detail::transform_orientation(in, StaticTF::BASELINK_TO_AIRCRAFT);
   }
   
   template<class T>
   inline T transform_orientation_absolute_frame_aircraft_baselink(const T & in)
   {
     return detail::transform_orientation(in, StaticTF::ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK);
   }
   
   template<class T>
   inline T transform_orientation_absolute_frame_baselink_aircraft(const T & in)
   {
     return detail::transform_orientation(in, StaticTF::ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT);
   }
   
   template<class T>
   inline T transform_frame_ned_enu(const T & in)
   {
     return detail::transform_static_frame(in, StaticTF::NED_TO_ENU);
   }
   
   template<class T>
   inline T transform_frame_enu_ned(const T & in)
   {
     return detail::transform_static_frame(in, StaticTF::ENU_TO_NED);
   }
   
   template<class T>
   inline T transform_frame_aircraft_baselink(const T & in)
   {
     return detail::transform_static_frame(in, StaticTF::AIRCRAFT_TO_BASELINK);
   }
   
   template<class T>
   inline T transform_frame_baselink_aircraft(const T & in)
   {
     return detail::transform_static_frame(in, StaticTF::BASELINK_TO_AIRCRAFT);
   }
   
   template<class T>
   inline T transform_frame_ecef_enu(const T & in, const T & map_origin)
   {
     return detail::transform_static_frame(in, map_origin, StaticEcefTF::ECEF_TO_ENU);
   }
   
   template<class T>
   inline T transform_frame_enu_ecef(const T & in, const T & map_origin)
   {
     return detail::transform_static_frame(in, map_origin, StaticEcefTF::ENU_TO_ECEF);
   }
   
   template<class T>
   inline T transform_frame_aircraft_ned(const T & in, const Eigen::Quaterniond & q)
   {
     return detail::transform_frame(in, q);
   }
   
   template<class T>
   inline T transform_frame_ned_aircraft(const T & in, const Eigen::Quaterniond & q)
   {
     return detail::transform_frame(in, q);
   }
   
   template<class T>
   inline T transform_frame_aircraft_enu(const T & in, const Eigen::Quaterniond & q)
   {
     return detail::transform_frame(in, q);
   }
   
   template<class T>
   inline T transform_frame_enu_aircraft(const T & in, const Eigen::Quaterniond & q)
   {
     return detail::transform_frame(in, q);
   }
   
   template<class T>
   inline T transform_frame_enu_baselink(const T & in, const Eigen::Quaterniond & q)
   {
     return detail::transform_frame(in, q);
   }
   
   template<class T>
   inline T transform_frame_baselink_enu(const T & in, const Eigen::Quaterniond & q)
   {
     return detail::transform_frame(in, q);
   }
   
   // -*- utils -*-
   
   
   Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d & rpy);
   
   inline Eigen::Quaterniond quaternion_from_rpy(
     const double roll, const double pitch,
     const double yaw)
   {
     return quaternion_from_rpy(Eigen::Vector3d(roll, pitch, yaw));
   }
   
   Eigen::Vector3d quaternion_to_rpy(const Eigen::Quaterniond & q);
   
   inline void quaternion_to_rpy(
     const Eigen::Quaterniond & q, double & roll, double & pitch,
     double & yaw)
   {
     const auto rpy = quaternion_to_rpy(q);
     roll = rpy.x();
     pitch = rpy.y();
     yaw = rpy.z();
   }
   
   double quaternion_get_yaw(const Eigen::Quaterniond & q);
   
   template<typename _Scalar, std::enable_if_t<std::is_floating_point<_Scalar>::value, bool> = true>
   inline void quaternion_to_mavlink(const Eigen::Quaternion<_Scalar> & q, std::array<float, 4> & qmsg)
   {
     qmsg[0] = q.w();
     qmsg[1] = q.x();
     qmsg[2] = q.y();
     qmsg[3] = q.z();
   }
   
   inline Eigen::Quaterniond mavlink_to_quaternion(const std::array<float, 4> & q)
   {
     return Eigen::Quaterniond(q[0], q[1], q[2], q[3]);
   }
   
   template<class T, std::size_t SIZE>
   inline void covariance_to_mavlink(const T & cov, std::array<float, SIZE> & covmsg)
   {
     std::copy(cov.cbegin(), cov.cend(), covmsg.begin());
   }
   
   template<class T, std::size_t ARR_SIZE>
   inline void covariance_urt_to_mavlink(const T & covmap, std::array<float, ARR_SIZE> & covmsg)
   {
     auto m = covmap;
     std::size_t COV_SIZE = m.rows() * (m.rows() + 1) / 2;
     rcpputils::assert_true(
       COV_SIZE == ARR_SIZE,
       "frame_tf: covariance matrix URT size is different from Mavlink msg covariance field size");
   
     auto out = covmsg.begin();
   
     for (Eigen::Index x = 0; x < m.cols(); x++) {
       for (Eigen::Index y = x; y < m.rows(); y++) {
         *out++ = m(y, x);
       }
     }
   }
   
   template<class T, std::size_t ARR_SIZE>
   inline void mavlink_urt_to_covariance_matrix(const std::array<float, ARR_SIZE> & covmsg, T & covmat)
   {
     std::size_t COV_SIZE = covmat.rows() * (covmat.rows() + 1) / 2;
     rcpputils::assert_true(
       COV_SIZE == ARR_SIZE,
       "frame_tf: covariance matrix URT size is different from Mavlink msg covariance field size");
   
     auto in = covmsg.begin();
   
     for (Eigen::Index x = 0; x < covmat.cols(); x++) {
       for (Eigen::Index y = x; y < covmat.rows(); y++) {
         covmat(x, y) = static_cast<double>(*in++);
         covmat(y, x) = covmat(x, y);
       }
     }
   }
   
   // [[[cog:
   // def make_to_eigen(te, tr, fields):
   //     fl = ", ".join(["r." + f for f in fields])
   //     cog.outl(f"""//! @brief Helper to convert common ROS geometry_msgs::{tr} to Eigen::{te}""")
   //     cog.outl(f"""inline Eigen::{te} to_eigen(const geometry_msgs::msg::{tr} r)""")
   //     cog.outl(f"""{{""")
   //     cog.outl(f"""  return Eigen::{te}({fl});""")
   //     cog.outl(f"""}}""")
   //
   // make_to_eigen("Vector3d", "Point", "xyz")
   // make_to_eigen("Vector3d", "Vector3", "xyz")
   // make_to_eigen("Quaterniond", "Quaternion", "wxyz")
   // ]]]
   inline Eigen::Vector3d to_eigen(const geometry_msgs::msg::Point r)
   {
     return Eigen::Vector3d(r.x, r.y, r.z);
   }
   inline Eigen::Vector3d to_eigen(const geometry_msgs::msg::Vector3 r)
   {
     return Eigen::Vector3d(r.x, r.y, r.z);
   }
   inline Eigen::Quaterniond to_eigen(const geometry_msgs::msg::Quaternion r)
   {
     return Eigen::Quaterniond(r.w, r.x, r.y, r.z);
   }
   // [[[end]]] (checksum: 2f12174368db2fc32ab814cb97b1bbec)
   
   }       // namespace ftf
   }       // namespace mavros
   
   #endif  // MAVROS__FRAME_TF_HPP_