private void OnDataReceived(KObject data)
{
//system.Stop();
_dataSource = (GoDataSet)data;
for (UInt32 i = 0; i < _dataSource.Count; i++)
{
GoDataMsg dataObj = (GoDataMsg)_dataSource.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);
}
}
break;
case GoDataMessageType.Surface:
{
GoSurfaceMsg surface = (GoSurfaceMsg)dataObj;
long size = surface.Size;
}
break;
case GoDataMessageType.Profile:
{
}
break;
}
}
// Dispose required to prevent memory leak.
_dataSource.Dispose();
}
public void onData(KObject data)
{
bDataSaved = false;
double yPoint = 1;
string strWrite = "";
bool length_incr = true;
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:
{
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.Surface: // Surface
{
GoSurfaceMsg surfaceMsg = (GoSurfaceMsg)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 (/*phy_z > 20*/ true) //这个过滤阈值根据实际情况选取
{
phy_x = j * context.xResolution + context.xOffset;
phy_y = m * context.yResolution + context.yOffset;
strWrite = string.Format("{0} {1} {2}", phy_x, phy_y, phy_z);
sw.WriteLine(strWrite);
}
}
}
sw.Flush();
//关闭流
sw.Close();
fs.Close();
}
break;
case GoDataMessageType.Profile:
{
StreamWriter write = new StreamWriter(strFileSave, true);
GoProfileMsg profileMsg = (GoProfileMsg)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 > 60 && real_z < 800)
{
write.WriteLine(real_x + " " + length + " " + real_z);
}
}
write.Flush();
}
write.Close();
}
break;
case 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.Measurement: // Measurement
{
GoMeasurementMsg measurementMsg = (GoMeasurementMsg)dataObj;
}
break;
}
}
bDataSaved = true;
}
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;
}
}
/// <summary>
/// Get Point Cloud from Gocator
/// </summary>
/// <returns></returns>
private void Point3DGenerator(KObject data)
{
GoDataSet goDataSource = (GoDataSet)data;
PointCloud pc = new PointCloud();
long count = goDataSource.Count;
for (int i = 0; i < count; i++)
{
GoDataMsg dataMsg = (GoDataMsg)goDataSource.Get(i);
switch (dataMsg.MessageType)
{
case GoDataMessageType.Surface:
GoSurfaceMsg goSurfaceMsg = (GoSurfaceMsg)(dataMsg);
pc.Width = (int)goSurfaceMsg.Width;
pc.Height = (int)goSurfaceMsg.Length;
pc.XResolution = (float)goSurfaceMsg.XResolution / 1000000;
pc.YResolution = (float)goSurfaceMsg.YResolution / 1000000;
pc.ZResolution = (float)goSurfaceMsg.ZResolution / 1000000;
pc.XOffset = (float)goSurfaceMsg.XOffset / 1000;
pc.ZOffset = (float)goSurfaceMsg.ZOffset / 1000;
pc.YOffset = (float)goSurfaceMsg.YOffset / 1000;
GoSetup goSetup = _sensor.Setup;
GoRole goRole = new GoRole();
pc.ZStart = (float)goSetup.GetActiveAreaZ(goRole);
pc.XStart = (float)goSetup.GetActiveAreaX(goRole);
pc.ZRange = (float)goSetup.GetActiveAreaHeight(goRole);
///
//generate csv file for point data save
///
pc.ProfileList = new List <List <Point3D> >();
//int pointsCount = 0;
pc.Point3DArray = new Point3D[pc.Width * pc.Height];
int pointsCount = 0;
for (int j = 0; j < pc.Height; j++)
{
List <Point3D> profileList = new List <Point3D>();
for (int k = 0; k < pc.Width; k++)
{
Point3D tempPoint = new Point3D();
tempPoint.X = pc.XOffset + k * pc.XResolution;
tempPoint.Y = pc.YOffset + j * pc.YResolution;
//tempPoint.Z = (goSurfaceMsg.Get(j, k) == -32768) ? 0 : (pc.ZOffset + goSurfaceMsg.Get(j, k) * pc.ZResolution);
//No Z Start
//tempPoint.X = k * pc.XResolution;
//tempPoint.Y = j * pc.YResolution;
tempPoint.Z = (goSurfaceMsg.Get(j, k) == -32768) ? 0 : (pc.ZOffset + goSurfaceMsg.Get(j, k) * pc.ZResolution - pc.ZStart);
profileList.Add(tempPoint);
pc.Point3DArray[pointsCount] = tempPoint;
pointsCount++;
}
pc.ProfileList.Add(profileList);
}
//data.Destroy();
dataMsg.Destroy();
goSurfaceMsg.Destroy();
SerializePointCloud(pc);
break;
default:
break;
}
}
}
