private static Mat ConvertToFC1(MatOfPoint3f orig)
{
Point3f[] pts = new Point3f[orig.Cols * orig.Rows];
orig.CopyTo(pts, 0);
Mat m = new Mat(orig.Rows, 3, MatType.CV_32FC1);
for (int i = 0; i < orig.Rows; i++)
{
var pt = orig.ElementAt(i);
m.SetArray(0, 0, pt.X);
m.SetArray(0, 1, pt.Y);
m.SetArray(0, 2, pt.Z);
}
return(m);
}