ftf_frame_conversions.cpp

Go to the documentation of this file.

/*
 * Copyright 2015 Nuno Marques.
 * Copyright 2016 Vladimir Ermakov
 *
 * 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
 */

#include <mavros/frame_tf.h>
#include <stdexcept>

namespace mavros {
namespace ftf {
namespace detail {
static const auto NED_ENU_Q = quaternion_from_rpy(M_PI, 0.0, M_PI_2);

static const auto AIRCRAFT_BASELINK_Q = quaternion_from_rpy(M_PI, 0.0, 0.0);

static const Eigen::Affine3d NED_ENU_AFFINE(NED_ENU_Q);

static const Eigen::Affine3d AIRCRAFT_BASELINK_AFFINE(AIRCRAFT_BASELINK_Q);

static const auto NED_ENU_R = NED_ENU_Q.normalized().toRotationMatrix();
static const auto AIRCRAFT_BASELINK_R = AIRCRAFT_BASELINK_Q.normalized().toRotationMatrix();

static const Eigen::PermutationMatrix<3> NED_ENU_REFLECTION_XY(Eigen::Vector3i(1,0,2));
static const Eigen::DiagonalMatrix<double,3> NED_ENU_REFLECTION_Z(1,1,-1);

using Matrix6d = Eigen::Matrix<double, 6, 6>;
using Matrix9d = Eigen::Matrix<double, 9, 9>;


Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond &q, const StaticTF transform)
{
        // Transform the attitude representation from frame to frame.
        // The proof for this transform can be seen
        // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/
        switch (transform) {
        case StaticTF::NED_TO_ENU:
        case StaticTF::ENU_TO_NED:
                return NED_ENU_Q * q;

        case StaticTF::AIRCRAFT_TO_BASELINK:
        case StaticTF::BASELINK_TO_AIRCRAFT:
                return q * AIRCRAFT_BASELINK_Q;

        case StaticTF::ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK:
        case StaticTF::ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT:
                return AIRCRAFT_BASELINK_Q * q;

        default:
                ROS_FATAL("unsupported StaticTF mode");
                return q;
        }
}


Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const StaticTF transform)
{
        switch (transform) {
        case StaticTF::NED_TO_ENU:
        case StaticTF::ENU_TO_NED:
                return NED_ENU_REFLECTION_XY * (NED_ENU_REFLECTION_Z * vec);

        case StaticTF::AIRCRAFT_TO_BASELINK:
        case StaticTF::BASELINK_TO_AIRCRAFT:
                return AIRCRAFT_BASELINK_AFFINE * vec;

        default:
                ROS_FATAL("unsupported StaticTF mode");
                return vec;
        }
}

Covariance3d transform_static_frame(const Covariance3d &cov, const StaticTF transform)
{
        Covariance3d cov_out_;
        EigenMapConstCovariance3d cov_in(cov.data());
        EigenMapCovariance3d cov_out(cov_out_.data());

        switch (transform) {
        case StaticTF::NED_TO_ENU:
        case StaticTF::ENU_TO_NED:
                cov_out = NED_ENU_REFLECTION_XY * (NED_ENU_REFLECTION_Z * cov_in * NED_ENU_REFLECTION_Z) *
                          NED_ENU_REFLECTION_XY.transpose();
                return cov_out_;

        case StaticTF::AIRCRAFT_TO_BASELINK:
        case StaticTF::BASELINK_TO_AIRCRAFT:
                cov_out = cov_in * AIRCRAFT_BASELINK_Q;
                return cov_out_;

        default:
                ROS_FATAL("unsupported StaticTF mode");
                return cov;
        }
}

Covariance6d transform_static_frame(const Covariance6d &cov, const StaticTF transform)
{
        Covariance6d cov_out_;
        Matrix6d R = Matrix6d::Zero();  // not `auto` because Zero ret is const

        EigenMapConstCovariance6d cov_in(cov.data());
        EigenMapCovariance6d cov_out(cov_out_.data());

        switch (transform) {
        case StaticTF::NED_TO_ENU:
        case StaticTF::ENU_TO_NED:
        {
                Eigen::PermutationMatrix<6> NED_ENU_REFLECTION_XY_6 (NED_ENU_REFLECTION_XY.indices().replicate<2,1>());
                NED_ENU_REFLECTION_XY_6.indices().middleRows<3>(3).array() += 3;
                Eigen::DiagonalMatrix<double,6> NED_ENU_REFLECTION_Z_6(NED_ENU_REFLECTION_Z.diagonal().replicate<2,1>());

                cov_out = NED_ENU_REFLECTION_XY_6 * (NED_ENU_REFLECTION_Z_6 * cov_in * NED_ENU_REFLECTION_Z_6) *
                          NED_ENU_REFLECTION_XY_6.transpose();

                return cov_out_;
        }
        case StaticTF::AIRCRAFT_TO_BASELINK:
        case StaticTF::BASELINK_TO_AIRCRAFT:
                R.block<3, 3>(0, 0) =
                        R.block<3, 3>(3, 3) = AIRCRAFT_BASELINK_R;

                cov_out = R * cov_in * R.transpose();
                return cov_out_;

        default:
                ROS_FATAL("unsupported StaticTF mode");
                return cov;
        }
}

Covariance9d transform_static_frame(const Covariance9d &cov, const StaticTF transform)
{
        Covariance9d cov_out_;
        Matrix9d R = Matrix9d::Zero();

        EigenMapConstCovariance9d cov_in(cov.data());
        EigenMapCovariance9d cov_out(cov_out_.data());

        switch (transform) {
        case StaticTF::NED_TO_ENU:
        case StaticTF::ENU_TO_NED:
        {
                Eigen::PermutationMatrix<9> NED_ENU_REFLECTION_XY_9 (NED_ENU_REFLECTION_XY.indices().replicate<3,1>());
                NED_ENU_REFLECTION_XY_9.indices().middleRows<3>(3).array() += 3;
                NED_ENU_REFLECTION_XY_9.indices().middleRows<3>(6).array() += 6;
                Eigen::DiagonalMatrix<double,9> NED_ENU_REFLECTION_Z_9(NED_ENU_REFLECTION_Z.diagonal().replicate<3,1>());

                cov_out = NED_ENU_REFLECTION_XY_9 * (NED_ENU_REFLECTION_Z_9 * cov_in * NED_ENU_REFLECTION_Z_9) *
                          NED_ENU_REFLECTION_XY_9.transpose();

                return cov_out_;
        }
        case StaticTF::AIRCRAFT_TO_BASELINK:
        case StaticTF::BASELINK_TO_AIRCRAFT:
                R.block<3, 3>(0, 0) =
                        R.block<3, 3>(3, 3) =
                                R.block<3, 3>(6, 6) = AIRCRAFT_BASELINK_R;

                cov_out = R * cov_in * R.transpose();
                return cov_out_;

        default:
                ROS_FATAL("unsupported StaticTF mode");
                return cov;
        }
}

Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const Eigen::Vector3d &map_origin, const StaticEcefTF transform)
{
        static constexpr double DEG_TO_RAD = (M_PI / 180.0);

        // Don't forget to convert from degrees to radians
        const double sin_lat = std::sin(map_origin.x() * DEG_TO_RAD);
        const double sin_lon = std::sin(map_origin.y() * DEG_TO_RAD);
        const double cos_lat = std::cos(map_origin.x() * DEG_TO_RAD);
        const double cos_lon = std::cos(map_origin.y() * DEG_TO_RAD);

        Eigen::Matrix3d R;
        R << -sin_lon,            cos_lon,           0.0,
             -cos_lon * sin_lat, -sin_lon * sin_lat, cos_lat,
              cos_lon * cos_lat,  sin_lon * cos_lat, sin_lat;

        switch (transform) {
        case StaticEcefTF::ECEF_TO_ENU:
                return R * vec;

        case StaticEcefTF::ENU_TO_ECEF:
                // ENU to ECEF rotation is just an inverse rotation from ECEF to ENU, which means transpose.
                R.transposeInPlace();
                return R * vec;

        default:
                ROS_FATAL("unsupported StaticTF mode");
                return vec;
        }
}

