Пример #1
0
        private void RawDataResolver(KObject data)
        {
            GoDataSet dataSet = (GoDataSet)data;

            for (UInt32 i = 0; i < dataSet.Count; i++)
            {
                GoDataMsg dataObj = (GoDataMsg)dataSet.Get(i);
                switch (dataObj.MessageType)
                {
                case GoDataMessageType.UniformSurface:
                {
                    GoUniformSurfaceMsg surfaceMsg = (GoUniformSurfaceMsg)dataObj;
                    double  xResolution            = (double)surfaceMsg.XResolution / 1000000;
                    double  zResolution            = (double)surfaceMsg.ZResolution / 1000000;
                    double  xOffset       = (double)surfaceMsg.XOffset / 1000;
                    double  zOffset       = (double)surfaceMsg.ZOffset / 1000;
                    long    width         = surfaceMsg.Width;
                    long    height        = surfaceMsg.Length;
                    long    bufferSize    = width * height;
                    IntPtr  bufferPointer = surfaceMsg.Data;
                    short[] ranges        = new short[bufferSize];
                    Marshal.Copy(bufferPointer, ranges, 0, ranges.Length);
                }
                break;

                case GoDataMessageType.SurfaceIntensity:
                {
                    GoSurfaceIntensityMsg surfaceMsg = (GoSurfaceIntensityMsg)dataObj;
                    long   width          = surfaceMsg.Width;
                    long   height         = surfaceMsg.Length;
                    long   bufferSize     = width * height;
                    IntPtr bufferPointeri = surfaceMsg.Data;
                    byte[] ranges         = new byte[bufferSize];
                    Marshal.Copy(bufferPointeri, ranges, 0, ranges.Length);
                }
                break;
                }
            }
        }
