点到平面的距离:
输入输出参数:输入点的坐标;输出平面参数:平面表示形式为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;
}