Namespaces | Typedefs | Enumerations | Functions
frame_tf.h File Reference

Frame transformation utilities. More...

#include <array>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <ros/assert.h>
#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>
Include dependency graph for frame_tf.h:
This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Namespaces

 mavros
 
 mavros::ftf
 
 mavros::ftf::detail
 

Typedefs

using mavros::ftf::Covariance3d = sensor_msgs::Imu::_angular_velocity_covariance_type
 Type matching rosmsg for 3x3 covariance matrix. More...
 
using mavros::ftf::Covariance6d = geometry_msgs::PoseWithCovariance::_covariance_type
 Type matching rosmsg for 6x6 covariance matrix. More...
 
using mavros::ftf::Covariance9d = boost::array< double, 81 >
 Type matching rosmsg for 9x9 covariance matrix. More...
 
using mavros::ftf::EigenMapConstCovariance3d = Eigen::Map< const Eigen::Matrix< double, 3, 3, Eigen::RowMajor > >
 
using mavros::ftf::EigenMapConstCovariance6d = Eigen::Map< const Eigen::Matrix< double, 6, 6, Eigen::RowMajor > >
 
using mavros::ftf::EigenMapConstCovariance9d = Eigen::Map< const Eigen::Matrix< double, 9, 9, Eigen::RowMajor > >
 
using mavros::ftf::EigenMapCovariance3d = Eigen::Map< Eigen::Matrix< double, 3, 3, Eigen::RowMajor > >
 Eigen::Map for Covariance3d. More...
 
using mavros::ftf::EigenMapCovariance6d = Eigen::Map< Eigen::Matrix< double, 6, 6, Eigen::RowMajor > >
 Eigen::Map for Covariance6d. More...
 
using mavros::ftf::EigenMapCovariance9d = Eigen::Map< Eigen::Matrix< double, 9, 9, Eigen::RowMajor > >
 Eigen::Map for Covariance9d. More...
 

Enumerations

enum  mavros::ftf::StaticEcefTF { mavros::ftf::StaticEcefTF::ECEF_TO_ENU, mavros::ftf::StaticEcefTF::ENU_TO_ECEF }
 Orientation transform options when applying rotations to data, for ECEF. More...
 
enum  mavros::ftf::StaticTF {
  mavros::ftf::StaticTF::NED_TO_ENU, mavros::ftf::StaticTF::ENU_TO_NED, mavros::ftf::StaticTF::AIRCRAFT_TO_BASELINK, mavros::ftf::StaticTF::BASELINK_TO_AIRCRAFT,
  mavros::ftf::StaticTF::ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK, mavros::ftf::StaticTF::ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT
}
 Orientation transform options when applying rotations to data. More...
 

Functions

template<class T , std::size_t SIZE>
void mavros::ftf::covariance_to_mavlink (const T &cov, std::array< float, SIZE > &covmsg)
 Convert covariance matrix to MAVLink float[n] format. More...
 
template<class T , std::size_t ARR_SIZE>
void mavros::ftf::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. More...
 
Eigen::Quaterniond mavros::ftf::mavlink_to_quaternion (const std::array< float, 4 > &q)
 Convert Mavlink float[4] quaternion to Eigen. More...
 
template<class T , std::size_t ARR_SIZE>
void mavros::ftf::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,n> full covariance matrix. More...
 
Eigen::Quaterniond mavros::ftf::quaternion_from_rpy (const Eigen::Vector3d &rpy)
 Convert euler angles to quaternion. More...
 
Eigen::Quaterniond mavros::ftf::quaternion_from_rpy (const double roll, const double pitch, const double yaw)
 Convert euler angles to quaternion. More...
 
double mavros::ftf::quaternion_get_yaw (const Eigen::Quaterniond &q)
 Get Yaw angle from quaternion. More...
 
template<typename _Scalar , typename std::enable_if< std::is_floating_point< _Scalar >::value, bool >::type = true>
void mavros::ftf::quaternion_to_mavlink (const Eigen::Quaternion< _Scalar > &q, std::array< float, 4 > &qmsg)
 Store Quaternion to MAVLink float[4] format. More...
 
Eigen::Vector3d mavros::ftf::quaternion_to_rpy (const Eigen::Quaterniond &q)
 Convert quaternion to euler angles. More...
 
void mavros::ftf::quaternion_to_rpy (const Eigen::Quaterniond &q, double &roll, double &pitch, double &yaw)
 Convert quaternion to euler angles. More...
 
Eigen::Vector3d mavros::ftf::to_eigen (const geometry_msgs::Point r)
 Helper to convert common ROS geometry_msgs::Point to Eigen::Vector3d. More...
 
Eigen::Vector3d mavros::ftf::to_eigen (const geometry_msgs::Vector3 r)
 Helper to convert common ROS geometry_msgs::Vector3 to Eigen::Vector3d. More...
 
Eigen::Quaterniond mavros::ftf::to_eigen (const geometry_msgs::Quaternion r)
 Helper to convert common ROS geometry_msgs::Quaternion to Eigen::Quaterniond. More...
 
