Interop Kinect FaceTracking FT_SENSOR_DATA

MY problem is, I am migrating Kinect Face Tracking to C#, but my requirements are a bit specific as I'm porting this to Unity, which doesn't allow .NET Framework 4.0 (only 3.5) and does not allow [ComImport] tag. This being said, what I did, and it work for EVERY method beside when I use FT_SENSOR_DATA, is to manually marshal the interface using virtual function tables and delegate to function pointer calls to create the interfaces in C# side.

Everything works fine, I can initialize the library, can call everything, initiate all interfaces correctly, but when I call the startTracking method, it fails, crashing everything. I narrowed it being the FT_SENSOR_DATA structure as if I pass null to it, I get a E_POINTER error from C++.

The C++ Call to StartTracking and the C++ Structure for FT_SENSOR_DATA is:

STDMETHOD(StartTracking)(THIS_ const FT_SENSOR_DATA* pSensorData, const RECT* pRoi, const FT_VECTOR3D headPoints[2], IFTResult* pFTResult) PURE;

 struct FT_SENSOR_DATA
 {
 #ifdef __cplusplus
     FT_SENSOR_DATA(IFTImage* pVideoFrameParam = NULL, IFTImage* pDepthFrameParam = NULL, FLOAT zoomFactorParam = 1.0f, const POINT* pViewOffsetParam = NULL)
         : pVideoFrame(pVideoFrameParam), pDepthFrame(pDepthFrameParam), ZoomFactor(zoomFactorParam)
     {
         ViewOffset.x = pViewOffsetParam ? pViewOffsetParam->x : 0;
         ViewOffset.y = pViewOffsetParam ? pViewOffsetParam->y : 0;
     }
 #endif
 
     IFTImage* pVideoFrame;  // a pointer to a video frame. Must be synchronized with the depth frame passed in the same structure.
     IFTImage* pDepthFrame;  // a pointer to a depth frame. Must be synchronized with the video frame passed in the same structure.
     FLOAT     ZoomFactor;   // camera’s zoom factor for the video frame (value 1.0f means - no zoom is used)
     POINT     ViewOffset;   // the left, top coordinates of the video frame view area read from the camera’s native frame (which may have a higher resolution than what is returned to a PC). 
 };

IFTImage a the COM interface.

I Marshalled IFTFaceTracker (which contains the StartTracking method), the IFTImage and the FT_SENSOR_DATA as follows:

