flightgoggles_ros_bridge/src/Common/transforms.hpp
/**
 * @brief Converts ROS global poses to NED poses. These poses are then converted to
 * Unity left handed coordinates. Assumes that ROS global coordinates for translation are
 * East north up (ENU) and robot local coordinates are North west up (NWU). E.g. the robot
 * local X axis is going out of the camera.
 *
 * @param ENUworld_T_object
 * @return Transform3
 */
@brief Converts ROS global poses to NED poses. These poses are then converted to
Unity left handed coordinates. Assumes that ROS global coordinates for translation are
East north up (ENU) and robot local coordinates are North west up (NWU).

@param ENUworld_T_object
@return Transform3
@brief Converts ROS global poses to NED poses . These poses are then converted to Unity left handed coordinates . Assumes that ROS global coordinates for translation are East north up ( ENU ) and robot local coordinates are North west up ( NWU ) . @param ENUworld_T_object @return Transform3
Transform3 convertEDNToNEDCoordinates(Transform3 EDNworld_T_object)
{
    Matrix4 NED_T_EDN;
    NED_T_EDN <<  0, 1, 0,  0,
                  1, 0, 0,  0,
                  0, 1, 0, 0,
                  0, 0, 0,  1;
    Transform3 NED_pose;
    NED_pose.matrix() = NED_T_EDN * EDNworld_T_object.matrix(); 
    return NED_pose;
}
Transform3 convertEDNToNEDCoordinates ( Transform3 EDNworld_T_object ) { Matrix4 NED_T_EDN ; NED_T_EDN << 0 , 1 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 1 ; Transform3 NED_pose ; NED_pose . matrix ( ) = NED_T_EDN * EDNworld_T_object . matrix ( ) ; return NED_pose ; }
@brief Converts ROS global poses to NED poses.
@brief Converts ROS global poses to NED poses .
// Switch ENU axis to NED axis. // x->y, y->x, z->-z, w->w // clang-format off // clang-format on //EDN_pose_T_NED_pose * EDNworld_T_object.matrix() * EDN_pose_T_NED_pose.transpose(); // Rotate robot pose by -90deg about robot z axis since ROS // expects that "X" is forward in robot frame. // Quaternionx Y_front_to_X_front_quat(sqrt(0.5f),	0.0f, 0.0f, -sqrt(0.5f)); // NED_pose = Y_front_to_X_front_quat * NED_pose;