OptiTrackConnector.cs 7.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195
  1. using System;
  2. using System.Runtime.InteropServices;
  3. using System.Threading;
  4. namespace OptiTrack
  5. {
  6. public class OptiTrackConnector
  7. {
  8. private bool _stop = true;
  9. private Thread _tracker;
  10. public delegate void OnFrameReady(Frame frame);
  11. public bool Init(String path)
  12. {
  13. int a = OptiTrackNativeWrapper.TT_Initialize();
  14. int b = OptiTrackNativeWrapper.TT_LoadProject(path);
  15. Console.WriteLine("a:" + a + ", b:" + b);
  16. if (a + b != 0)
  17. {
  18. // breakpoint
  19. }
  20. return a + b == 0;
  21. }
  22. public void StartTracking(OnFrameReady frameReadyDelegate)
  23. {
  24. _stop = false;
  25. _tracker = new Thread(delegate ()
  26. {
  27. HighPerformanceTimer hpt = new HighPerformanceTimer();
  28. int i = 0;
  29. hpt.Start();
  30. while (!_stop)
  31. {
  32. if (i++ == 200)
  33. {
  34. //Console.WriteLine("Tracker FPS: " + 200.0 / HPT.Stop());
  35. i = 0;
  36. hpt.Start();
  37. }
  38. OptiTrackNativeWrapper.TT_Update();
  39. frameReadyDelegate(BuildFrame());
  40. Thread.Sleep(15);
  41. }
  42. });
  43. _tracker.Start();
  44. }
  45. Frame _lastFrame;
  46. private Frame BuildFrame()
  47. {
  48. Frame frame = new Frame(OptiTrackNativeWrapper.TT_FrameMarkerCount(), OptiTrackNativeWrapper.TT_TrackableCount());
  49. for (int i = 0; i < OptiTrackNativeWrapper.TT_FrameMarkerCount(); i++)
  50. {
  51. frame.Markers[i] = new Marker(
  52. OptiTrackNativeWrapper.TT_FrameMarkerX(i),
  53. OptiTrackNativeWrapper.TT_FrameMarkerY(i),
  54. OptiTrackNativeWrapper.TT_FrameMarkerZ(i))
  55. {
  56. BoundToTrackable = null
  57. };
  58. }
  59. for (int i = 0; i < OptiTrackNativeWrapper.TT_TrackableCount(); i++)
  60. {
  61. Trackable t = new Trackable
  62. {
  63. IsTracked = OptiTrackNativeWrapper.TT_IsTrackableTracked(i),
  64. Id = OptiTrackNativeWrapper.TT_TrackableID(i)
  65. };
  66. if (t.IsTracked)
  67. {
  68. OptiTrackNativeWrapper.TT_TrackableLocation(i, out t.X, out t.Y, out t.Z, out t.Qx, out t.Qy, out t.Qz, out t.Qw,
  69. out t.Pitch, out t.Yaw, out t.Roll);
  70. if(_lastFrame != null)
  71. {
  72. if (_lastFrame.Trackables[i].IsAlmostSameCoordinates(t))
  73. t.IsTracked = false;
  74. }
  75. /*for (int j = 0; j < OptiTrackNativeWrapper.TT_TrackableMarkerCount(i); j++)
  76. {
  77. float markerRadius = trackableMarker(i, j);
  78. frame.markers[boundTrackable(frame, markerRadius,t)].boundToTrackable = t;
  79. }*/
  80. }
  81. frame.Trackables[i] = t;
  82. }
  83. _lastFrame = frame;
  84. return frame;
  85. }
  86. /*private float trackableMarker(int trackableId, int markerId)
  87. {
  88. Marker tempMarker = new Marker(0, 0, 0);
  89. OptiTrackNativeWrapper.TT_TrackableMarker(trackableId, markerId, out tempMarker.x, out tempMarker.y, out tempMarker.z);
  90. float markerRadius = (float)Math.Pow(((float)Math.Pow(tempMarker.x, 2.00) + (float)Math.Pow(tempMarker.y, 2.00) + (float)Math.Pow(tempMarker.z, 2.00)),0.50);
  91. markerRadius += (float)0.01;
  92. return markerRadius;
  93. }
  94. private int boundTrackable(Frame frame, float markerRadius, Trackable trackable)
  95. {
  96. List<float> distanceValues = new List<float>();
  97. for (int k = 0; k < frame.markers.Count(); k++)
  98. distanceValues.Add((float)Math.Pow(((float)Math.Pow(trackable.x - frame.markers[k].x, 2.00) + (float)Math.Pow(trackable.y - frame.markers[k].y, 2.00) + (float)Math.Pow(trackable.z - frame.markers[k].z, 2.00)),0.50));
  99. float minimumDistance = markerRadius;
  100. int boundMarkerId = 0;
  101. for (int i = 0; i < distanceValues.Count; ++i)
  102. {
  103. if (distanceValues[i] <= minimumDistance)
  104. {
  105. minimumDistance = distanceValues[i];
  106. boundMarkerId = i;
  107. }
  108. }
  109. return boundMarkerId;
  110. }*/
  111. public void StopTracking()
  112. {
  113. _stop = true;
  114. OptiTrackNativeWrapper.TT_Shutdown();
  115. }
  116. }
  117. class OptiTrackNativeWrapper
  118. {
  119. [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
  120. public static extern int TT_Initialize();
  121. [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
  122. public static extern int TT_Shutdown();
  123. [DllImport("NPTrackingTools.dll", CharSet = CharSet.Auto, CallingConvention = CallingConvention.Cdecl)]
  124. public static extern int TT_LoadProject([MarshalAs(UnmanagedType.LPStr)]string file);
  125. [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
  126. public static extern int TT_Update();
  127. [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
  128. public static extern int TT_FrameMarkerCount();
  129. [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
  130. public static extern int TT_TrackableCount();
  131. [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
  132. public static extern int TT_TrackableLocation([In]int index, [Out]out float x, [Out]out float y, [Out]out float z, [Out]out float qx, [Out]out float qy, [Out]out float qz, [Out]out float qw, [Out]out float yaw, [Out]out float pitch, [Out]out float roll);
  133. [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
  134. public static extern void TT_TrackableMarker([In]int rigidIndex, [In]int markerIndex, [Out]out float x, [Out]out float y, [Out]out float z);
  135. [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
  136. public static extern float TT_FrameMarkerX([In]int index);
  137. [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
  138. public static extern float TT_FrameMarkerY([In]int index);
  139. [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
  140. public static extern float TT_FrameMarkerZ([In]int index);
  141. [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
  142. public static extern bool TT_IsTrackableTracked([In]int index);
  143. [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
  144. public static extern int TT_TrackableID(int index);
  145. [DllImport("NPTrackingTools.dll", CallingConvention = CallingConvention.Cdecl)]
  146. public static extern int TT_TrackableMarkerCount(int index);
  147. }
  148. }