Пример #2
0
        /// <summary>
        /// 监听回调函数
        /// </summary>
        /// <param name="data"></param>
        public void onData(KObject data)
        {
            if (File.Exists(strFileSave))
            {
                File.Delete(strFileSave);
            }
            bool length_incr = true;

            lst_ImagSur.Clear();
            GoDataSet   dataSet = (GoDataSet)data;
            DataContext context = new DataContext();

            for (UInt32 i = 0; i < dataSet.Count; ++i)
            {
                GoDataMsg dataObj = (GoDataMsg)dataSet.Get(i);
                switch (dataObj.MessageType)
                {
                case GoDataMessageType.Stamp:
                {
                    Console.WriteLine("GoDataMessageType.Stamp");
                    //GoStampMsg stampMsg = (GoStampMsg)dataObj;
                    //for (UInt32 j = 0; j < stampMsg.Count; j++)
                    //{
                    //    GoStamp stamp = stampMsg.Get(j);
                    //    Console.WriteLine("Frame Index = {0}", stamp.FrameIndex);
                    //    Console.WriteLine("Time Stamp = {0}", stamp.Timestamp);
                    //    Console.WriteLine("Encoder Value = {0}", stamp.Encoder);
                    //}
                }
                break;

                case GoDataMessageType.UniformSurface:    //case GoDataMessageType.Surface:
                {
                    Console.WriteLine("GoDataMessageType.UniformSurface");
                    GoUniformSurfaceMsg surfaceMsg = (GoUniformSurfaceMsg)dataObj;
                    long   width         = surfaceMsg.Width;
                    long   height        = surfaceMsg.Length;
                    long   bufferSize    = width * height;
                    IntPtr bufferPointer = surfaceMsg.Data;

                    short[] ranges = new short[bufferSize];
                    Marshal.Copy(bufferPointer, ranges, 0, ranges.Length);

                    context.xResolution = (double)surfaceMsg.XResolution / 1000000.0;
                    context.zResolution = (double)surfaceMsg.ZResolution / 1000000.0;
                    context.yResolution = (double)surfaceMsg.YResolution / 1000000.0;
                    context.yOffset     = (double)surfaceMsg.YOffset / 1000.0;
                    context.xOffset     = (double)surfaceMsg.XOffset / 1000.0;
                    context.zOffset     = (double)surfaceMsg.ZOffset / 1000.0;

                    double phy_x;
                    double phy_y;
                    double phy_z;

                    FileStream   fs = new FileStream(strFileSave, FileMode.Create);
                    StreamWriter sw = new StreamWriter(fs);

                    for (int m = 0; m < height; m++)
                    {
                        for (int j = 0; j < width; j++)
                        {
                            phy_z = ranges[m * width + j] * context.zResolution + context.zOffset;
                            if (true)             //   /*phy_z > 20*/   这个过滤阈值根据实际情况选取
                            {
                                phy_x = j * context.xResolution + context.xOffset;
                                phy_y = m * context.yResolution + context.yOffset;

                                string strWrite = string.Format("{0} {1} {2}", phy_x, phy_y, phy_z);
                                sw.WriteLine(strWrite);
                                lst_ImagSur.Add(new Vector4((float)phy_x, (float)phy_y, (float)phy_z, 1.0f));
                            }
                        }
                    }
                    sw.Flush();
                    //关闭流
                    sw.Close();
                    fs.Close();
                }
                break;

                case GoDataMessageType.Measurement:
                {
                    Console.WriteLine("GoDataMessageType.Measurement");
                    //GoMeasurementMsg measurementMsg = (GoMeasurementMsg)dataObj;
                    //for (UInt32 k = 0; k < measurementMsg.Count; ++k)
                    //{
                    //    GoMeasurementData measurementData = measurementMsg.Get(k);
                    //    mPoint.X = measurementData.Value;
                    //    mPoint.Y = measurementData.Value;
                    //    mPoint.Z = measurementData.Value;
                    //}
                }
                break;

                case GoDataMessageType.ProfilePointCloud:    //case GoDataMessageType.Profile:
                {
                    Console.WriteLine("GoDataMessageType.Profile");
                    //StreamWriter write = new StreamWriter(strFileSave, true);
                    //GoProfilePointCloudMsg profileMsg = (GoProfilePointCloudMsg)dataObj;
                    //Console.WriteLine("  Profile Message batch count: {0}", profileMsg.Count);
                    //for (UInt32 k = 0; k < profileMsg.Count; ++k)
                    //{
                    //    //int validPointCount = 0;
                    //    long profilePointCount = profileMsg.Width;
                    //    Console.WriteLine("  Item[{0}]: Profile data ({1} points)", i, profileMsg.Width);
                    //    context.xResolution = (profileMsg.XResolution / 1000000.0);
                    //    context.zResolution = profileMsg.ZResolution / 1000000.0;
                    //    context.xOffset = profileMsg.XOffset / 1000.0;
                    //    context.zOffset = profileMsg.ZOffset / 1000.0;
                    //    GoPoints[] points = new GoPoints[profilePointCount];
                    //    //point[] point111 = new point[profilePointCount];
                    //    ProfilePoint[] profileBuffer = new ProfilePoint[profilePointCount];
                    //    int structSize = Marshal.SizeOf(typeof(GoPoints));
                    //    IntPtr pointsPtr = profileMsg.Data;
                    //    for (UInt32 array = 0; array < profilePointCount; ++array)
                    //    {
                    //        IntPtr incPtr = new IntPtr(pointsPtr.ToInt64() + array * structSize);
                    //        points[array] = (GoPoints)Marshal.PtrToStructure(incPtr, typeof(GoPoints));

                    //        double real_x = (context.xOffset + context.xResolution * points[array].x);
                    //        double real_z = (context.zOffset + context.zResolution * points[array].y);

                    //        if (length_incr == true)
                    //        {
                    //            length += 1;//ReadIniSettings.ReadIni.objIniValue.iniScanner.step;
                    //            length_incr = false;
                    //        }

                    //        if (/*(real_z > 15 && real_x > -500.0 && real_z < 400*/true)
                    //        //if (real_z > 5.0  && real_z < 25 && length >100 && length < 2000)
                    //        {
                    //            write.WriteLine(real_x + " " + length + " " + real_z);
                    //        }
                    //    }
                    //    write.Flush();
                    //}
                    //write.Close();
                }
                break;

                case GoDataMessageType.ProfileIntensity:
                {
                    Console.WriteLine("GoDataMessageType.ProfileIntensity");
                    //GoProfileIntensityMsg profileMsg = (GoProfileIntensityMsg)dataObj;
                    //Console.WriteLine("  Profile Intensity Message batch count: {0}", profileMsg.Count);
                    //for (UInt32 k = 0; k < profileMsg.Count; ++k)
                    //{
                    //    byte[] intensity = new byte[profileMsg.Width];
                    //    IntPtr intensityPtr = profileMsg.Data;
                    //    Marshal.Copy(intensityPtr, intensity, 0, intensity.Length);
                    //}
                }
                break;

                case GoDataMessageType.Alignment:
                    Console.WriteLine("GoDataMessageType.Alignment");
                    break;

                case GoDataMessageType.BoundingBoxMatch:
                    Console.WriteLine("GoDataMessageType.BoundingBoxMatch");
                    break;

                case GoDataMessageType.EdgeMatch:
                    Console.WriteLine("GoDataMessageType.EdgeMatch");
                    break;

                case GoDataMessageType.EllipseMatch:
                    Console.WriteLine("GoDataMessageType.EllipseMatch");
                    break;

                case GoDataMessageType.Event:
                    Console.WriteLine("GoDataMessageType.Event");
                    break;

                case GoDataMessageType.ExposureCal:
                    Console.WriteLine("GoDataMessageType.ExposureCal");
                    break;

                case GoDataMessageType.Generic:
                    Console.WriteLine("GoDataMessageType.Generic");
                    break;

                case GoDataMessageType.Health:
                    Console.WriteLine("GoDataMessageType.Health");
                    break;

                case GoDataMessageType.Range:
                    Console.WriteLine("GoDataMessageType.Range");
                    break;

                case GoDataMessageType.RangeIntensity:
                    Console.WriteLine("GoDataMessageType.RangeIntensity");
                    break;

                case GoDataMessageType.ResampledProfile:    //case GoDataMessageType.UniformProfile:
                    Console.WriteLine("GoDataMessageType.ResampledProfile");
                    break;

                case GoDataMessageType.Section:
                    Console.WriteLine("GoDataMessageType.Section");
                    break;

                case GoDataMessageType.SectionIntensity:
                    Console.WriteLine("GoDataMessageType.SectionIntensity");
                    break;

                case GoDataMessageType.SurfaceIntensity:
                    Console.WriteLine("GoDataMessageType.SurfaceIntensity");
                    break;

                case GoDataMessageType.SurfacePointCloud:
                    Console.WriteLine("GoDataMessageType.SurfacePointCloud");
                    break;

                case GoDataMessageType.Tracheid:
                    Console.WriteLine("GoDataMessageType.Tracheid");
                    break;

                case GoDataMessageType.Unknown:
                    Console.WriteLine("GoDataMessageType.Unknown");
                    break;

                case GoDataMessageType.Video:
                    Console.WriteLine("GoDataMessageType.Video");
                    break;
                }
            }
            dataSet.Dispose();
            if (ActGetBit != null && lst_ImagSur.Count > 0)
            {
                int      imgWidth, imgHeight;
                int      rowNum = lst_ImagSur.Count;
                double[] xVal   = new double[rowNum];
                double[] yVal   = new double[rowNum];
                ushort[] zVal   = new ushort[rowNum];

                for (int m = 0; m < rowNum; m++)
                {
                    xVal[m] = Math.Round(lst_ImagSur[m].x / pixelPitch);
                    yVal[m] = Math.Round(lst_ImagSur[m].y / pixelPitch);
                    zVal[m] = (ushort)lst_ImagSur[m].z;
                }
                ArrayList list_x = new ArrayList(xVal);
                list_x.Sort();
                if (list_x.Count == 0)
                {
                    double minVal_x = 0;        // Convert.ToInt32(list[0]);
                    double maxVal_x = 0;
                    ActGetBit(null);
                }
                else
                {
                    double minVal_x = Convert.ToDouble(list_x[0]); // Convert.ToInt32(list[0]);
                    double maxVal_x = Convert.ToDouble(list_x[list_x.Count - 1]);

                    ArrayList list_y = new ArrayList(yVal);
                    list_y.Sort();
                    double minVal_y = Convert.ToDouble(list_y[0]); // Convert.ToInt32(list[0]);
                    double maxVal_y = Convert.ToDouble(list_y[list_y.Count - 1]);

                    ArrayList list_z = new ArrayList(zVal);
                    list_z.Sort();
                    double minVal_z = Convert.ToDouble(list_z[0]); // Convert.ToInt32(list[0]);
                    double maxVal_z = Convert.ToDouble(list_z[list_z.Count - 1]);

                    imgHeight = Convert.ToInt32(Math.Round(maxVal_x - minVal_x)) + 1;
                    imgWidth  = Convert.ToInt32(Math.Round(maxVal_y - minVal_y)) + 1;

                    double scale = (double)imgHeight / imgWidth;
                    double[,] imgTemp = new double[imgHeight, imgWidth];

                    for (int m = 0; m < rowNum; m++)
                    {
                        xVal[m] = xVal[m] - minVal_x;
                        yVal[m] = yVal[m] - minVal_y;
                    }
                    for (int n = 0; n < rowNum; n++)
                    {
                        imgTemp[Convert.ToInt32(xVal[n]), Convert.ToInt32(yVal[n])] = zVal[n];
                    }

                    // 将imgTemp转成一维图像
                    ushort[] ZValues = new ushort[imgHeight * imgWidth];
                    for (int i = 0; i < ZValues.Length; i++)
                    {
                        ZValues[i] = (ushort)imgTemp[i / imgWidth, i % imgWidth];
                    }

                    ColorMaps MapColor = new ColorMaps();
                    ushort    minValue = 0, maxValue = 0;
                    MapColor.FindMinMaxForColor(ZValues, (UInt32)(imgHeight * imgWidth), 0, ref minValue, ref maxValue);
                    Color[] colors = new Color[imgHeight * imgWidth];
                    MapColor.ToColors(ZValues, minValue, maxValue, 0, ref colors, (UInt32)(imgHeight * imgWidth));

                    Bitmap     Bimage = new Bitmap(imgWidth, imgHeight);
                    BitmapData bmdata = Bimage.LockBits(new Rectangle(0, 0, imgWidth, imgHeight), ImageLockMode.ReadWrite, PixelFormat.Format24bppRgb);
                    //IntPtr intptr = bmdata.Scan0;
                    byte[] Pixlemaps = new byte[bmdata.Stride * bmdata.Height];
                    // int offset = bmdata.Stride - bmdata.Width * 3;

                    unsafe
                    {
                        byte *pp = (byte *)(void *)bmdata.Scan0;

                        for (int k = 0; k < bmdata.Height; k++)
                        {
                            for (int m = 0; m < bmdata.Width; m++)
                            {
                                pp[0] = (byte)(colors[k * bmdata.Width + m].R);
                                pp[1] = (byte)(colors[k * bmdata.Width + m].G);
                                pp[2] = (byte)(colors[k * bmdata.Width + m].B);
                                pp   += 3;
                            }
                            pp += bmdata.Stride - bmdata.Width * 3;
                        }
                    }
                    Bimage.UnlockBits(bmdata);
                    Bitmap aBM2 = Bimage.Clone(new RectangleF(0, 0, Bimage.Width, Bimage.Height), PixelFormat.Format24bppRgb);
                    ActGetBit(aBM2);
                    //pbxImage.Width = 150;
                    //pbxImage.Height = Convert.ToInt32(pbxImage.Width * scale);
                    //Bitmap imgShow_scale = new Bitmap(aBM2, pbxImage.Width, pbxImage.Height);
                    //pbxImage.Image = imgShow_scale;
                }
            }
        }
