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

[navigation] Added some conversion

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