NED to ENU

end_to_enu

#include <cmath>
#include <cstdio>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>

void print_quat(double x, double y, double z, double w)
{
  tf2::Quaternion quat(x, y, z, w);

  fprintf(stderr, "Quaternion:\n");
  fprintf(stderr, "x: %f\ny: %f\nz: %f\nw: %f\n\n", quat.x(), quat.y(), quat.z(), quat.w());

  double roll, pitch, yaw;

  tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);
  fprintf(stderr, "RPY:\n");
  fprintf(stderr, "roll: %f\npitch: %f\nyaw: %f\n\n", roll, pitch, yaw);
}

tf2::Quaternion rpy_to_quat(double roll, double pitch, double yaw)
{
  tf2::Quaternion quat;
  quat.setRPY(roll, pitch, yaw);
  return quat;
}

tf2::Quaternion ned_to_enu(const tf2::Quaternion & ned_quat)
{
  // Fourth try
  tf2::Quaternion ned_to_enu_quat(std::sqrt(0.5), -std::sqrt(0.5), 0.0, 0.0);
  return ned_to_enu_quat * ned_quat;
}

int main()
{
  tf2::Quaternion quat;

  fprintf(stderr, "-------------- NED NEGATIVE PITCH ----------------\n");
  quat = rpy_to_quat(0.0, -1.570795, 0.0);
  print_quat(quat.x(), quat.y(), quat.z(), quat.w());

  fprintf(stderr, "-------------- SHOULD BE AN ENU POSITIVE ROLL ----------------\n");
  quat = ned_to_enu(quat);
  print_quat(quat.x(), quat.y(), quat.z(), quat.w());

  return 0;
}