flightgoggles_ros_bridge/src/Common/transforms.hpp
// Converts a given LCM drone pose and relative camera pose into a global Unity
// pose for the camera.
/* Move world_T_cam from drone (right handed) coordinate system to Unity (left
handed) coordinate system:
Input vector in unity coordinates goes through the following
transformations:
Unity coordinates -> Drone coordinates -> perform rotation -> unity
coordinates
*/
Converts a given LCM drone pose and relative camera pose into a global Unity
pose for the camera.
Move world_T_cam from drone (right handed) coordinate system to Unity (left
handed) coordinate system:
Input vector in unity coordinates goes through the following
transformations:
Unity coordinates -> Drone coordinates -> perform rotation -> unity
coordinates
Converts a given LCM drone pose and relative camera pose into a global Unity pose for the camera . Move world_T_cam from drone ( right handed ) coordinate system to Unity ( left handed ) coordinate system : Input vector in unity coordinates goes through the following transformations : Unity coordinates - > Drone coordinates - > perform rotation - > unity coordinates
Transform3 convertCameraAndDronePoseToUnityCoordinates(
    Transform3 world_T_body, Transform3 body_T_cam,
    Transform3 unityWorld_T_NEDworld)
{
    Transform3 world_T_cam = world_T_body * body_T_cam;
    Matrix4 cam_T_unitycam;
    cam_T_unitycam <<   0, 1, 0, 0,
                        0, 0, 1, 0,
                        1, 0, 0, 0,
                        0, 0, 0, 1;
    Transform3 world_T_unitycam;
    world_T_unitycam.matrix() =
        world_T_cam *
        cam_T_unitycam; 
    Matrix4
        unity_from_drone; 
    unity_from_drone << 0, 1,  0, 0,
                        0, 0, -1, 0,
                        1, 0,  0, 0,
                        0, 0,  0, 1;
    Transform3 unityWorld_T_unitycam_unity; 
    unityWorld_T_unitycam_unity.matrix() =
        unity_from_drone * unityWorld_T_NEDworld.matrix() *
        world_T_unitycam.matrix() * unity_from_drone.transpose();
    return unityWorld_T_unitycam_unity;
}
Transform3 convertCameraAndDronePoseToUnityCoordinates ( Transform3 world_T_body , Transform3 body_T_cam , Transform3 unityWorld_T_NEDworld ) { Transform3 world_T_cam = world_T_body * body_T_cam ; Matrix4 cam_T_unitycam ; cam_T_unitycam << 0 , 1 , 0 , 0 , 0 , 0 , 1 , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 0 , 1 ; Transform3 world_T_unitycam ; world_T_unitycam . matrix ( ) = world_T_cam * cam_T_unitycam ; Matrix4 unity_from_drone ; unity_from_drone << 0 , 1 , 0 , 0 , 0 , 0 , -1 , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 0 , 1 ; Transform3 unityWorld_T_unitycam_unity ; unityWorld_T_unitycam_unity . matrix ( ) = unity_from_drone * unityWorld_T_NEDworld . matrix ( ) * world_T_unitycam . matrix ( ) * unity_from_drone . transpose ( ) ; return unityWorld_T_unitycam_unity ; }
Converts a given LCM drone pose and relative camera pose into a global Unity
pose for the camera.
Converts a given LCM drone pose and relative camera pose into a global Unity pose for the camera .
// clang-format off // clang-format on // Move from z aligned camera to x aligned camera // x->z, y->x, z->-y /**< Coordinate change from drone coordinate system to
                            unity coordinate system */ // clang-format off // clang-format on /**< Transformation from camera to
                                                world in unity coordinates */