Eigen::Vector3d mavros::ftf::detail::transform_frame (const Eigen::Vector3d &vec, const Eigen::Quaterniond &q)
 Transform data expressed in one frame to another frame. More...
 
Covariance3d mavros::ftf::detail::transform_frame (const Covariance3d &cov, const Eigen::Quaterniond &q)
 Transform 3x3 convariance expressed in one frame to another. More...
 
Covariance6d mavros::ftf::detail::transform_frame (const Covariance6d &cov, const Eigen::Quaterniond &q)
 Transform 6x6 convariance expressed in one frame to another. More...
 
Covariance9d mavros::ftf::detail::transform_frame (const Covariance9d &cov, const Eigen::Quaterniond &q)
 Transform 9x9 convariance expressed in one frame to another. More...
 
template<class T >
mavros::ftf::transform_frame_aircraft_baselink (const T &in)
 Transform data expressed in Aircraft frame to Baselink frame. More...
 
template<class T >
mavros::ftf::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 aircraft frame to ENU frame. More...
 
template<class T >
mavros::ftf::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 aircraft frame to NED frame. More...
 
template<class T >
mavros::ftf::transform_frame_baselink_aircraft (const T &in)
 Transform data expressed in Baselink frame to Aircraft frame. More...
 
template<class T >
mavros::ftf::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_link to ENU frame. More...
 
template<class T >
mavros::ftf::transform_frame_ecef_enu (const T &in, const T &map_origin)
 Transform data expressed in ECEF frame to ENU frame. More...
 
template<class T >
mavros::ftf::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 aircraft frame. More...
 
template<class T >
mavros::ftf::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 to base_link frame. More...
 
template<class T >
mavros::ftf::transform_frame_enu_ecef (const T &in, const T &map_origin)
 Transform data expressed in ENU frame to ECEF frame. More...
 
template<class T >
mavros::ftf::transform_frame_enu_ned (const T &in)
 Transform data expressed in ENU to NED frame. More...
 
template<class T >
mavros::ftf::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 aircraft frame. More...
 
template<class T >
mavros::ftf::transform_frame_ned_enu (const T &in)
 Transform data expressed in NED to ENU frame. More...
 
Eigen::Quaterniond mavros::ftf::detail::transform_orientation (const Eigen::Quaterniond &q, const StaticTF transform)
 Transform representation of attitude from 1 frame to another (e.g. transfrom attitude from representing from base_link -> NED to representing base_link -> ENU) More...
 
template<class T >
mavros::ftf::transform_orientation_absolute_frame_aircraft_baselink (const T &in)
 Transform from attitude represented WRT aircraft frame to attitude represented WRT base_link frame, treating aircraft frame as in an absolute frame of reference (local NED). More...
 
template<class T >
mavros::ftf::transform_orientation_absolute_frame_baselink_aircraft (const T &in)
 Transform from attitude represented WRT baselink frame to attitude represented WRT body frame, treating baselink frame as in an absolute frame of reference (local NED). More...
 
template<class T >
mavros::ftf::transform_orientation_aircraft_baselink (const T &in)
 Transform from attitude represented WRT aircraft frame to attitude represented WRT base_link frame. More...
 
template<class T >
mavros::ftf::transform_orientation_baselink_aircraft (const T &in)
 Transform from attitude represented WRT baselink frame to attitude represented WRT body frame. More...
 
template<class T >
mavros::ftf::transform_orientation_enu_ned (const T &in)
 Transform from attitude represented WRT ENU frame to attitude represented WRT NED frame. More...
 
template<class T >
mavros::ftf::transform_orientation_ned_enu (const T &in)
 Transform from attitude represented WRT NED frame to attitude represented WRT ENU frame. More...
 
Eigen::Vector3d mavros::ftf::detail::transform_static_frame (const Eigen::Vector3d &vec, const StaticTF transform)
 Transform data expressed in one frame to another frame. More...
 
Covariance3d mavros::ftf::detail::transform_static_frame (const Covariance3d &cov, const StaticTF transform)
 Transform 3d convariance expressed in one frame to another. More...
 
Covariance6d mavros::ftf::detail::transform_static_frame (const Covariance6d &cov, const StaticTF transform)
 Transform 6d convariance expressed in one frame to another. More...
 
Covariance9d mavros::ftf::detail::transform_static_frame (const Covariance9d &cov, const StaticTF transform)
 Transform 9d convariance expressed in one frame to another. More...
 
Eigen::Vector3d mavros::ftf::detail::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. More...
 

Detailed Description

Frame transformation utilities.

Author
Vladimir Ermakov vooon.nosp@m.341@.nosp@m.gmail.nosp@m..com
Eddy Scott scott.nosp@m..edw.nosp@m.ard@a.nosp@m.uror.nosp@m.a.aer.nosp@m.o
Nuno Marques n.mar.nosp@m.ques.nosp@m.21@ho.nosp@m.tmai.nosp@m.l.com

Definition in file frame_tf.h.



mavros
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 13 2023 02:17:50