SHARE
TWEET

Unity2Ros

a guest Jul 23rd, 2019 56 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. //Usage will be CalculateCoordinateTransformation(virtualFrameObject.transform, VICONFrameObject.transform);
  2.     private void CalculateCoordinateTransformation(Transform virtualFrameTransform, Transform viconFrameTransform)
  3.     {
  4.         //Matrix4x4 viconFrameObj = Matrix4x4.identity;
  5.         //viconFrameObj.SetTRS(viconFrameTransform.position, viconFrameTransform.rotation, new Vector3(1, 1, 1));
  6.  
  7.         //Matrix4x4 virtualFrame = Matrix4x4.identity;
  8.         //virtualFrame.SetTRS(virtualFrameTransform.position, virtualFrameTransform.rotation, new Vector3(1, 1, 1));
  9.  
  10.         //unityToVicon = virtualFrame * viconFrameObj.inverse;
  11.         //Vector4 hh = unityToVicon.GetColumn(3);
  12.  
  13.         //unityToVicon_translation.Set(hh.x, hh.y, hh.z);
  14.         //unityToVicon_rotation = unityToVicon.rotation;
  15.  
  16.  
  17.         //////// Vladimir's Code
  18.         ///
  19.  
  20.         Vector3 Unity_Pos = virtualFrameTransform.position;
  21.         Quaternion Unity_Rot = Quaternion.Normalize(virtualFrameTransform.rotation);
  22.  
  23.         // Vector3 offset_v = new Vector3(2, 2, 5 / 3) * 0.0254f;
  24.         //Quaternion offset_q = new Quaternion(offset_v.x, offset_v.y, offset_v.z, 0);
  25.         //offset_q = Unity_Rot * offset_q * new Quaternion(-Unity_Rot.x, -Unity_Rot.y, -Unity_Rot.z, Unity_Rot.w);
  26.  
  27.         //Unity_Pos -= new Vector3(offset_q.x, offset_q.y, offset_q.z);
  28.         //Unity_Rot = Quaternion.Normalize(Unity_Rot * Quaternion.Normalize(Quaternion.Euler(0, 90, 0)));
  29.  
  30.         Vector3 LHS_Vicon_Pos = new Vector3(viconFrameTransform.position.x, viconFrameTransform.position.y, viconFrameTransform.position.z).Ros2Unity();
  31.         Quaternion LHS_Vicon_Rot = new Quaternion(viconFrameTransform.rotation.x, viconFrameTransform.rotation.y, viconFrameTransform.rotation.z, viconFrameTransform.rotation.w).Ros2Unity();
  32.  
  33.        
  34.         Quaternion LHS_Vicon_Rot_Inv = Quaternion.Normalize(Quaternion.Inverse(LHS_Vicon_Rot));
  35.         Quaternion LHS_Vicon_Pos_Q = new Quaternion(LHS_Vicon_Pos.x, LHS_Vicon_Pos.y, LHS_Vicon_Pos.z, 0);
  36.         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);
  37.         Quaternion v2 = (Unity_Rot * v1) * new Quaternion(-Unity_Rot.x, -Unity_Rot.y, -Unity_Rot.z, Unity_Rot.w);
  38.  
  39.         unityToVicon_translation = Unity_Pos - new Vector3(v2.x, v2.y, v2.z);
  40.         unityToVicon_rotation = Quaternion.Normalize(new Quaternion(-LHS_Vicon_Rot.x, -LHS_Vicon_Rot.y, -LHS_Vicon_Rot.z, LHS_Vicon_Rot.w) * Unity_Rot);
  41.  
  42.         unityToVicon = Matrix4x4.TRS(unityToVicon_translation, unityToVicon_rotation, new Vector3(1, 1, 1));
  43.  
  44.         Debug.Log("Virtual Frame Object Position: " + virtualFrameTransform.position);
  45.         Debug.Log("Virtual Frame Object Rotation: " + virtualFrameTransform.rotation);
  46.         Debug.Log("Vicon Frame Object Position: " + viconFrameTransform.position);
  47.         Debug.Log("Vicon Frame Object Rotation: " + viconFrameTransform.rotation);
  48.  
  49.         Debug.Log("Output Position: " + unityToVicon_translation);
  50.         Debug.Log("Output Rotation: " + unityToVicon_rotation);
  51.         ////////
  52.  
  53.         UpdateMapObject();
  54.     }
  55.  
  56.     private void UpdateMapObject()
  57.     {
  58.         if (VICONMapFrame != null)
  59.         {
  60.             VICONMapFrame.transform.position = unityToVicon_translation;
  61.             VICONMapFrame.transform.rotation = unityToVicon_rotation;
  62.         }
  63.     }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top