Commit a056a29f authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[navigation] Added some enu_to_ned helpers

parent 80d53302
...@@ -33,10 +33,6 @@ Eigen::Quaternion<T> enu_to_ned_quaternion() ...@@ -33,10 +33,6 @@ Eigen::Quaternion<T> enu_to_ned_quaternion()
return ned_to_enu_quaternion<T>().inverse(); return ned_to_enu_quaternion<T>().inverse();
} }
/**
* Conversion of a Quaternion from North East Down (NED) convention to
* East North Up convention.
*/
template <typename T> template <typename T>
Eigen::Quaternion<T> ned_to_enu(const Eigen::Quaternion<T>& nedRot) Eigen::Quaternion<T> ned_to_enu(const Eigen::Quaternion<T>& nedRot)
{ {
...@@ -44,10 +40,6 @@ Eigen::Quaternion<T> ned_to_enu(const Eigen::Quaternion<T>& nedRot) ...@@ -44,10 +40,6 @@ Eigen::Quaternion<T> ned_to_enu(const Eigen::Quaternion<T>& nedRot)
return q.inverse()*nedRot*q; return q.inverse()*nedRot*q;
} }
/**
* Conversion of a Matrix3 from North East Down (NED) convention to
* East North Up convention.
*/
template <typename T> template <typename T>
Eigen::Matrix3<T> ned_to_enu(const Eigen::Matrix3<T>& m) Eigen::Matrix3<T> ned_to_enu(const Eigen::Matrix3<T>& m)
{ {
...@@ -56,16 +48,33 @@ Eigen::Matrix3<T> ned_to_enu(const Eigen::Matrix3<T>& m) ...@@ -56,16 +48,33 @@ Eigen::Matrix3<T> ned_to_enu(const Eigen::Matrix3<T>& m)
} }
/**
* Conversion of a Vector3 from North East Down (NED) convention to
* East North Up convention.
*/
template <typename T> template <typename T>
Eigen::Vector3<T> ned_to_enu(const Eigen::Vector3<T>& v) Eigen::Vector3<T> ned_to_enu(const Eigen::Vector3<T>& v)
{ {
return (Eigen::Vector3<T>() << v(1), v(0), -v(2)).finished(); return (Eigen::Vector3<T>() << v(1), v(0), -v(2)).finished();
} }
template <typename T>
Eigen::Quaternion<T> enu_to_ned(const Eigen::Quaternion<T>& nedRot)
{
auto q = enu_to_ned_quaternion<T>();
return q.inverse()*nedRot*q;
}
template <typename T>
Eigen::Matrix3<T> enu_to_ned(const Eigen::Matrix3<T>& m)
{
auto P = enu_to_ned_matrix<T>();
return P.transpose()*m*P;
}
template <typename T>
Eigen::Vector3<T> enu_to_ned(const Eigen::Vector3<T>& v)
{
return (Eigen::Vector3<T>() << v(1), v(0), -v(2)).finished();
}
/** /**
* Conversion from Tait-Bryan angles to Quaternion in NED convention. * Conversion from Tait-Bryan angles to Quaternion in NED convention.
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment