三维测量（3D测高）常用API总结，C#版本

三维测量（3D测高）常用API总结，C#版本

点到平面的距离：

输入输出参数：输入点的坐标；输出平面参数：平面表示形式为ax+by+cz=d，所以平面参数为1✖4矩阵知道一个点的坐标和平面参数，具体求解过程参考立体几何。

public static float Abs_DisPointToPlane(PointXYZ inuptPoint, float[,] Matrix_Plane){double num = Math.Sqrt((double)(Matrix_Plane[0, 0] * Matrix_Plane[0, 0] + Matrix_Plane[1, 0] * Matrix_Plane[1, 0] + Matrix_Plane[2, 0] * Matrix_Plane[2, 0]));if (num == 0.0){return float.NaN;}return (float)Math.Abs((double)(Matrix_Plane[0, 0] * inuptPoint.x + Matrix_Plane[1, 0] * inuptPoint.y + Matrix_Plane[2, 0] * inuptPoint.z + Matrix_Plane[3, 0]) / num);}

位图bitmap转halcon hobjectRGB：

输入输出参数：输入为C# Bitmap类型的图片；输出为Hobject

[DllImport("kernel32.dll", SetLastError = true)]private static extern void CopyMemory(long Destination, long Source, int Length);public static void BitmapGray_To_HObjectRgb(Bitmap bmp, out HObject ho){try{Rectangle rect = new Rectangle(0, 0, bmp.Width, bmp.Height);BitmapData bitmapData = bmp.LockBits(rect, ImageLockMode.ReadOnly, PixelFormat.Format8bppIndexed);IntPtr scan = bitmapData.Scan0;byte[] array = new byte[bmp.Width * bmp.Height];try{unsafe{fixed (byte* ptr = array){for (int i = 0; i < bmp.Height; i++){Process.CopyMemory((long)ptr + bmp.Width * i, (long)(scan + bitmapData.Stride * i), bmp.Width);}HOperatorSet.GenImage1(out ho, "byte", bmp.Width, bmp.Height, (long)ptr);new HImage("byte", bmp.Width, bmp.Height, (IntPtr)((void*)ptr));bmp.UnlockBits(bitmapData);}}}finally{unsafe{byte* ptr = null;}}}catch (Exception){ho = null;}}

第二种：

public static void BitmapRgb_To_HObjectRgb(Bitmap bmp, out HObject ho){try{Rectangle rect = new Rectangle(0, 0, bmp.Width, bmp.Height);BitmapData bitmapData = bmp.LockBits(rect, ImageLockMode.ReadOnly, PixelFormat.Format24bppRgb);HOperatorSet.GenImageInterleaved(out ho, bitmapData.Scan0, "bgr", bmp.Width, bmp.Height, 0, "byte", 0, 0, 0, 0, -1, 0);bmp.UnlockBits(bitmapData);}catch (Exception){ho = null;}}hobject转bitmap：public static void HObject_to_Bitmap(HObject image, out Bitmap res){res = null;try{if (image != null){HTuple hTuple;HOperatorSet.CountChannels(image, out hTuple);if (hTuple.I == 3){Process.HObjectRgb_To_BitmapRgb(image, out res);}else{Process.HObject_to_Bitmap8(image, out res);}}}catch{}}

hobject转16位bitmap：

public static void HObject_to_Bitmap16(HObject image, out Bitmap res){try{long[] array = new long[2];HTuple hTuple;HTuple hTuple2;HTuple hTuple3;HTuple hTuple4;HOperatorSet.GetImagePointer1(image, out hTuple, out hTuple2, out hTuple3, out hTuple4);res = new Bitmap(hTuple3, hTuple4, PixelFormat.Format16bppGrayScale);Rectangle rect = new Rectangle(0, 0, hTuple3, hTuple4);BitmapData bitmapData = res.LockBits(rect, ImageLockMode.WriteOnly, PixelFormat.Format16bppGrayScale);int t = Image.GetPixelFormatSize(bitmapData.PixelFormat) / 16;array[0] = bitmapData.Scan0.ToInt64();array[1] = hTuple.L;if (hTuple3 % 4 == 0){Process.CopyMemory(array[0], array[1], hTuple3 * hTuple4 * t);}else{int num = 0;while (num < hTuple4 - 1){long[] expr_C0_cp_0 = array;int expr_C0_cp_1 = 1;expr_C0_cp_0[expr_C0_cp_1] += hTuple3;Process.CopyMemory(array[0], array[1], hTuple3 * t);array[0] += (long)bitmapData.Stride;num++;}}res.UnlockBits(bitmapData);}catch (Exception){res = null;}}

hobject转8位bitmap：

private static void HObject_to_Bitmap8(HObject image, out Bitmap res){try{long[] array = new long[2];HTuple hTuple;HTuple hTuple2;HTuple hTuple3;HTuple hTuple4;HOperatorSet.GetImagePointer1(image, out hTuple, out hTuple2, out hTuple3, out hTuple4);res = new Bitmap(hTuple3, hTuple4, PixelFormat.Format8bppIndexed);ColorPalette palette = res.Palette;for (int i = 0; i <= 255; i++){palette.Entries[i] = Color.FromArgb(255, i, i, i);}res.Palette = palette;Rectangle rect = new Rectangle(0, 0, hTuple3, hTuple4);BitmapData bitmapData = res.LockBits(rect, ImageLockMode.WriteOnly, PixelFormat.Format8bppIndexed);int t = Image.GetPixelFormatSize(bitmapData.PixelFormat) / 8;array[0] = bitmapData.Scan0.ToInt64();array[1] = hTuple.L;if (hTuple3 % 4 == 0){Process.CopyMemory(array[0], array[1], hTuple3 * hTuple4 * t);}else{int num = 0;while (num < hTuple4 - 1){long[] expr_103_cp_0 = array;int expr_103_cp_1 = 1;expr_103_cp_0[expr_103_cp_1] += hTuple3;Process.CopyMemory(array[0], array[1], hTuple3 * t);array[0] += (long)bitmapData.Stride;num++;}}res.UnlockBits(bitmapData);}catch (Exception){res = null;}}

hobject RGB三通道转bitmap RGB三通道：

private unsafe static void HObjectRgb_To_BitmapRgb(HObject image, out Bitmap res){try{HTuple hTuple;HTuple hTuple2;HTuple hTuple3;HTuple hTuple4;HTuple hTuple5;HTuple hTuple6;HOperatorSet.GetImagePointer3(image, out hTuple, out hTuple2, out hTuple3, out hTuple4, out hTuple5, out hTuple6);res = new Bitmap(hTuple5, hTuple6, PixelFormat.Format32bppRgb);Rectangle rect = new Rectangle(0, 0, hTuple5, hTuple6);BitmapData bitmapData = res.LockBits(rect, ImageLockMode.ReadWrite, PixelFormat.Format32bppRgb);int num = hTuple5 * hTuple6;byte* ptr = (byte*)((void*)bitmapData.Scan0);byte* ptr2 = hTuple.L;byte* ptr3 = hTuple2.L;byte* ptr4 = hTuple3.L;for (int i = 0; i < num; i++){ptr[i * 4] = ptr4[i];ptr[i * 4 + 1] = ptr3[i];ptr[i * 4 + 2] = ptr2[i];ptr[i * 4 + 3] = 255;}res.UnlockBits(bitmapData);}catch (Exception arg_E9_0){res = null;throw arg_E9_0;}}

16位深度图转成点云结构体：

输入输出参数：输入为图片的指针，图像宽高，图像编码信息；输出为点云结构体

点云结构体：

public struct PointXYZ{public float x;public float y;public float z;}

16位深度图转点云

public static void DepthImage_To_PointCloud(IntPtr ImageData, int Width, int Height, float Zmin, float Zmax, double delta_X, double delta_Y, out PointXYZ[] PointCLoud){PointCLoud = null;HObject hObject;HOperatorSet.GenImage1(out hObject, "uint2", Width, Height, ImageData);if (hObject == null){return;}int num = 0;PointCLoud = new PointXYZ[Width * Height];double num2 = (double)(Zmax - Zmin) / 65536.0;for (int i = 0; i < Height; i++){for (int j = 0; j < Width; j++){HTuple hTuple;HOperatorSet.GetGrayval(hObject, i, j, out hTuple);PointCLoud[num].x = (float)((double)j * delta_X);PointCLoud[num].y = (float)((double)i * delta_Y);if (hTuple.D > 0.0 && hTuple.D < 65536.0){PointCLoud[num].z = (float)(hTuple.D * num2 + (double)Zmin);}else{PointCLoud[num].z = float.NaN;}num++;}}}

三维点云数据空间拟合平面：

输入输出参数：输入为XYZ点的集合；输出为过拟合平面的一点和经过该点的平面法向量，拟合的误差

private static bool fit_3d_plane_xyz(HTuple hv_X, HTuple hv_Y, HTuple hv_Z, out HTuple hv_Ox, out HTuple hv_Oy, out HTuple hv_Oz, out HTuple hv_Nx, out HTuple hv_Ny, out HTuple hv_Nz, out HTuple hv_MeanResidual){HTuple hTuple = null;HTuple hTuple2 = null;HTuple t = null;HTuple hTuple3 = null;HTuple hTuple4 = null;HTuple hTuple5 = null;HTuple hTuple6 = null;HTuple hTuple7 = null;HTuple hTuple8 = null;HTuple hTuple9 = null;HTuple hTuple10 = null;hv_Ox = new HTuple();hv_Oy = new HTuple();hv_Oz = new HTuple();hv_Nx = new HTuple();hv_Ny = new HTuple();hv_Nz = new HTuple();hv_MeanResidual = new HTuple();HTuple hTuple11 = new HTuple(hv_X.TupleLength());if (new HTuple(new HTuple(hTuple11.TupleLess(3)).TupleOr(new HTuple(hTuple11.TupleNotEqual(new HTuple(hv_Y.TupleLength()))))).TupleOr(new HTuple(hTuple11.TupleNotEqual(new HTuple(hv_Z.TupleLength())))) != 0){return false;}HOperatorSet.TupleMean(hv_X, out hv_Ox);HOperatorSet.TupleMean(hv_Y, out hv_Oy);HOperatorSet.TupleMean(hv_Z, out hv_Oz);HOperatorSet.CreateMatrix(3, new HTuple(hv_X.TupleLength()), (hv_X - hv_Ox).TupleConcat(hv_Y - hv_Oy).TupleConcat(hv_Z - hv_Oz), out hTuple);HOperatorSet.TransposeMatrix(hTuple, out hTuple2);HOperatorSet.SvdMatrix(hTuple2, "reduced", "right", out t, out hTuple3, out hTuple4);HOperatorSet.GetValueMatrix(hTuple3, new HTuple(0).TupleConcat(1).TupleConcat(2), new HTuple(0).TupleConcat(1).TupleConcat(2), out hTuple5);HOperatorSet.TupleSortIndex(hTuple5, out hTuple6);if (new HTuple(hTuple5.TupleSelect(hTuple6.TupleSelect(0)).TupleLess(1E-09)).TupleAnd(new HTuple(hTuple5.TupleSelect(hTuple6.TupleSelect(1)).TupleLess(1E-09))) != 0){return false;}HOperatorSet.GetValueMatrix(hTuple4, new HTuple(0).TupleConcat(1).TupleConcat(2), hTuple6.TupleSelect(0).TupleConcat(hTuple6.TupleSelect(0)).TupleConcat(hTuple6.TupleSelect(0)), out hTuple7);HOperatorSet.CreateMatrix(3, 1, hTuple7, out hTuple8);hv_Nx = hTuple7[0];hv_Ny = hTuple7[1];hv_Nz = hTuple7[2];HOperatorSet.MultMatrix(hTuple2, hTuple8, "AB", out hTuple9);HOperatorSet.GetFullMatrix(hTuple9, out hTuple10);hTuple10 = hTuple10.TupleAbs();hv_MeanResidual = hTuple10.TupleSum() / hTuple11;HOperatorSet.ClearMatrix(hTuple9.TupleConcat(hTuple8).TupleConcat(hTuple4).TupleConcat(hTuple3).TupleConcat(t).TupleConcat(hTuple2).TupleConcat(hTuple));return true;}

