Commit 80d53302 authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[navigation] Some bugs fixes

parent d25fa63b
......@@ -14,6 +14,12 @@ Eigen::Matrix<T,3,3> ned_to_enu_matrix()
0,0,-1).finished();
}
template <typename T>
Eigen::Matrix<T,3,3> enu_to_ned_matrix()
{
return ned_to_enu_matrix<T>().transpose();
}
template <typename T>
Eigen::Quaternion<T> ned_to_enu_quaternion()
{
......@@ -21,26 +27,46 @@ Eigen::Quaternion<T> ned_to_enu_quaternion()
return Eigen::Quaternion<T>(0, 0.70710678118, 0.70710678118, 0);
}
template <typename T>
Eigen::Quaternion<T> enu_to_ned_quaternion()
{
return ned_to_enu_quaternion<T>().inverse();
}
/**
* Conversion of a rotation matrix from North East Down (NED) convention to
* Conversion of a Quaternion from North East Down (NED) convention to
* East North Up convention.
*/
template <typename T>
Eigen::Matrix<T,3,3> ned_to_enu(const Eigen::Matrix<T,3,3>& nedRot)
Eigen::Quaternion<T> ned_to_enu(const Eigen::Quaternion<T>& nedRot)
{
return ned_to_enu_matrix<T>()*nedRot;
auto q = ned_to_enu_quaternion<T>();
return q.inverse()*nedRot*q;
}
/**
* Conversion of a Quaternion from North East Down (NED) convention to
* Conversion of a Matrix3 from North East Down (NED) convention to
* East North Up convention.
*/
template <typename T>
Eigen::Quaternion<T> ned_to_enu(const Eigen::Quaternion<T>& nedRot)
Eigen::Matrix3<T> ned_to_enu(const Eigen::Matrix3<T>& m)
{
auto P = ned_to_enu_matrix<T>();
return P.transpose()*m*P;
}
/**
* Conversion of a Vector3 from North East Down (NED) convention to
* East North Up convention.
*/
template <typename T>
Eigen::Vector3<T> ned_to_enu(const Eigen::Vector3<T>& v)
{
return ned_to_enu_quaternion<T>()*nedRot;
return (Eigen::Vector3<T>() << v(1), v(0), -v(2)).finished();
}
/**
* Conversion from Tait-Bryan angles to Quaternion in NED convention.
*
......
......@@ -16,6 +16,7 @@ list(APPEND test_names
ppmformat_test.cpp
nmea_utils.cpp
navigation_test.cpp
)
list(APPEND test_deps
......
#include <iostream>
using namespace std;
#include <rtac_base/navigation.h>
using namespace rtac::types;
using namespace rtac::navigation;
using Mat3 = Matrix3<double>;
using Vec3 = Vector3<double>;
int main()
{
Mat3 r0ned = quaternion_from_nautical_degrees<double>(45,10,0).toRotationMatrix();
Mat3 r0enu = ned_to_enu(r0ned);
cout << r0ned << endl << endl;
cout << r0enu << endl << endl;
return 0;
}
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