[StructLayout(LayoutKind.Sequential)] public class IFTFaceTracker { public struct IFTFaceTrackerVtbl { public IntPtr QueryInterface; public IntPtr AddRef; public IntPtr Release; public IntPtr Initialize; public IntPtr Reset; public IntPtr CreateFTResult; public IntPtr SetShapeUnits; public IntPtr GetShapeUnits; public IntPtr SetShapeComputationState; public IntPtr GetShapeComputationState; public IntPtr GetFaceModel; public IntPtr StartTracking; public IntPtr ContinueTracking; public IntPtr DetectFaces; }

     // Delegates Declaration
     private delegate int InitializeDelegate(IntPtr This, IntPtr pVideoCameraConfig, IntPtr pDepthCameraConfig, IntPtr depthToColorMappingFunc, string pszModelPath);
     private delegate int ResetDelegate(IntPtr This);
     private delegate int CreateFTResultDelegate(IntPtr This, ref IntPtr ppFTResult);
     private delegate int SetShapeUnitsDelegate(IntPtr This, float headScale, float[] pSUCoefs, uint suCount);
     private delegate int GetShapeUnitsDelegate(IntPtr This, ref float pHeadScale, ref float[] ppSUCoefs, ref uint pSUCount, ref bool pHaveConverged);
     private delegate int SetShapeComputationStateDelegate(IntPtr This, bool isEnabled);
     private delegate int GetShapeComputationStateDelegate(IntPtr This, ref bool pIsEnabled);
     private delegate int GetFaceModelDelegate(IntPtr This, ref IntPtr ppModel);
     private delegate int StartTrackingDelegate(IntPtr This, FT_SENSOR_DATA pSensorData, RECT pRoi, FT_VECTOR3D[] headPoints, ref IntPtr pFTResult);
     private delegate int ContinueTrackingDelegate(IntPtr This, FT_SENSOR_DATA pSensorData, FT_VECTOR3D[] headPoints, ref IntPtr pFTResult);
     private delegate int DetectFacesDelegate(IntPtr This, FT_SENSOR_DATA pSensorData, RECT pRoi, ref FT_WEIGHTED_RECT pFaces, ref uint pFaceCount);
     // Private variables
     private Kinect.IFTFaceTracker.IFTFaceTrackerVtbl vtbl;
     private IntPtr p;
     // Delegates Call
     private Kinect.IFTFaceTracker.InitializeDelegate initialize;
     private Kinect.IFTFaceTracker.ResetDelegate reset;
     private Kinect.IFTFaceTracker.CreateFTResultDelegate createFTResult;
     private Kinect.IFTFaceTracker.SetShapeUnitsDelegate setShapeUnits;
     private Kinect.IFTFaceTracker.GetShapeUnitsDelegate getShapeUnits;
     private Kinect.IFTFaceTracker.SetShapeComputationStateDelegate setShapeComputationState;
     private Kinect.IFTFaceTracker.GetShapeComputationStateDelegate getShapeComputationState;
     private Kinect.IFTFaceTracker.GetFaceModelDelegate getFaceModel;
     private Kinect.IFTFaceTracker.StartTrackingDelegate startTracking;
     private Kinect.IFTFaceTracker.ContinueTrackingDelegate continueTracking;
     private Kinect.IFTFaceTracker.DetectFacesDelegate detectFaces;

     // Constructor
     public IFTFaceTracker(IntPtr p)
     {
         this.p = p;
         IntPtr ptr = (IntPtr)Marshal.PtrToStructure(p, typeof(IntPtr));
         this.vtbl = (Kinect.IFTFaceTracker.IFTFaceTrackerVtbl)Marshal.PtrToStructure(ptr, typeof(Kinect.IFTFaceTracker.IFTFaceTrackerVtbl));
         this.initialize = (Kinect.IFTFaceTracker.InitializeDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.Initialize, typeof(Kinect.IFTFaceTracker.InitializeDelegate));
         this.reset = (Kinect.IFTFaceTracker.ResetDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.Reset, typeof(Kinect.IFTFaceTracker.ResetDelegate));
         this.createFTResult = (Kinect.IFTFaceTracker.CreateFTResultDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.CreateFTResult, typeof(Kinect.IFTFaceTracker.CreateFTResultDelegate));
         this.setShapeUnits = (Kinect.IFTFaceTracker.SetShapeUnitsDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.SetShapeUnits, typeof(Kinect.IFTFaceTracker.SetShapeUnitsDelegate));
         this.getShapeUnits = (Kinect.IFTFaceTracker.GetShapeUnitsDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.GetShapeUnits, typeof(Kinect.IFTFaceTracker.GetShapeUnitsDelegate));
         this.setShapeComputationState = (Kinect.IFTFaceTracker.SetShapeComputationStateDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.SetShapeComputationState, typeof(Kinect.IFTFaceTracker.SetShapeComputationStateDelegate));
         this.getShapeComputationState = (Kinect.IFTFaceTracker.GetShapeComputationStateDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.GetShapeComputationState, typeof(Kinect.IFTFaceTracker.GetShapeComputationStateDelegate));
         this.getFaceModel = (Kinect.IFTFaceTracker.GetFaceModelDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.GetFaceModel, typeof(Kinect.IFTFaceTracker.GetFaceModelDelegate));
         this.startTracking = (Kinect.IFTFaceTracker.StartTrackingDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.StartTracking, typeof(Kinect.IFTFaceTracker.StartTrackingDelegate));
         this.continueTracking = (Kinect.IFTFaceTracker.ContinueTrackingDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.ContinueTracking, typeof(Kinect.IFTFaceTracker.ContinueTrackingDelegate));
         this.detectFaces = (Kinect.IFTFaceTracker.DetectFacesDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.DetectFaces, typeof(Kinect.IFTFaceTracker.DetectFacesDelegate));
     }

     // Properties
     public IntPtr pointer
     {
         get
         {
             return this.p;
         }
     }

     // Method Call
     public int Initialize(FT_CAMERA_CONFIG pVideoCameraConfig, FT_CAMERA_CONFIG pDepthCameraConfig, Delegate depthToColorMappingFunc, string pszModelPath)
     {
         IntPtr videoptr = Marshal.AllocHGlobal(Marshal.SizeOf(pVideoCameraConfig));
         IntPtr depthptr = IntPtr.Zero;
         if (pDepthCameraConfig != null) depthptr = Marshal.AllocHGlobal(Marshal.SizeOf(pDepthCameraConfig));
         Marshal.StructureToPtr(pVideoCameraConfig, videoptr, false);
         if (pDepthCameraConfig != null) Marshal.StructureToPtr(pDepthCameraConfig, depthptr, false);
         if (depthToColorMappingFunc != null)
         {
             IntPtr delegatePointer = Marshal.GetFunctionPointerForDelegate(depthToColorMappingFunc);
             return this.initialize(this.p, videoptr, depthptr, delegatePointer, pszModelPath);
         }
         else
         {
             return this.initialize(this.p, videoptr, depthptr, IntPtr.Zero, pszModelPath);
         }
     }
     public int Reset()
     {
         return this.reset(this.p);
     }
     public int CreateFTResult(ref IFTResult ppFTResult)
     {
         IntPtr ftResultPointer = IntPtr.Zero;
         int result = this.createFTResult(this.p, ref ftResultPointer);
         ppFTResult = new IFTResult(ftResultPointer);
         return result;
     }
     public int SetShapeUnits(float headScale, float[] pSUCoefs, uint suCount)
     {
         return this.setShapeUnits(this.p, headScale, pSUCoefs, suCount);
     }
     public int GetShapeUnits(ref float pHeadScale, ref float[] ppSUCoefs, ref uint pSUCount, ref bool pHaveConverged)
     {
         return this.getShapeUnits(this.p, ref pHeadScale, ref ppSUCoefs, ref pSUCount, ref pHaveConverged);
     }
     public int SetShapeComputationState(bool isEnabled)
     {
         return this.setShapeComputationState(this.p, isEnabled);
     }
     public int GetShapeComputationState(ref bool pIsEnabled)
     {
         return this.getShapeComputationState(this.p, ref pIsEnabled);
     }
     public int GetFaceModel(ref IFTModel ppModel)
     {
         IntPtr ftModelPointer = IntPtr.Zero;
         int result = this.getFaceModel(this.p, ref ftModelPointer);
         ppModel = new IFTModel(ftModelPointer);
         return result;
     }
     public int StartTracking(ref FT_SENSOR_DATA pSensorData, RECT pRoi, FT_VECTOR3D[] headPoints, ref IFTResult pFTResult)
     {
         //IntPtr ftResultPointer = pFTResult.pointer;
         IntPtr psensorptr = IntPtr.Zero;
         //IntPtr dataptr = Marshal.AllocHGlobal(Marshal.SizeOf(pSensorData));
         //Marshal.StructureToPtr(pSensorData, dataptr, false);
         int result = this.startTracking(this.p, pSensorData, null, null, ref psensorptr);
         Debug.Log(result);
         //Debug.Log(psensorptr);
         //pFTResult = new IFTResult(psensorptr);
         return result;
     }
     public int ContinueTracking(FT_SENSOR_DATA pSensorData, FT_VECTOR3D[] headPoints, ref IFTResult pFTResult)
     {
         IntPtr ftResultPointer = pFTResult.pointer;
         int result = this.continueTracking(this.p, pSensorData, headPoints, ref ftResultPointer);
         pFTResult = new IFTResult(ftResultPointer);
         Debug.Log(result);
         return result;
         
     }
     public int DetectFaces(FT_SENSOR_DATA pSensorData, RECT pRoi, ref FT_WEIGHTED_RECT pFaces, ref uint pFaceCount)
     {
         return this.detectFaces(this.p, pSensorData, pRoi, ref pFaces, ref pFaceCount);
     }
 }

 [StructLayout(LayoutKind.Sequential)]
 public class IFTImage
 {
     public struct IFTImageVtbl
     {
         public IntPtr QueryInterface;
         public IntPtr AddRef;
         public IntPtr Release;
         public IntPtr Allocate;
         public IntPtr Attach;
         public IntPtr Reset;
         public IntPtr GetWidth;
         public IntPtr GetHeight;
         public IntPtr GetStride;
         public IntPtr GetBytesPerPixel;
         public IntPtr GetBufferSize;
         public IntPtr GetFormat;
         public IntPtr GetBuffer;
         public IntPtr IsAttached;
         public IntPtr CopyTo;
         public IntPtr DrawLine;
     }

     // Delegates Declaration
     private delegate int AllocateDelegate(IntPtr This, uint width, uint height, FTIMAGEFORMAT format);
     private delegate int AttachDelegate(IntPtr This, uint width, uint height, IntPtr pData, FTIMAGEFORMAT format, uint stride); // Intptr == void*
     private delegate int ResetDelegate(IntPtr This);
     private delegate uint GetWidthDelegate(IntPtr This);
     private delegate uint GetHeightDelegate(IntPtr This);
     private delegate uint GetStrideDelegate(IntPtr This);
     private delegate uint GetBytesPerPixelDelegate(IntPtr This);
     private delegate uint GetBufferSizeDelegate(IntPtr This);
     private delegate FTIMAGEFORMAT GetFormatDelegate(IntPtr This);
     private delegate byte[] GetBufferDelegate(IntPtr This);
     private delegate bool IsAttachedDelegate(IntPtr This);
     private delegate int CopyToDelegate(IntPtr This, IFTImage pDestImage, RECT pSrcRect, uint destRow, uint destColumn);
     private delegate int DrawLineDelegate(IntPtr This, POINT startPoint, POINT endPoint, uint color, uint lineWidthPx);
     // Delegates Call
     private Kinect.IFTImage.AllocateDelegate allocate;
     private Kinect.IFTImage.AttachDelegate attach;
     private Kinect.IFTImage.ResetDelegate reset;
     private Kinect.IFTImage.GetWidthDelegate getWidth;
     private Kinect.IFTImage.GetHeightDelegate getHeight;
     private Kinect.IFTImage.GetStrideDelegate getStride;
     private Kinect.IFTImage.GetBytesPerPixelDelegate getBytesPerPixel;
     private Kinect.IFTImage.GetBufferSizeDelegate getBufferSize;
     private Kinect.IFTImage.GetFormatDelegate getFormat;
     private Kinect.IFTImage.GetBufferDelegate getBuffer;
     private Kinect.IFTImage.IsAttachedDelegate isAttached;
     private Kinect.IFTImage.CopyToDelegate copyTo;
     private Kinect.IFTImage.DrawLineDelegate drawLine;

     // Private variables
     private Kinect.IFTImage.IFTImageVtbl vtbl;
     private IntPtr p;
     private IntPtr vtblp;

     // Properties Implementation
     #region Properties
     public uint GetWidth
     {
         get
         {
             return this.getWidth(this.p);
         }
     }
     public uint GetHeight
     {
         get
         {
             return this.getHeight(this.p);
         }
     }
     public uint GetStride
     {
         get
         {
             return this.getStride(this.p);
         }
     }
     public uint GetBytesPerPixel
     {
         get
         {
             return this.getBytesPerPixel(this.p);
         }
     }
     public uint GetBufferSize
     {
         get
         {
             return this.getBufferSize(this.p);
         }
     }
     public FTIMAGEFORMAT GetFormat
     {
         get
         {
             return this.getFormat(this.p);
         }
     }
     public byte[] GetBuffer
     {
         get
         {
             return this.getBuffer(this.p);
         }
     }
     public bool IsAttached
     {
         get
         {
             return this.isAttached(this.p);
         }
     }
     public IntPtr pointer
     {
         get
         {
             return this.p;
         }
     }
     public IntPtr VtblPointer
     {
         get
         {
             return this.vtblp;
         }
     }
     #endregion

     // Constructor
     public IFTImage(IntPtr p)
     {
         this.p = p;
         IntPtr ptr = (IntPtr)Marshal.PtrToStructure(p, typeof(IntPtr));
         vtblp = ptr;
         this.vtbl = (Kinect.IFTImage.IFTImageVtbl)Marshal.PtrToStructure(ptr, typeof(Kinect.IFTImage.IFTImageVtbl));
         this.allocate = (Kinect.IFTImage.AllocateDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.Allocate, typeof(Kinect.IFTImage.AllocateDelegate));
         this.attach = (Kinect.IFTImage.AttachDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.Attach, typeof(Kinect.IFTImage.AttachDelegate));
         this.reset = (Kinect.IFTImage.ResetDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.Reset, typeof(Kinect.IFTImage.ResetDelegate));
         this.getWidth = (Kinect.IFTImage.GetWidthDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.GetWidth, typeof(Kinect.IFTImage.GetWidthDelegate));
         this.getHeight = (Kinect.IFTImage.GetHeightDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.GetHeight, typeof(Kinect.IFTImage.GetHeightDelegate));
         this.getStride = (Kinect.IFTImage.GetStrideDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.GetStride, typeof(Kinect.IFTImage.GetStrideDelegate));
         this.getBytesPerPixel = (Kinect.IFTImage.GetBytesPerPixelDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.GetBytesPerPixel, typeof(Kinect.IFTImage.GetBytesPerPixelDelegate));
         this.getBufferSize = (Kinect.IFTImage.GetBufferSizeDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.GetBufferSize, typeof(Kinect.IFTImage.GetBufferSizeDelegate));
         this.getFormat = (Kinect.IFTImage.GetFormatDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.GetFormat, typeof(Kinect.IFTImage.GetFormatDelegate));
         this.getBuffer = (Kinect.IFTImage.GetBufferDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.GetBuffer, typeof(Kinect.IFTImage.GetBufferDelegate));
         this.isAttached = (Kinect.IFTImage.IsAttachedDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.IsAttached, typeof(Kinect.IFTImage.IsAttachedDelegate));
         this.copyTo = (Kinect.IFTImage.CopyToDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.CopyTo, typeof(Kinect.IFTImage.CopyToDelegate));
         this.drawLine = (Kinect.IFTImage.DrawLineDelegate)Marshal.GetDelegateForFunctionPointer(this.vtbl.DrawLine, typeof(Kinect.IFTImage.DrawLineDelegate));
     }
     // Method Call
     public int Allocate(uint width, uint height, FTIMAGEFORMAT format)
     {
         return this.allocate(this.p, width, height, format);
     }
     public int Attach(uint width, uint height, IntPtr pData, FTIMAGEFORMAT format, uint stride)
     {
         return this.attach(this.p, width, height, pData, format, stride);
     }
     public int Reset()
     {
         return this.reset(this.p);
     }
     public int CopyTo(IFTImage pDestImage, RECT pSrcRect, uint destRow, uint destColumn)
     {
         return this.copyTo(this.p, pDestImage, pSrcRect, destRow, destColumn);
     }
     public int DrawLine(POINT startPoint, POINT endPoint, uint color, uint lineWidthPx)
     {
         return this.drawLine(this.p, startPoint, endPoint, color, lineWidthPx);
     }
 }

[StructLayout(LayoutKind.Sequential)] public class FT_SENSOR_DATA { public IFTImage pVideoFrame; // a pointer to a video frame. Must be synchronized with the depth frame passed in the same structure. public IFTImage pDepthFrame; // a pointer to a depth frame. Must be synchronized with the video frame passed in the same structure. public float ZoomFactor; // camera’s zoom factor for the video frame (value 1.0f means - no zoom is used) public POINT ViewOffset; // the left, top coordinates of the video frame view area read from the camera’s native frame (which may have a higher resolution than what is returned to a PC). }

I tried changing the IFTImage and POINT in the FT_SENSOR_DATA structures to IntPtr and pass the pointer given to me when the interface is created, but to no avail.

Anyone has any idea on how can I marshal it so I can call startTracking successfully, without crashes?

Thanks a lot,

-Roger-