拟合平面：

输入输出参数：输入为XYZ三维点集集合，一个百分比数；输出为经过百分比筛选后的输出点集集合，输出的平面参数ax+by+cz+d=0，为abcd的浮点数组。

public static bool Halcon_FitPlane(List<PointXYZ> inputPoints, float FilPercent, out List<PointXYZ> PlanePoints, out float[,] Matrix_Plane){PlanePoints = new List<PointXYZ>();Matrix_Plane = new float[4, 1];PlanePoints = inputPoints;if (inputPoints.Count < 3){return false;}HTuple hTuple = new HTuple();HTuple hTuple2 = new HTuple();HTuple hTuple3 = new HTuple();foreach (PointXYZ current in inputPoints){hTuple.Append((double)current.x);hTuple2.Append((double)current.y);hTuple3.Append((double)current.z);}HTuple hTuple4;HTuple hTuple5;HTuple hTuple6;HTuple hTuple7;HTuple hTuple8;HTuple hTuple9;HTuple hTuple10;if (!Process.fit_3d_plane_xyz(hTuple, hTuple2, hTuple3, out hTuple4, out hTuple5, out hTuple6, out hTuple7, out hTuple8, out hTuple9, out hTuple10)){return false;}Matrix_Plane[0, 0] = (float)hTuple7.D;Matrix_Plane[1, 0] = (float)hTuple8.D;Matrix_Plane[2, 0] = (float)hTuple9.D;Matrix_Plane[3, 0] = -(float)((double)Matrix_Plane[0, 0] * hTuple4.D + (double)Matrix_Plane[1, 0] * hTuple5.D + (double)Matrix_Plane[2, 0] * hTuple6.D);return true;}

获取图像区域的宽高坐标，即像素坐标而不是像素值：

输入输出参数：输入为hobject的区域，输出为Point2DInt结构体链表

Point2DInt结构体：

public struct Point2DInt{public int x;public int y;}

public static void GetPoints2D_in_HObjectRegion(HObject Region, out List<Point2DInt> targPoints){targPoints = new List<Point2DInt>();if (Region == null){return;}HTuple hTuple = null;HTuple hTuple2 = null;HOperatorSet.GetRegionPoints(Region, out hTuple, out hTuple2);Point2DInt item = default(Point2DInt);int num = hTuple.TupleLength();int num2 = 1;if (num > 1000000){num2 = 100;}else if (num > 100000){num2 = 10;}else if (num > 10000){num2 = 2;}for (int i = 0; i < num; i += num2){item.x = hTuple2[i].I;item.y = hTuple[i].I;targPoints.Add(item);}}

平面矫正，即空间中所有的点求到一个平面的距离，然后重新输出这些点的坐标

输入输出参数：输入点云集合，图像的宽高，参考平面的平面参数，Z向高度的最大最小值；输出参数，灰度图像，矫正后的点云数据

public static bool Plane_Correct(PointXYZ[] PointCloud, int ImgWidth, int ImgHeight, float[,] Matrix_Plane, float Z_Max, float Z_Min, out HObject Image, out PointXYZ[] corect_PointCloud){Image = null;corect_PointCloud = null;float num = (float)Math.Sqrt((double)(Matrix_Plane[0, 0] * Matrix_Plane[0, 0] + Matrix_Plane[1, 0] * Matrix_Plane[1, 0] + Matrix_Plane[2, 0] * Matrix_Plane[2, 0]));if (PointCloud == null || num == 0f || PointCloud.Length != ImgWidth * ImgHeight){return false;}corect_PointCloud = new PointXYZ[PointCloud.Length];num = 1f / num;List<float> list = new List<float>();int num2 = 0;for (int i = 0; i < PointCloud.Length; i++){PointXYZ pointXYZ = PointCloud[i];corect_PointCloud[num2] = pointXYZ;if (float.IsNaN(pointXYZ.z)){list.Add(float.NaN);corect_PointCloud[num2].z = float.NaN;}else{float num3;if (Matrix_Plane[2, 0] >= 0f){num3 = (Matrix_Plane[0, 0] * pointXYZ.x + Matrix_Plane[1, 0] * pointXYZ.y + Matrix_Plane[2, 0] * pointXYZ.z + Matrix_Plane[3, 0]) * num;}else{num3 = -(Matrix_Plane[0, 0] * pointXYZ.x + Matrix_Plane[1, 0] * pointXYZ.y + Matrix_Plane[2, 0] * pointXYZ.z + Matrix_Plane[3, 0]) * num;}list.Add(num3);corect_PointCloud[num2].z = num3;}num2++;}float num4 = Z_Max - Z_Min;byte[] array = new byte[list.Count];if (num4 > 0f){float num5 = 1f / num4;for (int j = 0; j < list.Count; j++){if (Z_Min < list[j] && list[j] < Z_Max){array[j] = (byte)(255f * (list[j] - Z_Min) * num5);}else{array[j] = 0;}}}IntPtr intPtr = Marshal.AllocHGlobal(array.Length);Marshal.Copy(array, 0, intPtr, array.Length);HOperatorSet.GenImage1(out Image, "byte", ImgWidth, ImgHeight, intPtr);Marshal.FreeCoTaskMem(intPtr);return true;}

