List <ImageTaking> TakeSequentialImage(double ax, double ay, double dz, int nimage) { MotorControler mc = MotorControler.GetInstance(parameterManager); Camera camera = Camera.GetInstance(); Vector3 initialpos = mc.GetPoint(); List <ImageTaking> lit = new List <ImageTaking>(); for (int i = 0; i < nimage; i++) { Vector3 dstpoint = new Vector3( initialpos.X + ax * dz * i, initialpos.Y + ay * dz * i, initialpos.Z + dz * i ); mc.MovePoint(dstpoint); mc.Join(); byte[] b = camera.ArrayImage; Mat image = new Mat(440, 512, MatType.CV_8U, b); ImageTaking it = new ImageTaking(); it.img = image.Clone(); it.StageCoord = mc.GetPoint(); lit.Add(it); //image.Release(); //imagec.Release(); } return(lit); }
private void task() { MotorControler mc = MotorControler.GetInstance(parameterManager); Surface sur = Surface.GetInstance(parameterManager); Camera camera = Camera.GetInstance(); Led led = Led.GetInstance(); CoordManager cm = new CoordManager(parameterManager); Vector3 initialpoint = mc.GetPoint(); int pixthre = 500; for (int bx = -5; bx <= 5; bx++) { for (int by = -5; by <= 5; by++) { string txtfileName = string.Format(@"E:\img\{0}_{1}.txt", bx, by); StreamWriter twriter = File.CreateText(txtfileName); Vector3 blockstartpoint = new Vector3(); blockstartpoint.X = initialpoint.X + bx * 1.0; blockstartpoint.Y = initialpoint.Y + by * 1.0; blockstartpoint.Z = initialpoint.Z; mc.MoveTo(new Vector3(blockstartpoint.X + 0.5, blockstartpoint.Y + 0.5, initialpoint.Z - 0.020)); mc.Join(); int ledbrightness = led.AdjustLight(parameterManager); camera.Start(); surfrecog(pixthre, 0.003); camera.Stop(); double surfaceZup = mc.GetPoint().Z; //上面 ベース中からはじめ、ベース上側を表面認識 //ベース上側からはじめてZ方向正の向きにスキャン for (int vy = 0; vy < 10; vy++) { Vector3 linestartpoint = mc.GetPoint(); linestartpoint.X = blockstartpoint.X; linestartpoint.Y = blockstartpoint.Y + vy * parameterManager.SpiralShiftY; linestartpoint.Z = surfaceZup; for (int vx = 0; vx < 8;) { if (vx == 0) { Vector3 approachingpoint = mc.GetPoint(); approachingpoint.X = blockstartpoint.X + vx * parameterManager.SpiralShiftX - 0.05; approachingpoint.Y = blockstartpoint.Y + vy * parameterManager.SpiralShiftY - 0.05; approachingpoint.Z = linestartpoint.Z - 0.006; mc.MoveTo(approachingpoint); mc.Join(); } Vector3 viewstartpoint = mc.GetPoint(); viewstartpoint.X = blockstartpoint.X + vx * parameterManager.SpiralShiftX; viewstartpoint.Y = blockstartpoint.Y + vy * parameterManager.SpiralShiftY; viewstartpoint.Z = linestartpoint.Z - 0.006; mc.MoveTo(viewstartpoint); mc.Join(); Thread.Sleep(100); Vector3 viewpoint = mc.GetPoint(); List <ImageTaking> lit = new List <ImageTaking>(); mc.Inch(PlusMinus.Plus, 0.15, VectorId.Z); int viewcounter = 0; while (viewcounter < 16 + 3) { byte[] b = Ipt.CaptureMain(); Vector3 p = mc.GetPoint(); if (viewcounter >= 3) { ImageTaking it = new ImageTaking(p, b); string stlog = ""; stlog += String.Format("{0} {1} {2} {3}\n", ledbrightness, p.X, p.Y, p.Z); twriter.Write(stlog); } viewcounter++; }//view viewcounter = 0; double endz = mc.GetPoint().Z; mc.SlowDownStop(VectorId.Z); mc.Join(); if (endz - viewstartpoint.Z < 0.070) { tsparams tsp = new tsparams(); tsp.phthre = 10; List <microtrack> lm = TrackSelector.Select(lit, tsp); foreach (microtrack m in lm) { double viewx = viewpoint.X; double viewy = viewpoint.Y; double pixelx = 135.0 / 512.0; double pixely = 115.0 / 440.0; double x = viewx - (m.cx - 256) * pixelx; double y = viewy + (m.cy - 220) * pixely; Console.WriteLine(string.Format("{0:0.0} {1:0.0} {2:0.0} {3:0.0} {4:0.0} {5:0.0} {6:0.0} {7:0.0}", m.ph, m.pv, m.ax, m.ay, x, y, m.cx, m.cy)); } vx++; } } //vx } //vy //下面 ベース中からはじめ、ベース下側を表面認識 //ベース下側からはじめてZ方向負の向きにスキャン mc.MoveTo(new Vector3(blockstartpoint.X + 0.5, blockstartpoint.Y + 0.5, initialpoint.Z - 0.140)); mc.Join(); camera.Start(); surfrecog(pixthre, -0.003); camera.Stop(); double surfaceZdown = mc.GetPoint().Z; for (int vy = 0; vy < 10; vy++) { Vector3 linestartpoint = mc.GetPoint(); linestartpoint.X = blockstartpoint.X; linestartpoint.Y = blockstartpoint.Y + vy * parameterManager.SpiralShiftY; linestartpoint.Z = surfaceZdown; for (int vx = 0; vx < 8;) { if (vx == 0) { Vector3 approachingpoint = mc.GetPoint(); approachingpoint.X = blockstartpoint.X + vx * parameterManager.SpiralShiftX - 0.05; approachingpoint.Y = blockstartpoint.Y + vy * parameterManager.SpiralShiftY - 0.05; approachingpoint.Z = linestartpoint.Z + 0.006; mc.MoveTo(approachingpoint); mc.Join(); } Vector3 viewstartpoint = mc.GetPoint(); viewstartpoint.X = blockstartpoint.X + vx * parameterManager.SpiralShiftX; viewstartpoint.Y = blockstartpoint.Y + vy * parameterManager.SpiralShiftY; viewstartpoint.Z = linestartpoint.Z + 0.006; mc.MoveTo(viewstartpoint); mc.Join(); Thread.Sleep(100); Vector3 viewpoint = mc.GetPoint(); byte[] bb = new byte[440 * 512 * 16]; string datfileName = string.Format(@"E:\img\d_{0}_{1}_{2}_{3}.dat", (int)(viewpoint.X * 1000), (int)(viewpoint.Y * 1000), vx, vy ); BinaryWriter writer = new BinaryWriter(File.Open(datfileName, FileMode.Create)); mc.Inch(PlusMinus.Minus, 0.15, VectorId.Z); int viewcounter = 0; while (viewcounter < 16 + 3) { byte[] b = Ipt.CaptureMain(); Vector3 p = mc.GetPoint(); if (viewcounter >= 3) { b.CopyTo(bb, 440 * 512 * (viewcounter - 3)); string stlog = ""; stlog += String.Format("{0} {1} {2} {3}\n", ledbrightness, p.X, p.Y, p.Z); twriter.Write(stlog); } viewcounter++; }//view viewcounter = 0; double endz = mc.GetPoint().Z; mc.SlowDownStop(VectorId.Z); mc.Join(); Thread.Sleep(100); if (viewstartpoint.Z - endz < 0.070) { vx++; writer.Write(bb); writer.Flush(); writer.Close(); } } //vx } //vy camera.Stop(); twriter.Close(); } //blocky } //blockx } //task