Пример #3
0
        private void ResolveRawData(KObject kData)
        {
            GoDataSet dataSet = (GoDataSet)kData;

            ushort[]         zValue           = new ushort[0];
            TopBottomSurface topBottomSurface = new TopBottomSurface(SurfaceType.TopBottom);

            for (UInt32 i = 0; i < dataSet.Count; i++)
            {
                GoDataMsg dataObj = (GoDataMsg)dataSet.Get(i);

                switch (dataObj.MessageType)
                {
                    #region SurfaceMsg
                case GoDataMessageType.Surface:
                    GocatorContext      mContext   = new GocatorContext();
                    GoUniformSurfaceMsg surfaceMsg = (GoUniformSurfaceMsg)dataObj;
                    long width      = surfaceMsg.Width;
                    long height     = surfaceMsg.Length;
                    long bufferSize = width * height;
                    mContext.XResolution = (double)surfaceMsg.XResolution / 1000000;
                    mContext.ZResolution = (double)surfaceMsg.ZResolution / 1000000;
                    mContext.XOffset     = (double)surfaceMsg.XOffset / 1000;
                    mContext.ZOffset     = (double)surfaceMsg.ZOffset / 1000 - mContext.ZResolution * 32768;
                    mContext.YResolution = (double)surfaceMsg.YResolution / 1000000;
                    mContext.Width       = (int)width;
                    mContext.Height      = (int)height;
                    IntPtr  bufferPointer = surfaceMsg.Data;
                    short[] ranges        = new short[bufferSize];
                    zValue = new ushort[bufferSize];
                    Marshal.Copy(bufferPointer, ranges, 0, ranges.Length);
                    Parallel.For(0, bufferSize, (index) =>
                    {
                        zValue[index] = (ushort)(ranges[index] + 32768);
                    });
                    if (surfaceMsg.Source.Value == 0)
                    {
                        topBottomSurface.TopSurfaceData = zValue;
                        mContextTop = mContext;
                    }
                    else if (surfaceMsg.Source.Value == 1)
                    {
                        topBottomSurface.BottomSurfaceData = zValue;
                        mContextBottom = mContext;
                    }

                    break;
                    #endregion


                case GoDataMessageType.SurfaceIntensity:
                    GoSurfaceIntensityMsg goSurfaceIntensityMsg = (GoSurfaceIntensityMsg)dataObj;
                    long   widthIntensity      = goSurfaceIntensityMsg.Width;
                    long   heightIntensity     = goSurfaceIntensityMsg.Length;
                    long   bufferSizeIntensity = widthIntensity * heightIntensity;
                    IntPtr bufferPointeri      = goSurfaceIntensityMsg.Data;
                    byte[] rangesIntensity     = new byte[bufferSizeIntensity];
                    Marshal.Copy(bufferPointeri, rangesIntensity, 0, rangesIntensity.Length);
                    if (goSurfaceIntensityMsg.Source.Value == 0)
                    {
                        topBottomSurface.TopSurfaceIntensityData = rangesIntensity;
                    }
                    else if (goSurfaceIntensityMsg.Source.Value == 1)
                    {
                        topBottomSurface.BottomSurfaceIntensityData = rangesIntensity;
                    }
                    break;
                }
            }
            mResult.Add(topBottomSurface);
            DeviceStatusEvent?.Invoke(this, $"Finished {100 * (mResult.Count) / BufferSize * 1.0} %");
        }