Eigen::Vector3d transform_frame(const Eigen::Vector3d &vec, const Eigen::Quaterniond &q)
{
        Eigen::Affine3d transformation(q);
        return transformation * vec;
}

Covariance3d transform_frame(const Covariance3d &cov, const Eigen::Quaterniond &q)
{
        Covariance3d cov_out_;
        EigenMapConstCovariance3d cov_in(cov.data());
        EigenMapCovariance3d cov_out(cov_out_.data());

        cov_out = cov_in * q;
        return cov_out_;
}

Covariance6d transform_frame(const Covariance6d &cov, const Eigen::Quaterniond &q)
{
        Covariance6d cov_out_;
        Matrix6d R = Matrix6d::Zero();

        EigenMapConstCovariance6d cov_in(cov.data());
        EigenMapCovariance6d cov_out(cov_out_.data());

        R.block<3, 3>(0, 0) =
                R.block<3, 3>(3, 3) = q.normalized().toRotationMatrix();

        cov_out = R * cov_in * R.transpose();
        return cov_out_;
}

Covariance9d transform_frame(const Covariance9d &cov, const Eigen::Quaterniond &q)
{
        Covariance9d cov_out_;
        Matrix9d R = Matrix9d::Zero();

        EigenMapConstCovariance9d cov_in(cov.data());
        EigenMapCovariance9d cov_out(cov_out_.data());

        R.block<3, 3>(0, 0) =
                R.block<3, 3>(3, 3) =
                        R.block<3, 3>(6, 6) = q.normalized().toRotationMatrix();

        cov_out = R * cov_in * R.transpose();
        return cov_out_;
}

}       // namespace detail
}       // namespace ftf
}       // namespace mavros