IntelCameraHandler.cs 5.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149
  1. using Emgu.CV;
  2. using Emgu.CV.Structure;
  3. using System;
  4. using System.Collections.Generic;
  5. using System.Linq;
  6. using System.Reflection;
  7. using System.Security.Permissions;
  8. using System.Text;
  9. using System.Threading;
  10. using System.Threading.Tasks;
  11. namespace BBIWARG.Input.InputProviding
  12. {
  13. class IntelCameraWrapper : MarshalByRefObject
  14. {
  15. private Assembly assembly;
  16. public InputProviderIntel inputProviderIntel;
  17. private dynamic senseManager;
  18. private dynamic session;
  19. private dynamic AccessRead;
  20. private dynamic PixelFormat;
  21. private dynamic EverythingFine;
  22. private MethodInfo acquireAccessMethod;
  23. private object[] methodParams;
  24. //public bool errorstate = false;
  25. public bool terminated = false;
  26. public IntelCameraWrapper()
  27. {
  28. }
  29. [SecurityPermissionAttribute(SecurityAction.Demand, Flags = SecurityPermissionFlag.Infrastructure)]
  30. public override object InitializeLifetimeService()
  31. {
  32. return null;
  33. }
  34. public void init(InputProviderIntel intelInputProvider) {
  35. inputProviderIntel = intelInputProvider;
  36. var domain = System.AppDomain.CreateDomain("IntelCamDomain");
  37. assembly = Assembly.Load("libpxcclr.cs");
  38. AccessRead = (dynamic)Enum.Parse(assembly.GetType("PXCMImage+Access"), "ACCESS_READ");
  39. PixelFormat = (dynamic)Enum.Parse(assembly.GetType("PXCMImage+PixelFormat"), "PIXEL_FORMAT_DEPTH");
  40. EverythingFine = (dynamic)Enum.Parse(assembly.GetType("pxcmStatus"), "PXCM_STATUS_NO_ERROR");
  41. // here be dragons
  42. Type imageDataType = assembly.GetType("PXCMImage+ImageData");
  43. Type refType = imageDataType.MakeByRefType();
  44. Type[] types = new Type[] { AccessRead.GetType(), PixelFormat.GetType(), refType };
  45. acquireAccessMethod = assembly.GetType("PXCMImage").GetMethod("AcquireAccess", types);
  46. //depthImage.AcquireAccess(AccessRead, PixelFormat, out imageData);
  47. methodParams = new object[] { AccessRead, PixelFormat, null };
  48. session = assembly.GetType("PXCMSession").GetMethod("CreateInstance").Invoke(null, null);
  49. senseManager = session.CreateSenseManager();
  50. dynamic ddesc = Activator.CreateInstance(assembly.GetType("PXCMVideoModule+DataDesc"));
  51. ddesc.deviceInfo.streams = (dynamic)Enum.Parse(assembly.GetType("PXCMCapture+StreamType"), "STREAM_TYPE_DEPTH");
  52. senseManager.EnableStreams(ddesc);
  53. var result = senseManager.Init();
  54. dynamic fov = senseManager.captureManager.device.QueryDepthFieldOfView();
  55. inputProviderIntel.FieldOfViewHorizontal = (float) (fov.x * Math.PI / 180.0f);
  56. inputProviderIntel.FieldOfViewVertical = (float) (fov.y * Math.PI / 180.0f);
  57. inputProviderIntel.lowConfidenceValue = senseManager.captureManager.device.QueryDepthLowConfidenceValue();
  58. senseManager.captureManager.device.SetDepthConfidenceThreshold((UInt16)Parameters.ConfidenceImageMinThreshold);
  59. Console.WriteLine("Started camera. Delivering frames...");
  60. }
  61. internal void run()
  62. {
  63. while (inputProviderIntel.IsActive)
  64. {
  65. if (terminated)
  66. return;
  67. //errorstate = false;
  68. var status = senseManager.AcquireFrame(true);
  69. if (status != EverythingFine)
  70. {
  71. Console.WriteLine("Found crash inside CameraHandler...");
  72. //errorstate = true;
  73. return;
  74. }
  75. if (terminated)
  76. return;
  77. if (inputProviderIntel.hasNewFrameEvent())
  78. {
  79. dynamic sample = senseManager.QuerySample();
  80. dynamic depthImage = sample.depth;
  81. dynamic info = depthImage.info;
  82. acquireAccessMethod.Invoke(depthImage, methodParams);
  83. dynamic imageData = methodParams[2];
  84. ushort[] data = imageData.ToUShortArray(0, info.width * info.height);
  85. Image<Gray, UInt16> dImg;
  86. unsafe
  87. {
  88. fixed (ushort* d = data)
  89. {
  90. IntPtr ptr = (IntPtr)d;
  91. dImg = new Image<Gray, UInt16>(info.width, info.height, info.width * sizeof(ushort), ptr).Copy();
  92. }
  93. }
  94. depthImage.ReleaseAccess(imageData);
  95. depthImage.Dispose();
  96. // confidence filter
  97. Image<Gray, byte> mask = dImg.InRange(new Gray(inputProviderIntel.lowConfidenceValue), new Gray(inputProviderIntel.lowConfidenceValue));
  98. dImg.SetValue(new Gray(Int16.MaxValue), mask);
  99. inputProviderIntel.newFrame(inputProviderIntel.CurrentFrameID, dImg);
  100. }
  101. inputProviderIntel.CurrentFrameID += 1;
  102. senseManager.ReleaseFrame();
  103. }
  104. senseManager.Dispose();
  105. }
  106. private void closeCamera()
  107. {
  108. senseManager.Close();
  109. session.Dispose();
  110. }
  111. }
  112. }