点云转real类型数据的tif格式图片

输入输出参数：输入自定义数据类，但其实只用到里面的点云集合；返回值为hobject的图像

public static HObject PointCloud_To_Tif(PointXYZ[] m_PointCloud){HObject result;try{HObject hObject = null;float[] array = new float[m_PointCloud.Length];for (int i = 0; i < m_PointCloud.Length; i++){array[i] = m_PointCloud[i].z;}PinnedObject pinnedObject = new PinnedObject(array);HOperatorSet.GenImage1(out hObject, "real", data.m_ImageWidth, data.m_ImageHeight, pinnedObject.Pointer);pinnedObject.Dispose();result = hObject;}catch{result = null;}return result;}

点云转16位深度图：

输入输出参数：输入自定义数据类，但其实只用到里面的点云集合；返回值为hobject的图像

public static HObject PointCloud_To_Unit2(PointXYZ[] m_PointCloud){HObject result;try{HObject hObject = null;byte[] array = new byte[m_PointCloud.Length * 2];for (int i = 0; i < m_PointCloud.Length; i++){if (float.IsNaN(m_PointCloud[i].z)){array[2 * i] = 0;array[2 * i + 1] = 0;}else{int num = (int)Convert.ToInt16((m_PointCloud[i].z - data.m_Zmin) * 1000f);if (num < 65536){byte[] array2 = new byte[]{0,(byte)(num >> 8 & 255)};array2[0] = (byte)(num & 255);array[2 * i] = array2[0];array[2 * i + 1] = array2[1];}else{array[2 * i] = 255;array[2 * i + 1] = 255;}}}PinnedObject pinnedObject = new PinnedObject(array);HOperatorSet.GenImage1(out hObject, "uint2", data.m_ImageWidth, data.m_ImageHeight, pinnedObject.Pointer);pinnedObject.Dispose();result = hObject;}catch{result = null;}return result;}

点云集合转灰度图像：

输入输出参数：输入点云数据集合，图像宽高，点云Z向最大最小值；返回值hobject格式图像

public static HObject PointXYZ_To_GrayHobject(ref PointXYZ[] PointCloudData, int ImgWidth, int ImgHeight, float ZMin, float ZMax){HObject result;try{HObject hObject = null;if (PointCloudData == null){result = hObject;}else{float num = ZMax - ZMin;byte[] array = new byte[PointCloudData.Length];if (num > 0f){float num2 = 1f / num;for (int i = 0; i < PointCloudData.Length; i++){if (ZMin < PointCloudData[i].z && PointCloudData[i].z < ZMax){array[i] = (byte)(255f * (PointCloudData[i].z - ZMin) * num2);}else{array[i] = 0;}}}IntPtr intPtr = Marshal.AllocHGlobal(array.Length);Marshal.Copy(array, 0, intPtr, array.Length);HOperatorSet.GenImage1(out hObject, "byte", ImgWidth, ImgHeight, intPtr);Marshal.FreeCoTaskMem(intPtr);result = hObject;}}catch{result = null;}return result;}