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; } } }
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} %"); }
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); }
public void ReceiveData(KObject data) { // 데이터 들어오면 카메라 시스템 정지 var pDocument = DeepSight.CDocument.GetDocument; //if( CDefine.enumTrigger.TRIGGER_OFF == pDocument.GetTrigger( ( int )CDefine.enumCamera.CAMERA_1 ) ) return; m_objCameraSystem.Stop(); CImageData3D objImageData3D = new CImageData3D(); // 일단 해상도를 임의로 지정하자 GoDataSet dataSet = ( GoDataSet )data; pDocument.SetUpdateLog(CDefine.enumLogType.LOG_PROCESS, "GOCATOR RECEIVE DATA : " + dataSet.Count.ToString()); 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: { #pragma warning disable CS0618 // 형식 또는 멤버는 사용되지 않습니다. GoSurfaceMsg surfaceMsg = ( GoSurfaceMsg )dataObj; #pragma warning restore CS0618 // 형식 또는 멤버는 사용되지 않습니다. objImageData3D.iOffsetZ = surfaceMsg.ZOffset; objImageData3D.iResolutionX = surfaceMsg.XResolution; // * DEF_3D_DATA_MULTIPLE; objImageData3D.iResolutionY = surfaceMsg.YResolution; objImageData3D.iResolutionZ = surfaceMsg.ZResolution; int lWidth = objImageData3D.iWidth = ( int )surfaceMsg.Width; int lHeight = objImageData3D.iHeight = ( int )surfaceMsg.Length; int bufferSize = lWidth * lHeight; IntPtr bufferPointer = surfaceMsg.Data; Console.WriteLine("Whole Part Height Map received:"); Console.WriteLine(" Buffer width: {0}", lWidth); Console.WriteLine(" Buffer Height: {0}", lHeight); objImageData3D.objHeightDataOrigin = new short[bufferSize]; Marshal.Copy(bufferPointer, objImageData3D.objHeightDataOrigin, 0, objImageData3D.objHeightDataOrigin.Length); // SetDataToCsv("d:\\test.csv", objImageData3D.objHeightDataOrigin); } break; case GoDataMessageType.SurfaceIntensity: { GoSurfaceIntensityMsg surfaceMsg = ( GoSurfaceIntensityMsg )dataObj; long width = surfaceMsg.Width; long length = surfaceMsg.Length; long bufferSize = width * length; IntPtr bufferPointeri = surfaceMsg.Data; Console.WriteLine("Whole Part Intensity Image received:"); Console.WriteLine(" Buffer width: {0}", width); Console.WriteLine(" Buffer length: {0}", length); objImageData3D.objIntensityDataOrigin = new byte[bufferSize]; Marshal.Copy(bufferPointeri, objImageData3D.objIntensityDataOrigin, 0, objImageData3D.objIntensityDataOrigin.Length); // objImageData3D.objBitmapIntensity = CopyDataToBitmap( ( int )length, ( int )width, objImageData3D.objIntensityDataOrigin ); // objImageData3D.objBitmapIntensity.Save( "d:\\Intensity.bmp" ); } break; default: break; } } if (null != m_objCallback) { CImageData objData = new CImageData(); objData.bGrabComplete = true; objData.bitmapImage = objImageData3D.objBitmapIntensity; objData.objCameraData3D = objImageData3D; m_objCallback(objData); } iGCCollectCount++; if (1 < iGCCollectCount) { GC.Collect(); iGCCollectCount = 0; } }