Пример #4
0
        public static void takephoto(string name2)
        {
            try
            {
                KApiLib.Construct();
                GoSdkLib.Construct();
                GoSystem   system = new GoSystem();
                GoSensor   sensor;
                KIpAddress ipAddress = KIpAddress.Parse(Constants.SENSOR_IP);
                GoDataSet  dataSet   = new GoDataSet();
                sensor = system.FindSensorByIpAddress(ipAddress);
                sensor.Connect();
                GoSetup setup = sensor.Setup;
                setup.ScanMode = GoMode.Surface;
                system.EnableData(true);
                system.Start();
                Console.WriteLine("Waiting for Whole Part Data...");
                dataSet = system.ReceiveData(30000000);
                //DataContext context = new DataContext();
                for (UInt32 i = 0; i < dataSet.Count; i++)
                {
                    GoDataMsg dataObj = (GoDataMsg)dataSet.Get(i);
                    switch (dataObj.MessageType)
                    {
                    case GoDataMessageType.Stamp:
                    {
                        GoStampMsg stampMsg = (GoStampMsg)dataObj;
                        for (UInt32 j = 0; j < stampMsg.Count; j++)
                        {
                            GoStamp stamp = stampMsg.Get(j);
                            Console.WriteLine("Frame Index = {0}", stamp.FrameIndex);
                            Console.WriteLine("Time Stamp = {0}", stamp.Timestamp);
                            Console.WriteLine("Encoder Value = {0}", stamp.Encoder);
                        }
                    }
                    break;

                    case GoDataMessageType.UniformSurface:
                    {
                        GoUniformSurfaceMsg goSurfaceMsg = (GoUniformSurfaceMsg)dataObj;    // 定义变量gosurfacemsg,类型gosurfacemsg
                        long            length = goSurfaceMsg.Length;                       //surface长度
                        long            width = goSurfaceMsg.Width;                         //surface宽度
                        long            bufferSize = width * length;
                        double          XResolution = goSurfaceMsg.XResolution / 1000000.0; //surface 数据X方向分辨率为nm,转为mm
                        double          YResolution = goSurfaceMsg.YResolution / 1000000.0; //surface 数据Y方向分辨率为nm,转为mm
                        double          ZResolution = goSurfaceMsg.ZResolution / 1000000.0; //surface 数据Z方向分辨率为nm,转为mm
                        double          XOffset = goSurfaceMsg.XOffset / 1000.0;            //接收到surface数据X方向补偿单位um,转mm
                        double          YOffset = goSurfaceMsg.YOffset / 1000.0;            //接收到surface数据Y方向补偿单位um,转mm
                        double          ZOffset = goSurfaceMsg.ZOffset / 1000.0;            //接收到surface数据Z方向补偿单位um,转mm
                        IntPtr          bufferPointer = goSurfaceMsg.Data;
                        int             rowIdx, colIdx;
                        SurfacePoints[] surfacePointCloud = new SurfacePoints[bufferSize];
                        short[]         ranges            = new short[bufferSize];
                        Marshal.Copy(bufferPointer, ranges, 0, ranges.Length);
                        FileStream fs;
                        string     path = string.Format("D:\\Grasp\\Grasp\\EyeToHandData\\{0}.txt", name2);
                        //if (!File.Exists(path))
                        //{
                        fs = new FileStream(path, FileMode.Create, FileAccess.Write);

                        //else
                        //{
                        //    fs = new FileStream(path, FileMode.Append, FileAccess.Write);
                        //}
                        StreamWriter sr = new StreamWriter(fs);
                        for (rowIdx = 0; rowIdx < length; rowIdx++)                                                                                                                                   //row is in Y direction
                        {
                            for (colIdx = 0; colIdx < width; colIdx++)                                                                                                                                //col is in X direction
                            {
                                surfacePointCloud[rowIdx * width + colIdx].x = colIdx * XResolution + XOffset;                                                                                        //客户需要的点云数据X值
                                surfacePointCloud[rowIdx * width + colIdx].y = rowIdx * YResolution + YOffset;                                                                                        //客户需要的点云数据Y值
                                surfacePointCloud[rowIdx * width + colIdx].z = ranges[rowIdx * width + colIdx] * ZResolution + ZOffset;                                                               //客户需要的点云数据Z值
                                sr.WriteLine(surfacePointCloud[rowIdx * width + colIdx].x + "," + surfacePointCloud[rowIdx * width + colIdx].y + "," + surfacePointCloud[rowIdx * width + colIdx].z); //开始写入值
                            }
                        }
                        sr.Write("end");
                        //ushort[] ZValues = new ushort[ranges.Length];
                        //for (int k = 0; k < ranges.Length; k++)
                        //{
                        //    ZValues[k] = (ushort)(ranges[k] - short.MinValue);
                        //}

                        //for (UInt32 k = 0; k < bufferSize; k++)
                        //{
                        //    sr.WriteLine(surfacePointCloud[k].x.ToString() + "," + surfacePointCloud[k].y.ToString() + "," + surfacePointCloud[k].z.ToString());//开始写入值
                        //}
                        sr.Close();
                        fs.Close();
                    }
                    break;

                    case GoDataMessageType.SurfaceIntensity:
                    {
                        GoSurfaceIntensityMsg surfaceMsg = (GoSurfaceIntensityMsg)dataObj;
                        long   width          = surfaceMsg.Width;
                        long   height         = surfaceMsg.Length;
                        long   bufferSize     = width * height;
                        IntPtr bufferPointeri = surfaceMsg.Data;

                        Console.WriteLine("Whole Part Intensity Image received:");
                        Console.WriteLine(" Buffer width: {0}", width);
                        Console.WriteLine(" Buffer height: {0}", height);
                        byte[] ranges = new byte[bufferSize];
                        Marshal.Copy(bufferPointeri, ranges, 0, ranges.Length);
                    }
                    break;
                    }
                }
                system.Stop();
            }
            catch (KException ex)
            {
                Console.WriteLine("Error: {0}", ex.Status);
            }
            // wait for ESC key
            //Console.WriteLine("\nPress ENTER to continue");
            //do
            //{
            //    System.Threading.Thread.Sleep(100);
            //} while (Console.Read() != (int)ConsoleKey.Enter);
        }