private static Matrix<float> ToMatrix(MCvPoint3D32f[][] data)
{
int elementCount = 0;
foreach (MCvPoint3D32f[] d in data) elementCount += d.Length;
Matrix<float> res = new Matrix<float>(elementCount, 3);
Int64 address = res.MCvMat.data.ToInt64();
foreach (MCvPoint3D32f[] d in data)
{
int lengthInBytes = d.Length * StructSize.MCvPoint3D32f;
GCHandle handle = GCHandle.Alloc(d, GCHandleType.Pinned);
Emgu.Util.Toolbox.memcpy(new IntPtr(address), handle.AddrOfPinnedObject(), lengthInBytes);
handle.Free();
address += lengthInBytes;
}
return res;
}