/// <summary>
/// Map points
/// </summary>
/// <param name="laserLocations"></param>
/// <param name="image"></param>
/// <returns></returns>
public ScanLine MapPoints(List <PointF> laserLocations, Bitmap image, bool useCorrection)
{
Point3DList points = Mapper.MapPoints(laserLocations, image, DefaultColor);
ScanLine ret = new ScanLine(Id, points.Count);
ret.DisplayAsLine = true;
if (useCorrection)
{
Matrix4d m = Correction.GetMatrix();
int count = points.Count;
for (int i = 0; i < count; i++)
{
Point3D p = points[i];
Vector3d v = Vector3d.Transform(p.Position, m);
Vector3d n = Vector3d.Transform(p.Normal, m);
n.Normalize();
ret.Add(new Point3D(v, n, p.Color));
}
}
else
{
ret.Add(points);
}
return(ret);
}
/// <summary>
/// Map points
/// </summary>
/// <param name="laserLocations"></param>
/// <param name="image"></param>
/// <returns></returns>
public ScanLine MapPoints(List<PointF> laserLocations, Bitmap image, bool useCorrection)
{
Point3DList points = Mapper.MapPoints(laserLocations, image, DefaultColor);
ScanLine ret = new ScanLine(Id, points.Count);
ret.DisplayAsLine = true;
if (useCorrection)
{
Matrix4d m = Correction.GetMatrix();
int count = points.Count;
for (int i = 0; i < count; i++)
{
Point3D p = points[i];
Vector3d v = Vector3d.Transform(p.Position, m);
Vector3d n = Vector3d.Transform(p.Normal, m);
n.Normalize();
ret.Add(new Point3D(v, n, p.Color));
}
}
else
{
ret.Add(points);
}
return ret;
}
/// <summary>
/// Render thes Slice WireFrame
/// </summary>
/// <param name="context"></param>
protected void RenderAsWireFrame(ref RenderingContext context)
{
PrimitiveType oldPrim = m_Primitive;
m_Primitive = PrimitiveType.LineStrip;
context.ApplyLineDefault();
base.Render(ref context);
ScanLine l1 = new ScanLine(-1, Count / 2);
ScanLine l2 = new ScanLine(-1, Count / 2);
for (int i = 0; i < Count; i++)
{
if (i % 2 == 0)
{
l1.Add(this[i]);
}
else
{
l2.Add(this[i]);
}
}
l1.Render(ref context);
l2.Render(ref context);
m_Primitive = oldPrim;
}
public override ScanData DoTask(ScanData source)
{
if (!HardwareAvailable)
{
throw new Exception(string.Format("HardWare missing : TURNTABLE:{0} LASER:{1} CAMERA:{2}", HardwarePresentTrace(TurnTable), HardwarePresentTrace(Laser), HardwarePresentTrace(Camera)));
}
RotationStep = (double)Math.Round(TurnTable.MinimumRotation() + (15f - TurnTable.MinimumRotation()) * ((100 - Precision) / 100f), 2);
Settings settings = Settings.Get <Settings>();
CameraLoc.X = settings.Read(Settings.CAMERA, Settings.X, 0f);
CameraLoc.Y = settings.Read(Settings.CAMERA, Settings.Y, 270f);
CameraLoc.Z = settings.Read(Settings.CAMERA, Settings.Z, 70f);
double thres = settings.Read(Settings.LASER_COMMON, Settings.MAGNITUDE_THRESHOLD, 10);
int min = settings.Read(Settings.LASER_COMMON, Settings.MIN_WIDTH, 1);
int max = settings.Read(Settings.LASER_COMMON, Settings.MAX_WIDTH, 60);
ICameraProxy camera = Settings.Get <ICameraProxy>();
ImageProcessor = new ImageProcessor(thres, min, max);
SizeF tableSize = new SizeF(
(float)settings.Read(Settings.TABLE, Settings.DIAMETER, 20f),
(float)settings.Read(Settings.TABLE, Settings.HEIGHT, 15f)
);
Lasers = new List <LaserInfo>(LaserId.Length);
for (int i = 0; i < LaserId.Length; i++)
{
Lasers.Add(new LaserInfo(LaserId[i], CameraLoc, tableSize));
}
ScanData ret = new ScanData();
UpdatePercent(0, ret);
int fadeTime = settings.Read(Settings.LASER_COMMON, Settings.FADE_DELAY, 100);
TurnTable.InitialiseRotation();
int laserCount = Lasers.Count;
// Scan all laser location,
for (double currentAngle = 0; currentAngle < 360f; currentAngle += RotationStep)
{
if (this.CancelPending)
{
return(ret);
}
Laser.TurnAll(false); // All laser off
Thread.Sleep(fadeTime); // wait fade laser
Bitmap imgoff = GetCapture();
for (int laserIndex = 0; laserIndex < laserCount; laserIndex++)
{
Laser.Turn(Lasers[laserIndex].Id, true);
Thread.Sleep(fadeTime); // wait fade laser
Bitmap imgon = GetCapture();
Laser.Turn(Lasers[laserIndex].Id, false);
List <PointF> laserloc = ImageProcessor.Process(imgoff, imgon, null);
Point3DList samplePoints = Lasers[laserIndex].MapPoints(laserloc, UseTexture ? imgoff : null, UseCorrectionMatrix);
PositionPostProcess(ref samplePoints, -Utils.DEGREES_TO_RADIANS(currentAngle));
ScanLine line = new ScanLine(laserIndex, samplePoints);
line.DisplayAsLine = true;
ret.Add(line);
}
int percent = (int)((currentAngle / 360f) * 100f);
UpdatePercent(percent, ret);
TurnTable.Rotate(currentAngle, false);
}
LineSort lineSort = new LineSort();
ret = lineSort.Run(ret, CallerControl, this.Worker, this.WorkerArg);
if (!string.IsNullOrEmpty(FileName))
{
string path = Path.Combine(Program.UserDataPath, FileName);
ScanDataIO.Write(path, ret);
}
return(ret);
}
/// <summary>
/// Ctor
/// </summary>
/// <param name="laserID"></param>
/// <param name="points"></param>
public ScanLine(ScanLine other)
: this(other.LaserID, (Point3DList)other)
{
DisplayAsLine = other.DisplayAsLine;
}
public override ScanLine DoTask(ScanLine source)
{
try
{
//Debug.WriteLine("\nBegin outlier data detection using k-means clustering demo\n");
//Debug.WriteLine("Loading all (height-weight) data into memory");
string[] attributes = new string[] { "X", "Y", "Z" };
Vector3d[] rawData = new Vector3d[source.Count]; // in most cases data will be in a text file or SQl table
for (int i = 0; i < rawData.Length; i++)
rawData[i] = new Vector3d(source[i].Position.X, source[i].Position.Y, source[i].Position.Z );
//Debug.WriteLine("\nRaw data:\n");
//ShowMatrix(rawData, rawData.Length, true);
int numAttributes = attributes.Length; // 2 in this demo (height,weight)
int numClusters = NumClusters; // vary this to experiment (must be between 2 and number data tuples)
int maxCount = 30; // trial and error
//Debug.WriteLine("\nBegin clustering data with k = " + numClusters + " and maxCount = " + maxCount);
int[] clustering = Cluster(rawData, numClusters, numAttributes, maxCount);
//Debug.WriteLine("\nClustering complete");
//Debug.WriteLine("\nClustering in internal format: \n");
//ShowVector(clustering, true); // true -> newline after display
//Debug.WriteLine("\nClustered data:");
//ShowClustering(rawData, numClusters, clustering, true);
double maxDist = Distance(source.Max,source.Min) / (50);
List<int> outlier = Outlier(rawData, clustering, maxDist);
//Debug.WriteLine("Outlier for cluster 0 is:");
//ShowVector(outlier, true);
//Debug.WriteLine("\nEnd demo\n");
List<int> count = new List<int>(numClusters);
for (int i = 0; i < numClusters; i++)
count.Add(0);
for (int i = 0; i < clustering.Length; i++)
{
count[clustering[i]]++;
}
int pointCount = source.Count;
List<bool> clusterOk = new List<bool>(numClusters);
for (int i = 0; i < numClusters; i++)
{
double pct = (100f*count[i])/pointCount;
clusterOk.Add(pct >= this.RejectPercent);
}
ScanLine ret = new ScanLine(source.LaserID, source.Count);
for (int i = 0; i < source.Count; i++)
{
Point3D sp = source[i];
int clusterIndex = clustering[i];
Color col = sp.Color;
#if DEBUG
if (ColoriseOnly)
{
float clamp = (float)((1.0f * clusterIndex) / (numClusters - 1));
col = ColorExtension.ColorFromVector(new Vector4(clamp, clamp, clamp, clamp));
if (!clusterOk[clusterIndex])
col = Color.Green;
else if (outlier.Contains(i))
col = Color.Red;
ret.Add(new Point3D(sp.Position, sp.Normal, col));
}
else if (clusterOk[clusterIndex] && !outlier.Contains(i))
#endif
ret.Add(new Point3D(sp.Position, sp.Normal, col));
}
return ret;
}
catch (Exception ex)
{
Debug.WriteLine(ex.Message);
}
return source;
} // Main
/// <summary>
/// Render thes Slice WireFrame
/// </summary>
/// <param name="context"></param>
protected void RenderAsWireFrame(ref RenderingContext context)
{
PrimitiveType oldPrim = m_Primitive;
m_Primitive = PrimitiveType.LineStrip;
context.ApplyLineDefault();
base.Render(ref context);
ScanLine l1 = new ScanLine(-1, Count / 2);
ScanLine l2 = new ScanLine(-1, Count / 2);
for (int i = 0; i < Count; i++)
if (i % 2 == 0)
l1.Add(this[i]);
else
l2.Add(this[i]);
l1.Render(ref context);
l2.Render(ref context);
m_Primitive = oldPrim;
}
/// <summary>
/// Read ScanData File
/// </summary>
/// <param name="file"></param>
/// <returns></returns>
public static ScanData Read(string file)
{
ScanData ret = new ScanData();
using (StreamReader r = System.IO.File.OpenText(file))
{
string line = r.ReadLine();
string[] part = line.Split(":".ToArray());
int slicecount = int.Parse(part[1]);
for (int i = 0; i < slicecount; i++)
{
line = r.ReadLine();
part = line.Split(":".ToArray());
ScanLine slice = new ScanLine((int)double.Parse(part[1]));
line = r.ReadLine();
part = line.Split(":".ToArray());
if (part[0] == "DrawAs")
{
OpenTK.Graphics.OpenGL.PrimitiveType primitive = OpenTK.Graphics.OpenGL.PrimitiveType.Points;
Enum.TryParse<OpenTK.Graphics.OpenGL.PrimitiveType>(part[1], true, out primitive);
switch (primitive)
{
case OpenTK.Graphics.OpenGL.PrimitiveType.TriangleStrip:
{
slice = new ScanSlice(10000);
break;
}
case OpenTK.Graphics.OpenGL.PrimitiveType.LineStrip:
{
slice.DisplayAsLine = true;
break;
}
default:
{
slice.DisplayAsLine = false;
break;
}
}
line = r.ReadLine();
part = line.Split(":".ToArray());
}
int pointcount = int.Parse(part[1]);
for (int j = 0; j < pointcount; j++)
{
line = r.ReadLine();
part = line.Split("|".ToArray());
Vector3d pos = GetVector(part[0]);
Vector3d normal = pos.Normalized();
try
{
normal = GetVector(part[1]);
}
catch { }
Color color = System.Drawing.ColorTranslator.FromHtml(part[2]);
Point3D p = new Point3D(pos, normal, color);
slice.Add(p);
}
ret.Add(slice);
}
}
return ret;
}
public override ScanData DoTask(ScanData source)
{
if (!HardwareAvailable)
throw new Exception(string.Format("HardWare missing : TURNTABLE:{0} LASER:{1} CAMERA:{2}", HardwarePresentTrace(TurnTable), HardwarePresentTrace(Laser), HardwarePresentTrace(Camera)));
RotationStep = (double)Math.Round(TurnTable.MinimumRotation() + (15f - TurnTable.MinimumRotation()) * ((100 - Precision) / 100f), 2);
Settings settings = Settings.Get<Settings>();
CameraLoc.X = settings.Read(Settings.CAMERA, Settings.X, 0f);
CameraLoc.Y = settings.Read(Settings.CAMERA, Settings.Y, 270f);
CameraLoc.Z = settings.Read(Settings.CAMERA, Settings.Z, 70f);
double thres = settings.Read(Settings.LASER_COMMON, Settings.MAGNITUDE_THRESHOLD, 10);
int min = settings.Read(Settings.LASER_COMMON, Settings.MIN_WIDTH, 1);
int max = settings.Read(Settings.LASER_COMMON, Settings.MAX_WIDTH, 60);
ICameraProxy camera = Settings.Get<ICameraProxy>();
ImageProcessor = new ImageProcessor(thres, min, max);
SizeF tableSize = new SizeF(
(float)settings.Read(Settings.TABLE, Settings.DIAMETER, 20f),
(float)settings.Read(Settings.TABLE, Settings.HEIGHT, 15f)
);
Lasers = new List<LaserInfo>(LaserId.Length);
for (int i = 0; i < LaserId.Length; i++)
{
Lasers.Add(new LaserInfo(LaserId[i], CameraLoc, tableSize));
}
ScanData ret = new ScanData();
UpdatePercent(0, ret);
int fadeTime = settings.Read(Settings.LASER_COMMON, Settings.FADE_DELAY, 100);
TurnTable.InitialiseRotation();
int laserCount = Lasers.Count;
// Scan all laser location,
for (double currentAngle = 0; currentAngle < 360f; currentAngle += RotationStep)
{
if (this.CancelPending) return ret;
Laser.TurnAll(false); // All laser off
Thread.Sleep(fadeTime); // wait fade laser
Bitmap imgoff = GetCapture();
for (int laserIndex = 0; laserIndex < laserCount; laserIndex++)
{
Laser.Turn(Lasers[laserIndex].Id, true);
Thread.Sleep(fadeTime); // wait fade laser
Bitmap imgon = GetCapture();
Laser.Turn(Lasers[laserIndex].Id, false);
List<PointF> laserloc = ImageProcessor.Process(imgoff,imgon,null);
Point3DList samplePoints = Lasers[laserIndex].MapPoints(laserloc, UseTexture ? imgoff : null, UseCorrectionMatrix);
PositionPostProcess(ref samplePoints, -Utils.DEGREES_TO_RADIANS(currentAngle));
ScanLine line = new ScanLine(laserIndex, samplePoints);
line.DisplayAsLine = true;
ret.Add(line);
}
int percent = (int)((currentAngle / 360f) * 100f);
UpdatePercent(percent, ret);
TurnTable.Rotate(currentAngle, false);
}
LineSort lineSort = new LineSort();
ret = lineSort.Run(ret, CallerControl, this.Worker, this.WorkerArg);
if (!string.IsNullOrEmpty(FileName))
{
string path = Path.Combine(Program.UserDataPath, FileName);
ScanDataIO.Write(path, ret);
}
return ret;
}
/// <summary>
/// Ctor
/// </summary>
/// <param name="laserID"></param>
/// <param name="points"></param>
public ScanLine(ScanLine other)
: this(other.LaserID, (Point3DList)other)
{
DisplayAsLine = other.DisplayAsLine;
}
protected void DrawScanLine(Graphics g, PointF translation, double scale, ScanLine line, LaserCorrection corr,bool selected)
{
List<PointF> points = new List<PointF>(line.Count);
Matrix4d m = corr.GetMatrix();
int laserid = line.LaserID;
int count = line.Count;
Color baseCol = GetLaserColor(laserid);
Color col = Color.FromArgb(selected ? 128 / NumClass : 64 / NumClass, baseCol);
for (int i = 0; i < line.Count; i++)
{
Point3D p = line[i];
Vector3d v = Vector3d.Transform(p.Position, m);
PointF pt = new PointF((float)(v.X * scale), (float)(v.Z * scale));
pt.X += translation.X;
pt.Y += translation.Y;
points.Add(pt);
}
using (SolidBrush b = new SolidBrush(col))
g.FillPolygon(b, points.ToArray());
using (Pen b = new Pen(Color.FromArgb(selected ? 128 : 64 , baseCol)))
g.DrawPolygon(b, points.ToArray());
}
