Unity2Ros

//Usage will be CalculateCoordinateTransformation(virtualFrameObject.transform, VICONFrameObject.transform);
    private void CalculateCoordinateTransformation(Transform virtualFrameTransform, Transform viconFrameTransform)
    {
        //Matrix4x4 viconFrameObj = Matrix4x4.identity;
        //viconFrameObj.SetTRS(viconFrameTransform.position, viconFrameTransform.rotation, new Vector3(1, 1, 1));

        //Matrix4x4 virtualFrame = Matrix4x4.identity;
        //virtualFrame.SetTRS(virtualFrameTransform.position, virtualFrameTransform.rotation, new Vector3(1, 1, 1));

        //unityToVicon = virtualFrame * viconFrameObj.inverse;
        //Vector4 hh = unityToVicon.GetColumn(3);

        //unityToVicon_translation.Set(hh.x, hh.y, hh.z);
        //unityToVicon_rotation = unityToVicon.rotation;


        //////// Vladimir's Code
        ///

        Vector3 Unity_Pos = virtualFrameTransform.position;
        Quaternion Unity_Rot = Quaternion.Normalize(virtualFrameTransform.rotation);

        // Vector3 offset_v = new Vector3(2, 2, 5 / 3) * 0.0254f;
        //Quaternion offset_q = new Quaternion(offset_v.x, offset_v.y, offset_v.z, 0);
        //offset_q = Unity_Rot * offset_q * new Quaternion(-Unity_Rot.x, -Unity_Rot.y, -Unity_Rot.z, Unity_Rot.w);

        //Unity_Pos -= new Vector3(offset_q.x, offset_q.y, offset_q.z);
        //Unity_Rot = Quaternion.Normalize(Unity_Rot * Quaternion.Normalize(Quaternion.Euler(0, 90, 0)));

        Vector3 LHS_Vicon_Pos = new Vector3(viconFrameTransform.position.x, viconFrameTransform.position.y, viconFrameTransform.position.z).Ros2Unity();
        Quaternion LHS_Vicon_Rot = new Quaternion(viconFrameTransform.rotation.x, viconFrameTransform.rotation.y, viconFrameTransform.rotation.z, viconFrameTransform.rotation.w).Ros2Unity();


        Quaternion LHS_Vicon_Rot_Inv = Quaternion.Normalize(Quaternion.Inverse(LHS_Vicon_Rot));
        Quaternion LHS_Vicon_Pos_Q = new Quaternion(LHS_Vicon_Pos.x, LHS_Vicon_Pos.y, LHS_Vicon_Pos.z, 0);
        Quaternion v1 = (LHS_Vicon_Rot_Inv * LHS_Vicon_Pos_Q) * new Quaternion(-LHS_Vicon_Rot_Inv.x, -LHS_Vicon_Rot_Inv.y, -LHS_Vicon_Rot_Inv.z, LHS_Vicon_Rot_Inv.w);
        Quaternion v2 = (Unity_Rot * v1) * new Quaternion(-Unity_Rot.x, -Unity_Rot.y, -Unity_Rot.z, Unity_Rot.w);

        unityToVicon_translation = Unity_Pos - new Vector3(v2.x, v2.y, v2.z);
        unityToVicon_rotation = Quaternion.Normalize(new Quaternion(-LHS_Vicon_Rot.x, -LHS_Vicon_Rot.y, -LHS_Vicon_Rot.z, LHS_Vicon_Rot.w) * Unity_Rot);

        unityToVicon = Matrix4x4.TRS(unityToVicon_translation, unityToVicon_rotation, new Vector3(1, 1, 1));

        Debug.Log("Virtual Frame Object Position: " + virtualFrameTransform.position);
        Debug.Log("Virtual Frame Object Rotation: " + virtualFrameTransform.rotation);
        Debug.Log("Vicon Frame Object Position: " + viconFrameTransform.position);
        Debug.Log("Vicon Frame Object Rotation: " + viconFrameTransform.rotation);

        Debug.Log("Output Position: " + unityToVicon_translation);
        Debug.Log("Output Rotation: " + unityToVicon_rotation);
        ////////

        UpdateMapObject();
    }

    private void UpdateMapObject()
    {
        if (VICONMapFrame != null)
        {
            VICONMapFrame.transform.position = unityToVicon_translation;
            VICONMapFrame.transform.rotation = unityToVicon_rotation;
        }
    }