frame_tf.h

Go to the documentation of this file.

/*
 * Copyright 2016,2017 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

#include <array>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <ros/assert.h>

// for Covariance types
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseWithCovariance.h>

namespace mavros {
namespace ftf {
using Covariance3d = sensor_msgs::Imu::_angular_velocity_covariance_type;

using Covariance6d = geometry_msgs::PoseWithCovariance::_covariance_type;

using Covariance9d = boost::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,   
        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 StaticTF 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_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, StaticTF::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, StaticTF::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);

inline void quaternion_to_mavlink(const Eigen::Quaterniond &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;
        ROS_ASSERT_MSG(COV_SIZE == ARR_SIZE,
                                "frame_tf: covariance matrix URT size (%lu) is different from Mavlink msg covariance field size (%lu)",
                                COV_SIZE, ARR_SIZE);

        auto out = covmsg.begin();

        for (size_t x = 0; x < m.cols(); x++) {
                for (size_t 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;
        ROS_ASSERT_MSG(COV_SIZE == ARR_SIZE,
                                "frame_tf: covariance matrix URT size (%lu) is different from Mavlink msg covariance field size (%lu)",
                                COV_SIZE, ARR_SIZE);

        auto in = covmsg.begin();

        for (size_t x = 0; x < covmat.cols(); x++) {
                for (size_t 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):
//     cog.outl("""//! @brief Helper to convert common ROS geometry_msgs::{tr} to Eigen::{te}""".format(**locals()))
//     cog.outl("""inline Eigen::{te} to_eigen(const geometry_msgs::{tr} r) {{""".format(**locals()))
//     cog.outl("""\treturn Eigen::{te}({fl});""".format(te=te, fl=", ".join(["r." + f for f in fields])))
//     cog.outl("""}""")
//
// 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::Point r) {
        return Eigen::Vector3d(r.x, r.y, r.z);
}
inline Eigen::Vector3d to_eigen(const geometry_msgs::Vector3 r) {
        return Eigen::Vector3d(r.x, r.y, r.z);
}
inline Eigen::Quaterniond to_eigen(const geometry_msgs::Quaternion r) {
        return Eigen::Quaterniond(r.w, r.x, r.y, r.z);
}
// [[[end]]] (checksum: 1b3ada1c4245d4e31dcae9768779b952)
}       // namespace ftf
}       // namespace mavros