Commit 82b83a44 authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[navigation] Added some conversion

parent 3c8e1c3d
...@@ -2,6 +2,7 @@ ...@@ -2,6 +2,7 @@
#define _DEF_RTAC_BASE_NAVIGATION_H_ #define _DEF_RTAC_BASE_NAVIGATION_H_
#include <rtac_base/types/common.h> #include <rtac_base/types/common.h>
#include <rtac_base/types/Pose.h>
#include <rtac_base/geometry.h> #include <rtac_base/geometry.h>
namespace rtac { namespace navigation { namespace rtac { namespace navigation {
...@@ -47,13 +48,19 @@ Eigen::Matrix3<T> ned_to_enu(const Eigen::Matrix3<T>& m) ...@@ -47,13 +48,19 @@ Eigen::Matrix3<T> ned_to_enu(const Eigen::Matrix3<T>& m)
return P.transpose()*m*P; return P.transpose()*m*P;
} }
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>
types::Pose<T> ned_to_enu(const types::Pose<T>& p)
{
return types::Pose<T>(ned_to_enu<T>(p.translation()),
ned_to_enu<T>(p.orientation()));
}
template <typename T> template <typename T>
Eigen::Quaternion<T> enu_to_ned(const Eigen::Quaternion<T>& nedRot) Eigen::Quaternion<T> enu_to_ned(const Eigen::Quaternion<T>& nedRot)
{ {
...@@ -75,6 +82,12 @@ Eigen::Vector3<T> enu_to_ned(const Eigen::Vector3<T>& v) ...@@ -75,6 +82,12 @@ Eigen::Vector3<T> enu_to_ned(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>
types::Pose<T> enu_to_ned(const types::Pose<T>& p)
{
return types::Pose<T>(enu_to_ned(p.translation()),
enu_to_ned(p.orientation()));
}
/** /**
* 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