예제 #1
0
        private void task()
        {
            // レンズが50倍に設定されていない場合は例外を返すようにしたいがやり方が分からん(20140724)

            //現在地からスパイラルサーチ30視野でグリッドマークを検出する
            //検出したら視野の真ん中に持ってくる
            try
            {
                MotorControler      mc = MotorControler.GetInstance(parameterManager);
                IGridMarkRecognizer GridMarkRecognizer = coordManager;
                mc.SetSpiralCenterPoint();
                Led     led          = Led.GetInstance();
                Vector2 encoderPoint = new Vector2(-1, -1);
                encoderPoint.X = mc.GetPoint().X;
                encoderPoint.Y = mc.GetPoint().Y;//おこられたのでしかたなくこうする 吉田20150427
                Vector2 viewerPoint = new Vector2(-1, -1);

                bool continueFlag = true;
                while (continueFlag)
                {
                    led.AdjustLight(parameterManager);
                    viewerPoint = GridMarkRecognizer.SearchGridMarkx50();
                    if (viewerPoint.X < 0 || viewerPoint.Y < 0)
                    {
                        System.Diagnostics.Debug.WriteLine(String.Format("grid mark not found"));
                        mc.MoveInSpiral(true);
                        mc.Join();
                        continueFlag = (mc.SpiralIndex < 30);
                    }
                    else
                    {
                        System.Diagnostics.Debug.WriteLine(String.Format("******** {0}  {1}", viewerPoint.X, viewerPoint.Y));
                        encoderPoint = coordManager.TransToEmulsionCoord(viewerPoint);
                        mc.MovePointXY(encoderPoint);
                        mc.Join();
                        continueFlag = false;
                    }
                } // while

                mc.MovePointXY(encoderPoint);
                mc.Join();
                viewerPoint  = GridMarkRecognizer.SearchGridMarkx50();
                encoderPoint = coordManager.TransToEmulsionCoord(viewerPoint);
                mc.MovePointXY(encoderPoint);
                mc.Join();
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine("exception");
            }
        }
예제 #2
0
        private void task()
        {
            MotorControler mc      = MotorControler.GetInstance(parameterManager);
            Camera         camera  = Camera.GetInstance();
            TracksManager  tm      = parameterManager.TracksManager;
            Surface        surface = Surface.GetInstance(parameterManager);
            int            mod     = parameterManager.ModuleNo;
            int            pl      = parameterManager.PlateNo;
            bool           dubflag;
            Led            led_ = Led.GetInstance();

            for (int i = 0; i < tm.NumOfTracks; i++)
            {
                Track myTrack = tm.GetTrack(i);
                Console.WriteLine(myTrack.IdString);
                string[] sp1 = myTrack.IdString.Split('-');

                string       logtxt = string.Format(@"C:\MKS_test\WorkingTime\{0}\{1}-{2}_Trackingtime.txt", mod, mod, pl);
                SimpleLogger SL1    = new SimpleLogger(logtxt, sp1[0], sp1[1]);

                MessageBoxResult r;
                // Massage box to check tracking is ok or not, if OK is put, will go to track position.
                r = MessageBox.Show(
                    String.Format("Let's go to {0}; {1} {2}. OK->Go, Cancel->exit", myTrack.IdString, myTrack.MsX, myTrack.MsY),
                    Properties.Strings.LensTypeSetting,
                    MessageBoxButton.OKCancel);
                if (r == MessageBoxResult.Cancel)
                {
                    return;
                }

                // Go to the track position
                double dstx = myTrack.MsX;
                double dsty = myTrack.MsY;
                mc.MovePointXY(dstx, dsty, delegate {
                    stage.WriteLine(Properties.Strings.MovingComplete);
                });
                mc.Join();

                SL1.Info("Tracking Start");
                // Beampatternmatching
                BPMW(myTrack, mod, pl);
                SL1.Info("Tracking End");
                mc.Join();
                Thread.Sleep(100);

                // Massage box to cheack tracking is ok or not, it Ok is put, it will go to next track.
                r = MessageBox.Show(
                    "OK->Next, Cancel->exit",
                    Properties.Strings.LensTypeSetting,
                    MessageBoxButton.OKCancel);
                if (r == MessageBoxResult.Cancel)
                {
                    return;
                }
            }
        }
예제 #3
0
        private void gotrack(Track myTrack)
        {
            MotorControler mc   = MotorControler.GetInstance(parameterManager);
            double         dstx = myTrack.MsX + coordManager.HFDX;
            double         dsty = myTrack.MsY + coordManager.HFDY;

            mc.MovePointXY(dstx, dsty, delegate {
                stage.WriteLine(Properties.Strings.MovingComplete);
            });
        }
예제 #4
0
        /// <summary>
        /// アクティビティの動作スレッド用のメソッドです。
        /// 直接呼び出さないでください。
        /// </summary>
        private void task()
        {
            GridMark       nearestMark = getNearestGirdMark();
            MotorControler mc          = MotorControler.GetInstance(parameterManager);

            mc.MovePointXY(nearestMark.x, nearestMark.y);
            mc.Join();

            // 新たにグリッドーマークの座標を取得したら、
            // Ipt.setHyperFineXYにその座標と既知の座標(予測点)のズレ(エンコーダ座標系)を渡す
        }
예제 #5
0
            private void nextTrackButton_Click(object sender, RoutedEventArgs e)
            {
                if (!parameterManager.TracksManager.IsInitialized)
                {
                    MessageBox.Show(Properties.Strings.TracksManagerException01);
                    return;
                }

                TracksManager tracksManager = parameterManager.TracksManager;

                if (tracksManager.TrackingIndex + 1 >= tracksManager.NumOfTracks)
                {
                    MessageBox.Show(Properties.Strings.TrackFoundComplete);
                    return;
                }
                tracksManager.UpdateTrack();
                UpdateTrackInfo();

                MotorControler mc = MotorControler.GetInstance();

                if (mc.IsMoving)
                {
                    if (askAbortMotorMoving())
                    {
                        mc.AbortMoving();
                        stage.WriteLine(Properties.Strings.AbortMotor);
                    }
                    else
                    {
                        return;
                    }
                }
                Track track = tracksManager.Track;

                stage.WriteLine(Properties.Strings.Moving);
#if !NoHardware
                mc.MovePointXY(track.MsX, track.MsX, delegate {
                    stage.WriteLine(Properties.Strings.MovingComplete);
                });
#endif
                tracksItem[tracksManager.TrackingIndex].FontWeight = FontWeights.Bold;
            }
예제 #6
0
        /// <summary>
        /// グリッドマークを基準に座標系を作成します.
        /// </summary>
        /// <exception cref="Exception">定義されたグリッドマーク数が少ない場合</exception>
        public void CreateCoordSystem()
        {
            double     x = new double();
            double     y = new double();
            double     emX, emY;
            Vector2Int iId;
            Vector2    offset;

            try {
                setAngleAndMagnitOfGrid();
                offset = calcOffset();
            } catch (Exception ex) {
                throw ex;
            }

            // 原点を設定
            gridOffsetX[1, 1] = 0;
            gridOffsetY[1, 1] = 0;

            for (int i = 1; i < AllGridMarksNum; ++i)
            {
                iId = getXYGridId(i);
                if (gridMarks[i].Existed)
                {
                    emX = gridMarks[i].x - offset.X;
                    emY = gridMarks[i].y - offset.Y;
                    Ipt.MtoG(0, emX, emY, ref x, ref y);
                    iId.X = (int)(((x + 100) / 100) + 0.5);
                    iId.Y = (int)(((y + 100) / 100) + 0.5);
                    gridOffsetX[iId.X, iId.Y] = x - gridOrgX[iId.X, iId.Y];
                    gridOffsetY[iId.X, iId.Y] = y - gridOrgY[iId.X, iId.Y];
                }
                else
                {
                    gridOffsetX[iId.X, iId.Y] = 0;
                    gridOffsetX[iId.X, iId.Y] = 0;
                }
            }

            /* 新しい原点のためにモータコントロールボードを初期化する.*/
            // 新しい原点に移動を開始し,別スレッドで移動完了を感知したら,
            // モータコントロールボードを初期化し原点を再設定する
            MotorControler mc = MotorControler.GetInstance();

            mc.MovePointXY(offset.X, offset.Y);

            Thread thread = new Thread(new ParameterizedThreadStart(delegate(object o) {
                MotorControler motor = MotorControler.GetInstance();
                while (motor.IsMoving)
                {
                    // モータの移動中は待機
                    Thread.Sleep(10);
                }

                // モータコントロールボードを初期化し,原点を設定する
                //motor.InitializeMotorControlBoard(MechaAxisAddress.XAddress);
                //motor.InitializeMotorControlBoard(MechaAxisAddress.YAddress);
                //motor.InitializeMotorControlBoard(MechaAxisAddress.ZAddress);
                coordDefined = true;
            }));

            try {
                thread.Start();
            } catch (Exception ex) {
                throw ex;
            }
        }
예제 #7
0
        private void task()
        {
            Camera         camera              = Camera.GetInstance();
            MotorControler mc                  = MotorControler.GetInstance(parameterManager);
            Vector3        CurrentPoint        = mc.GetPoint();
            Vector3        p                   = new Vector3();
            int            BinarizeThreshold   = 10;
            int            BrightnessThreshold = 7;
            Mat            sum                 = Mat.Zeros(440, 512, MatType.CV_8UC1);

            string       datfileName = string.Format(@"c:\img\{0}.dat", System.DateTime.Now.ToString("yyyyMMdd_HHmmss_fff"));
            BinaryWriter writer      = new BinaryWriter(File.Open(datfileName, FileMode.Create));

            for (int i = 0; i < 10; i++)
            {
                byte[] b = camera.ArrayImage;
                writer.Write(b);
                p = mc.GetPoint();
                Mat mat = new Mat(440, 512, MatType.CV_8U, b);
                mat.ImWrite(String.Format(@"c:\img\{0}_{1}_{2}_{3}.bmp",
                                          System.DateTime.Now.ToString("yyyyMMdd_HHmmss_fff"),
                                          (int)(p.X * 1000),
                                          (int)(p.Y * 1000),
                                          (int)(p.Z * 1000)));
                Cv2.GaussianBlur(mat, mat, Cv.Size(3, 3), -1);
                Mat gau = mat.Clone();
                Cv2.GaussianBlur(gau, gau, Cv.Size(31, 31), -1);
                Cv2.Subtract(gau, mat, mat);
                Cv2.Threshold(mat, mat, BinarizeThreshold, 1, ThresholdType.Binary);
                Cv2.Add(sum, mat, sum);
                mc.MoveDistance(-0.003, VectorId.Z);
                mc.Join();
            }

            Cv2.Threshold(sum, sum, BrightnessThreshold, 1, ThresholdType.Binary);

            //Cv2.FindContoursをつかうとAccessViolationExceptionになる(Release/Debug両方)ので、C-API風に書く
            using (CvMemStorage storage = new CvMemStorage())
            {
                using (CvContourScanner scanner = new CvContourScanner(sum.ToIplImage(), storage, CvContour.SizeOf, ContourRetrieval.Tree, ContourChain.ApproxSimple))
                {
                    //string fileName = string.Format(@"c:\img\{0}.txt",
                    //        System.DateTime.Now.ToString("yyyyMMdd_HHmmss_fff"));
                    string fileName = string.Format(@"c:\img\u.txt");

                    foreach (CvSeq <CvPoint> c in scanner)
                    {
                        CvMoments mom = new CvMoments(c, false);
                        if (c.ElemSize < 2)
                        {
                            continue;
                        }
                        if (mom.M00 == 0.0)
                        {
                            continue;
                        }
                        double mx = mom.M10 / mom.M00;
                        double my = mom.M01 / mom.M00;
                        File.AppendAllText(fileName, string.Format("{0:F} {1:F}\n", mx, my));
                    }
                }
            }

            sum *= 255;
            sum.ImWrite(String.Format(@"c:\img\{0}_{1}_{2}.bmp",
                                      System.DateTime.Now.ToString("yyyyMMdd_HHmmss_fff"),
                                      (int)(p.X * 1000),
                                      (int)(p.Y * 1000)));


            Vector2 encoderPoint = new Vector2(-1, -1);

            encoderPoint.X = mc.GetPoint().X;
            encoderPoint.Y = mc.GetPoint().Y;//おこられたのでしかたなくこうする 吉田20150427
            Vector2 viewerPoint = new Vector2(-1, -1);

            if (TigerPatternMatch.PatternMatch(ref viewerPoint))
            {
                encoderPoint = coordManager.TransToEmulsionCoord(viewerPoint);
                mc.MovePointXY(encoderPoint);
                mc.Join();
            }
        }
예제 #8
0
        private void task()
        {
            // レンズが50倍に設定されていない場合は例外を返すようにしたいがやり方が分からん(20140724)
            try
            {
                MotorControler mc          = MotorControler.GetInstance(parameterManager);
                GridMark       nearestMark = coordManager.GetTheNearestGridMark(mc.GetPoint());
                System.Diagnostics.Debug.WriteLine(String.Format("{0},  {1}", nearestMark.x, nearestMark.y));
                mc.MovePointXY(nearestMark.x, nearestMark.y);
                mc.Join();
            }
            catch (GridMarkNotFoundException ex)
            {
                System.Diagnostics.Debug.WriteLine(String.Format("{0}", ex.ToString()));
            }
            try
            {
                MotorControler      mc = MotorControler.GetInstance(parameterManager);
                IGridMarkRecognizer GridMarkRecognizer = coordManager;
                mc.SetSpiralCenterPoint();
                Led     led          = Led.GetInstance();
                Vector2 encoderPoint = new Vector2(-1, -1);
                encoderPoint.X = mc.GetPoint().X;
                encoderPoint.Y = mc.GetPoint().Y;//おこられたのでしかたなくこうする 吉田20150427
                Vector2 viewerPoint = new Vector2(-1, -1);

                bool continueFlag = true;
                while (continueFlag)
                {
                    led.AdjustLight(parameterManager);
                    viewerPoint = GridMarkRecognizer.SearchGridMarkx50();
                    if (viewerPoint.X < 0 || viewerPoint.Y < 0)
                    {
                        System.Diagnostics.Debug.WriteLine(String.Format("grid mark not found"));
                        mc.MoveInSpiral(true);
                        mc.Join();
                        continueFlag = (mc.SpiralIndex < 30);
                    }
                    else
                    {
                        System.Diagnostics.Debug.WriteLine(String.Format("******** {0}  {1}", viewerPoint.X, viewerPoint.Y));
                        encoderPoint = coordManager.TransToEmulsionCoord(viewerPoint);
                        mc.MovePointXY(encoderPoint);
                        mc.Join();
                        continueFlag = false;
                    }
                } // while

                //重心検出と移動を2回繰り返して、グリッドマークを視野中心にもっていく
                mc.MovePointXY(encoderPoint);
                mc.Join();
                viewerPoint  = GridMarkRecognizer.SearchGridMarkx50();
                encoderPoint = coordManager.TransToEmulsionCoord(viewerPoint);
                mc.MovePointXY(encoderPoint);
                mc.Join();
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine("exception");
            }
            try
            {
                MotorControler mc = MotorControler.GetInstance(parameterManager);
                Vector3        CurrentCenterPoint = mc.GetPoint();
                GridMark       nearestMark        = coordManager.GetTheNearestGridMark(CurrentCenterPoint);
                System.Diagnostics.Debug.WriteLine(String.Format("{0},  {1}", CurrentCenterPoint.X, CurrentCenterPoint.Y));
                coordManager.HFDX = CurrentCenterPoint.X - nearestMark.x;
                coordManager.HFDY = CurrentCenterPoint.Y - nearestMark.y;
            }
            catch (EntryPointNotFoundException ex)
            {
                MessageBox.Show("エントリポイントが見当たりません。 " + ex.Message);
                System.Diagnostics.Debug.WriteLine("エントリポイントが見当たりません。 " + ex.Message);
            }
        }
예제 #9
0
        private void task()
        {
            System.Diagnostics.Stopwatch worktime = new System.Diagnostics.Stopwatch();//Stopwatch
            worktime.Start();

            MotorControler mc      = MotorControler.GetInstance(parameterManager);
            Camera         camera  = Camera.GetInstance();
            TracksManager  tm      = parameterManager.TracksManager;
            Track          myTrack = tm.GetTrack(tm.TrackingIndex);
            Surface        surface = Surface.GetInstance(parameterManager);


            //for up layer

            double Sh    = 0.5 / (surface.UpTop - surface.UpBottom);
            double tansi = Math.Sqrt(myTrack.MsDX * myTrack.MsDX + myTrack.MsDY * myTrack.MsDY);
            double theta = Math.Atan(tansi);

            double dz;
            double dz_price_img = (6 * Math.Cos(theta) / Sh) / 1000;
            double dz_img       = dz_price_img * (-1);

            string datarootdirpath = string.Format(@"C:\MKS_test\{0}", myTrack.IdString);//Open forder to store track information

            System.IO.DirectoryInfo mydir = System.IO.Directory.CreateDirectory(datarootdirpath);

            List <OpenCvSharp.CPlusPlus.Point2d> Msdxdy = new List <OpenCvSharp.CPlusPlus.Point2d>();

            Msdxdy.Add(new OpenCvSharp.CPlusPlus.Point2d(myTrack.MsDX, myTrack.MsDY));

            List <OpenCvSharp.CPlusPlus.Point3d> LStage = new List <OpenCvSharp.CPlusPlus.Point3d>();
            List <OpenCvSharp.CPlusPlus.Point>   LPeak  = new List <OpenCvSharp.CPlusPlus.Point>();
            List <Point3d>             LTrack           = new List <Point3d>();
            List <List <ImageTaking> > UpTrackInfo      = new List <List <ImageTaking> >();

            //for down layer................................................................

            double Sh_low;

            Sh_low = 0.5 / (surface.LowTop - surface.LowBottom);

            List <OpenCvSharp.CPlusPlus.Point2d> Msdxdy_Low = new List <OpenCvSharp.CPlusPlus.Point2d>();
            List <OpenCvSharp.CPlusPlus.Point3d> LStage_Low = new List <OpenCvSharp.CPlusPlus.Point3d>();
            List <OpenCvSharp.CPlusPlus.Point>   LPeak_Low  = new List <OpenCvSharp.CPlusPlus.Point>();
            List <Point3d>             LTrack_Low           = new List <Point3d>();
            List <List <ImageTaking> > LowTrackInfo         = new List <List <ImageTaking> >();

            dz_price_img = (6 * Math.Cos(theta) / Sh) / 1000;
            dz_img       = dz_price_img * (-1);
            dz           = dz_img;
            int gotobase   = 0;
            int not_detect = 0;


            for (int i = 0; gotobase < 1; i++)
            {
                Vector3 initialpos = mc.GetPoint();
                double  moverange  = 9 * dz_img;
                double  predpoint  = moverange + initialpos.Z;

                if (predpoint < surface.UpBottom)
                {
                    gotobase = 1;

                    dz = surface.UpBottom - initialpos.Z + 9 * dz_price_img;
                }


                //gotobase = 1のときは、移動して画像を撮影するようにする。
                if (i != 0)
                {
                    Vector3 dstpoint = new Vector3(
                        LTrack[i - 1].X + Msdxdy[i].X * dz * Sh,
                        LTrack[i - 1].Y + Msdxdy[i].Y * dz * Sh,
                        LTrack[i - 1].Z + dz
                        );
                    mc.MovePoint(dstpoint);
                    mc.Join();
                }

                List <ImageTaking> LiITUpMid = TakeSequentialImage( //image taking
                    Msdxdy[i].X * Sh,                               //Dx
                    Msdxdy[i].Y * Sh,                               //Dy
                    dz_img,                                         //Dz
                    10);                                            //number of images


                LStage.Add(new OpenCvSharp.CPlusPlus.Point3d(LiITUpMid[9].StageCoord.X, LiITUpMid[9].StageCoord.Y, LiITUpMid[9].StageCoord.Z));
                LiITUpMid[9].img.ImWrite(datarootdirpath + string.Format(@"\img_l_up_{0}.bmp", i));
                UpTrackInfo.Add(LiITUpMid);


                List <Mat> binimages = new List <Mat>();
                for (int t = 0; t <= 9; t++)
                {
                    Mat bin = (Mat)DogContrastBinalize(LiITUpMid[t].img);

                    double xx = myTrack.MsDX * myTrack.MsDX;
                    double yy = myTrack.MsDY * myTrack.MsDY;
                    if (Math.Sqrt(xx + yy) >= 0.4)
                    {//////////////////????????????????????
                        Cv2.Dilate(bin, bin, new Mat());
                    }
                    Cv2.Dilate(bin, bin, new Mat());
                    binimages.Add(bin);
                }

                //trackを重ねる処理を入れる。
                Point2d pixel_cen = TrackDetection(binimages, 256, 220, 3, 3, 4, 90, 6);//, true);
                //Point2d pixel_cen = TrackDetection(binimages, 256, 220, 3, 3, 4, 90, true);

                if (pixel_cen.X == -1 & pixel_cen.Y == -1)
                {
                    mc.Join();
                    not_detect = 1;
                    goto not_detect_track;
                }



                LPeak.Add(new OpenCvSharp.CPlusPlus.Point(pixel_cen.X - 256, pixel_cen.Y - 220));

                double firstx = LStage[i].X - LPeak[i].X * 0.000267;
                double firsty = LStage[i].Y + LPeak[i].Y * 0.000267;
                double firstz = LStage[i].Z;
                LTrack.Add(new Point3d(firstx, firsty, firstz));
                //

                //上側乳剤層上面の1回目のtrack検出によって、次のtrackの位置を検出する角度を求める。
                //その角度が、1回目のtrack検出の結果によって大きな角度にならないように調整をする。

                if (i == 0)
                {
                    Msdxdy.Add(new OpenCvSharp.CPlusPlus.Point2d(myTrack.MsDX, myTrack.MsDY));
                }
                else if (i == 1)
                {
                    List <Point3d> LTrack_ghost = new List <Point3d>();
                    double         dzPrev       = (LStage[0].Z - surface.UpTop) * Sh;
                    double         Lghost_x     = LTrack[0].X - Msdxdy[i].X * dzPrev;
                    double         Lghost_y     = LTrack[0].Y - Msdxdy[i].Y * dzPrev;
                    LTrack_ghost.Add(new Point3d(Lghost_x, Lghost_y, surface.UpTop));//上側乳剤層上面にtrackがあるならどの位置にあるかを算出する。

                    string       txtfileName_ltrackghost = datarootdirpath + string.Format(@"\LTrack_ghost.txt");
                    StreamWriter twriter_ltrackghost     = File.CreateText(txtfileName_ltrackghost);
                    twriter_ltrackghost.WriteLine("{0} {1} {2}", LTrack_ghost[0].X, LTrack_ghost[0].Y, LTrack_ghost[0].Z);
                    twriter_ltrackghost.Close();

                    OpenCvSharp.CPlusPlus.Point2d Tangle = ApproximateStraight(Sh, LTrack_ghost[0], LTrack[0], LTrack[1]);
                    Msdxdy.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle.X, Tangle.Y));
                }
                else
                {
                    OpenCvSharp.CPlusPlus.Point2d Tangle = ApproximateStraight(Sh, LTrack[i - 2], LTrack[i - 1], LTrack[i]);
                    Msdxdy.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle.X, Tangle.Y));
                }
            }//for i-loop

            //baseまたぎ
            int ltrack_counter = LTrack.Count();
            int msdxdy_counter = Msdxdy.Count();

            mc.MovePoint(
                LTrack[ltrack_counter - 1].X + Msdxdy[msdxdy_counter - 1].X * (surface.LowTop - surface.UpBottom),
                LTrack[ltrack_counter - 1].Y + Msdxdy[msdxdy_counter - 1].Y * (surface.LowTop - surface.UpBottom),
                surface.LowTop
                );
            mc.Join();

            //////ここから下gelの処理
            Msdxdy_Low.Add(new OpenCvSharp.CPlusPlus.Point2d(Msdxdy[msdxdy_counter - 1].X, Msdxdy[msdxdy_counter - 1].Y));

            //今までのtrack追跡プログラムとは異なる角度等の使い方をする。
            dz_price_img = (6 * Math.Cos(theta) / Sh_low) / 1000;
            dz_img       = dz_price_img * (-1);
            dz           = dz_img;

            int goto_dgel = 0;

            for (int i = 0; goto_dgel < 1; i++)
            {
                ///////移動して画像処理をしたときに、下gelの下に入らないようにする。
                Vector3 initialpos = mc.GetPoint();
                double  moverange  = 9 * dz_img;
                double  predpoint  = moverange + initialpos.Z;

                if (predpoint < surface.LowBottom)//もしもbaseに入りそうなら、8枚目の画像がちょうど下gelを撮影するようにdzを調整する。
                {
                    goto_dgel = 1;

                    dz = surface.LowBottom - initialpos.Z + 9 * dz_price_img;
                }
                ////////

                //goto_dgel == 1のときは、移動して画像を撮影するようにする。
                if (i != 0)
                {
                    Vector3 dstpoint = new Vector3(
                        LTrack_Low[i - 1].X + Msdxdy_Low[i].X * dz * Sh_low,
                        LTrack_Low[i - 1].Y + Msdxdy_Low[i].Y * dz * Sh_low,
                        LTrack_Low[i - 1].Z + dz
                        );
                    mc.MovePoint(dstpoint);
                    mc.Join();
                }

                //今までのtrack追跡プログラムとは異なる角度等の使い方をする。
                List <ImageTaking> LiITLowMid = TakeSequentialImage(
                    Msdxdy[i].X * Sh_low,
                    Msdxdy[i].Y * Sh_low,
                    dz_img,
                    10);

                //画像・座標の記録
                LStage_Low.Add(new OpenCvSharp.CPlusPlus.Point3d(LiITLowMid[9].StageCoord.X, LiITLowMid[9].StageCoord.Y, LiITLowMid[9].StageCoord.Z));
                LiITLowMid[9].img.ImWrite(datarootdirpath + string.Format(@"\img_l_low_{0}.png", i));

                LowTrackInfo.Add(LiITLowMid);//撮影した8枚の画像と、撮影した位置を記録する。

                //撮影した画像をここで処理する。
                List <Mat> binimages = new List <Mat>();
                for (int t = 0; t <= 9; t++)
                {
                    Mat bin = (Mat)DogContrastBinalize(LiITLowMid[t].img);

                    double xx = myTrack.MsDX * myTrack.MsDX;
                    double yy = myTrack.MsDY * myTrack.MsDY;
                    if (Math.Sqrt(xx + yy) >= 0.4)
                    {
                        Cv2.Dilate(bin, bin, new Mat());
                    }
                    Cv2.Dilate(bin, bin, new Mat());
                    binimages.Add(bin);
                }
                //trackを重ねる処理を入れる。
                Point2d pixel_cen = TrackDetection(binimages, 256, 220, 3, 3, 4, 90, 6);//, true);//画像の8枚目におけるtrackのpixel座標を算出する。

                //もし検出に失敗した場合はループを抜ける。
                if (pixel_cen.X == -1 & pixel_cen.Y == -1)
                {
                    //mc.MovePoint(LTrack_Low[i - 1].X, LTrack_Low[i - 1].Y, LTrack_Low[i - 1].Z);
                    mc.Join();

                    not_detect = 1;
                    goto not_detect_track;
                }
                //

                //検出したpixel座標をstage座標に変換するなどlistに追加する。
                LPeak_Low.Add(new OpenCvSharp.CPlusPlus.Point(pixel_cen.X - 256, pixel_cen.Y - 220));

                double firstx = LStage_Low[i].X - LPeak_Low[i].X * 0.000267;
                double firsty = LStage_Low[i].Y + LPeak_Low[i].Y * 0.000267;
                double firstz = LStage_Low[i].Z;
                LTrack_Low.Add(new Point3d(firstx, firsty, firstz));
                //

                //ここからは、最小二乗法で角度を算出するプログラムである。
                //上側乳剤層上面の1回目のtrack検出によって、次のtrackの位置を検出する角度を求める。
                //その角度が、1回目のtrack検出の結果によって大きな角度にならないように調整をする。
                if (i == 0)
                {
                    OpenCvSharp.CPlusPlus.Point2d Tangle_l = ApproximateStraightBase(Sh, Sh_low, LTrack[ltrack_counter - 2], LTrack[ltrack_counter - 1], LTrack_Low[i], surface);
                    Msdxdy_Low.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle_l.X, Tangle_l.Y));
                }
                else if (i == 1)
                {
                    OpenCvSharp.CPlusPlus.Point2d Tangle_l = ApproximateStraightBase(Sh, Sh_low, LTrack[ltrack_counter - 1], LTrack_Low[i - 1], LTrack_Low[i], surface);
                    Msdxdy_Low.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle_l.X, Tangle_l.Y));
                }
                else
                {
                    OpenCvSharp.CPlusPlus.Point2d Tangle_l = ApproximateStraight(Sh_low, LTrack_Low[i - 2], LTrack_Low[i - 1], LTrack_Low[i]);
                    Msdxdy_Low.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle_l.X, Tangle_l.Y));
                }
            }//i_loop

            //
            int ltrack_low_count = LTrack_Low.Count();

            mc.MovePointXY(LTrack_Low[ltrack_low_count - 1].X, LTrack_Low[ltrack_low_count - 1].Y);

            mc.Join();

            //検出に失敗した場合は、ループを抜けてここに来る。
            not_detect_track :;//検出に失敗したと考えられる地点で画像を取得し、下ゲル下面まで移動する。(現在は下ゲル下面とするが、今後変更する可能性有。)

            if (not_detect != 0)
            {
                //写真撮影
                List <ImageTaking> NotDetect = TakeSequentialImage(
                    Msdxdy[0].X * Sh,
                    Msdxdy[0].Y * Sh,
                    0,
                    1);

                string       txtfileName_t_not_detect = datarootdirpath + string.Format(@"\not_detect.txt");
                StreamWriter twriter_t_not_detect     = File.CreateText(txtfileName_t_not_detect);
                for (int i = 0; i < NotDetect.Count; i++)
                {
                    NotDetect[i].img.ImWrite(datarootdirpath + string.Format(@"\img_t_not_detect.bmp"));
                    Vector3 p = NotDetect[i].StageCoord;
                    twriter_t_not_detect.WriteLine("{0} {1} {2} {3}", i, p.X, p.Y, p.Z);
                }
                twriter_t_not_detect.Close();

                mc.MovePointZ(surface.LowBottom);

                mc.Join();
            }

            //file write out up_gel
            string       txtfileName_surface = datarootdirpath + string.Format(@"\surface.txt");
            StreamWriter twriter_surface     = File.CreateText(txtfileName_surface);

            twriter_surface.WriteLine("{0} {1} {2} {3}", surface.UpTop, surface.UpBottom, surface.LowTop, surface.LowBottom);
            twriter_surface.Close();


            string       txtfileName_sh_up = datarootdirpath + string.Format(@"\Sh_up.txt");
            StreamWriter twriter_sh_up     = File.CreateText(txtfileName_sh_up);

            twriter_sh_up.WriteLine("{0}", Sh);
            twriter_sh_up.Close();

            //file write out
            string       txtfileName_t_info_up = datarootdirpath + string.Format(@"\location_up.txt");
            StreamWriter twriter_t_info_up     = File.CreateText(txtfileName_t_info_up);

            for (int i = 0; i < UpTrackInfo.Count; i++)
            {
                for (int t = 0; t < UpTrackInfo[i].Count; t++)
                {
                    UpTrackInfo[i][t].img.ImWrite(datarootdirpath + string.Format(@"\img_t_info_up_{0}-{1}.bmp", i, t));
                    Vector3 p = UpTrackInfo[i][t].StageCoord;
                    twriter_t_info_up.WriteLine("{0} {1} {2} {3} {4}", i, t, p.X, p.Y, p.Z);
                }
            }
            twriter_t_info_up.Close();

            string       txtfileName_lpeak = datarootdirpath + string.Format(@"\lpeak_up.txt");
            StreamWriter twriter_lpeak     = File.CreateText(txtfileName_lpeak);

            for (int i = 0; i < LPeak.Count(); i++)
            {
                OpenCvSharp.CPlusPlus.Point p = LPeak[i];
                twriter_lpeak.WriteLine("{0} {1} {2}", i, p.X, p.Y);
            }
            twriter_lpeak.Close();

            string       txtfileName_ltrack = datarootdirpath + string.Format(@"\ltrack_up.txt");
            StreamWriter twriter_ltrack     = File.CreateText(txtfileName_ltrack);

            for (int i = 0; i < LTrack.Count(); i++)
            {
                OpenCvSharp.CPlusPlus.Point3d p = LTrack[i];
                twriter_ltrack.WriteLine("{0} {1} {2} {3}", i, p.X, p.Y, p.Z);
            }
            twriter_ltrack.Close();

            string       txtfileName_msdxdy = datarootdirpath + string.Format(@"\msdxdy.txt");
            StreamWriter twriter_msdxdy     = File.CreateText(txtfileName_msdxdy);

            for (int i = 0; i < Msdxdy.Count(); i++)
            {
                OpenCvSharp.CPlusPlus.Point2d p = Msdxdy[i];
                twriter_msdxdy.WriteLine("{0} {1} {2}", i, p.X, p.Y);
            }
            twriter_msdxdy.Close();


            //file write out low_gel
            string       txtfileName_sh_low = datarootdirpath + string.Format(@"\Sh_low.txt");
            StreamWriter twriter_sh_low     = File.CreateText(txtfileName_sh_low);

            twriter_sh_low.WriteLine("{0}", Sh_low);
            twriter_sh_low.Close();

            string       txtfileName_t_info_low = datarootdirpath + string.Format(@"\location_low.txt");
            StreamWriter twriter_t_info_low     = File.CreateText(txtfileName_t_info_low);

            for (int i = 0; i < LowTrackInfo.Count; i++)
            {
                for (int t = 0; t < LowTrackInfo[i].Count; t++)
                {
                    LowTrackInfo[i][t].img.ImWrite(datarootdirpath + string.Format(@"\img_t_info_low_{0}-{1}.bmp", i, t));
                    Vector3 p = LowTrackInfo[i][t].StageCoord;
                    twriter_t_info_low.WriteLine("{0} {1} {2} {3} {4}", i, t, p.X, p.Y, p.Z);
                }
            }
            twriter_t_info_low.Close();

            string       txtfileName_lpeak_low = datarootdirpath + string.Format(@"\lpeak_low.txt");
            StreamWriter twriter_lpeak_low     = File.CreateText(txtfileName_lpeak_low);

            for (int i = 0; i < LPeak_Low.Count(); i++)
            {
                OpenCvSharp.CPlusPlus.Point p = LPeak_Low[i];
                twriter_lpeak_low.WriteLine("{0} {1} {2}", i, p.X, p.Y);
            }
            twriter_lpeak_low.Close();

            string       txtfileName_ltrack_low = datarootdirpath + string.Format(@"\ltrack_low.txt");
            StreamWriter twriter_ltrack_low     = File.CreateText(txtfileName_ltrack_low);

            for (int i = 0; i < LTrack_Low.Count(); i++)
            {
                OpenCvSharp.CPlusPlus.Point3d p = LTrack_Low[i];
                twriter_ltrack_low.WriteLine("{0} {1} {2} {3}", i, p.X, p.Y, p.Z);
            }
            twriter_ltrack_low.Close();

            string       txtfileName_msdxdy_low = datarootdirpath + string.Format(@"\msdxdy_low.txt");
            StreamWriter twriter_msdxdy_low     = File.CreateText(txtfileName_msdxdy_low);

            for (int i = 0; i < Msdxdy_Low.Count(); i++)
            {
                OpenCvSharp.CPlusPlus.Point2d p = Msdxdy_Low[i];
                twriter_msdxdy_low.WriteLine("{0} {1} {2}", i, p.X, p.Y);
            }
            twriter_msdxdy_low.Close();

            worktime.Stop();

            string       txtfileName_time_log = datarootdirpath + string.Format(@"\time_log.txt");
            StreamWriter twriter_time_log     = File.CreateText(txtfileName_time_log);

            twriter_time_log.WriteLine("{0} {1} {2}", myTrack.MsDX, myTrack.MsDY, worktime.Elapsed);
            twriter_time_log.Close();

            GoTopUp();
            mc.Join();
        }
예제 #10
0
        private void task()
        {
            TracksManager  tm                = parameterManager.TracksManager;
            Track          myTrack           = tm.GetTrack(tm.TrackingIndex);
            MotorControler mc                = MotorControler.GetInstance(parameterManager);
            Camera         camera            = Camera.GetInstance();
            List <Mat>     image_set         = new List <Mat>();
            List <Mat>     image_set_reverse = new List <Mat>();

            Surface surface   = Surface.GetInstance(parameterManager);//表面認識から境界値を取得
            double  uptop     = surface.UpTop;
            double  upbottom  = surface.UpBottom;
            double  lowtop    = surface.LowTop;
            double  lowbottom = surface.LowBottom;

            double now_x = mc.GetPoint().X;
            double now_y = mc.GetPoint().Y;
            double now_z = mc.GetPoint().Z;


            common_dx = myTrack.MsDX + ((0.265625 * over_dx * 3) / (0.024 * 2.2 * 1000));
            common_dy = myTrack.MsDY - ((0.265625 * over_dy * 3) / (0.024 * 2.2 * 1000));


            for (int i = 0; i < 8; i++)
            {                                                        //myTrack.MsD○はdz1mmあたりのd○の変位mm
                double next_x = now_x - i * common_dx * 0.003 * 2.2; //3μm間隔で撮影
                double next_y = now_y - i * common_dy * 0.003 * 2.2; //Shrinkage Factor は2.2で計算(仮)
                mc.MovePoint(next_x, next_y, now_z - 0.003 * i);
                mc.Join();

                byte[] b      = camera.ArrayImage;
                Mat    image  = new Mat(440, 512, MatType.CV_8U, b);
                Mat    imagec = image.Clone();
                image_set.Add(imagec);
            }

            for (int i = 7; i >= 0; i--)
            {
                image_set_reverse.Add(image_set[i]);
            }

            int n = image_set.Count();//1回分の取得画像の枚数

            Mat cont  = new Mat(440, 512, MatType.CV_8U);
            Mat gau_1 = new Mat(440, 512, MatType.CV_8U);
            Mat gau_2 = new Mat(440, 512, MatType.CV_8U);
            Mat sub   = new Mat(440, 512, MatType.CV_8U);
            Mat bin   = new Mat(440, 512, MatType.CV_8U);

            double Max_kido;
            double Min_kido;

            OpenCvSharp.CPlusPlus.Point maxloc;
            OpenCvSharp.CPlusPlus.Point minloc;

            List <Mat> two_set  = new List <Mat>();
            List <Mat> Part_img = new List <Mat>();

            for (int i = 0; i < image_set.Count(); i++)
            {
                Cv2.GaussianBlur((Mat)image_set_reverse[i], gau_1, Cv.Size(3, 3), -1); //パラメータ見ないといけない。
                Cv2.GaussianBlur(gau_1, gau_2, Cv.Size(51, 51), -1);                   //パラメータ見ないといけない。
                Cv2.Subtract(gau_2, gau_1, sub);
                Cv2.MinMaxLoc(sub, out Min_kido, out Max_kido, out minloc, out maxloc);
                cont = (sub - Min_kido) * 255 / (Max_kido - Min_kido);
                cont.ImWrite(string.Format(@"C:\set\cont_{0}.bmp", i));
                Cv2.Threshold(cont, bin, 115, 1, ThresholdType.Binary);//パラメータ見ないといけない。
                two_set.Add(bin);
            }

            List <mm> white_area = new List <mm>();
            int       x0         = 256;
            int       y0         = 220;//視野の中心


            for (int delta_xx = -1; delta_xx <= 1; delta_xx++)//一番下の画像よりどれだけずらすか
            {
                for (int delta_yy = -1; delta_yy <= 1; delta_yy++)
                {
                    {
                        //    //積層写真の型作り(行列の中身は0行列)
                        //    Mat superimposed = Mat.Zeros(440 + (n - 1) * Math.Abs(delta_yy), 512 + (n - 1) * Math.Abs(delta_xx), MatType.CV_8UC1);
                        //
                        //    //各写真の型作り
                        //    for (int i = 0; i < two_set.Count; i++) {
                        //        Mat Part = Mat.Zeros(440 + (n - 1) * Math.Abs(delta_yy), 512 + (n - 1) * Math.Abs(delta_xx), MatType.CV_8UC1);
                        //        Part_img.Add(Part);
                        //    }

                        //積層写真の型作り(行列の中身は0行列)
                        Mat superimposed = Mat.Zeros(440 + 3 * Math.Abs(delta_yy), 512 + 3 * Math.Abs(delta_xx), MatType.CV_8UC1);


                        //各写真の型作り
                        for (int i = 0; i < two_set.Count; i++)
                        {
                            Mat Part = Mat.Zeros(440 + 3 * Math.Abs(delta_yy), 512 + 3 * Math.Abs(delta_xx), MatType.CV_8UC1);
                            Part_img.Add(Part);
                        }//2枚を1セットにしてずらす場合



                        if (delta_xx >= 0 && delta_yy >= 0)//画像の右下への移動
                        {
                            for (int i = 0; i < two_set.Count; i++)
                            {
                                if (i == 0 || i == 1)
                                {
                                    Part_img[i][
                                        0
                                        , 440
                                        , 0
                                        , 512
                                    ] = two_set[i];     //処理済み画像をPartの対応する部分に入れていく
                                }
                                else if (i == 2 || i == 3)
                                {
                                    Part_img[i][
                                        0 + Math.Abs(delta_yy)     //yの値のスタート地点
                                        , 440 + Math.Abs(delta_yy) //yの値のゴール地点
                                        , 0 + Math.Abs(delta_xx)   //xの値のスタート地点
                                        , 512 + Math.Abs(delta_xx) //xの値のゴール地点
                                    ] = two_set[i];                //処理済み画像をPartの対応する部分に入れていく
                                }
                                else if (i == 4 || i == 5)
                                {
                                    Part_img[i][
                                        0 + 2 * Math.Abs(delta_yy)     //yの値のスタート地点
                                        , 440 + 2 * Math.Abs(delta_yy) //yの値のゴール地点
                                        , 0 + 2 * Math.Abs(delta_xx)   //xの値のスタート地点
                                        , 512 + 2 * Math.Abs(delta_xx) //xの値のゴール地点
                                    ] = two_set[i];                    //処理済み画像をPartの対応する部分に入れていく
                                }
                                else if (i == 6 || i == 7)
                                {
                                    Part_img[i][
                                        0 + 3 * Math.Abs(delta_yy)     //yの値のスタート地点
                                        , 440 + 3 * Math.Abs(delta_yy) //yの値のゴール地点
                                        , 0 + 3 * Math.Abs(delta_xx)   //xの値のスタート地点
                                        , 512 + 3 * Math.Abs(delta_xx) //xの値のゴール地点
                                    ] = two_set[i];                    //処理済み画像をPartの対応する部分に入れていく
                                }
                            }
                            for (int i = 0; i < Part_img.Count(); i++)
                            {
                                superimposed += Part_img[i];
                            }

                            Cv2.Threshold(superimposed, superimposed, 5, 255, ThresholdType.ToZero);//パラメータ見ないといけない。

                            superimposed.SubMat(0
                                                , 440
                                                , 0
                                                , 512).CopyTo(superimposed);    //1枚目の画像の大きさ、場所で切り取る
                        }



                        if (delta_xx >= 0 && delta_yy < 0)//画像の右上への移動
                        {
                            for (int i = 0; i < two_set.Count; i++)
                            {
                                if (i == 0 || i == 1)
                                {
                                    Part_img[i][
                                        0 + 3
                                        , 440 + 3
                                        , 0
                                        , 512
                                    ] = two_set[i];     //処理済み画像をPartの対応する部分に入れていく
                                }
                                else if (i == 2 || i == 3)
                                {
                                    Part_img[i][
                                        0 + 3 - 1                  //yの値のスタート地点
                                        , 440 + 3 - 1              //yの値のゴール地点
                                        , 0 + Math.Abs(delta_xx)   //xの値のスタート地点
                                        , 512 + Math.Abs(delta_xx) //xの値のゴール地点
                                    ] = two_set[i];                //処理済み画像をPartの対応する部分に入れていく
                                }
                                else if (i == 4 || i == 5)
                                {
                                    Part_img[i][
                                        0 + 3 - 2                      //yの値のスタート地点
                                        , 440 + 3 - 2                  //yの値のゴール地点
                                        , 0 + 2 * Math.Abs(delta_xx)   //xの値のスタート地点
                                        , 512 + 2 * Math.Abs(delta_xx) //xの値のゴール地点
                                    ] = two_set[i];                    //処理済み画像をPartの対応する部分に入れていく
                                }
                                else if (i == 6 || i == 7)
                                {
                                    Part_img[i][
                                        0 + 3 - 3                      //yの値のスタート地点
                                        , 440 + 3 - 3                  //yの値のゴール地点
                                        , 0 + 3 * Math.Abs(delta_xx)   //xの値のスタート地点
                                        , 512 + 3 * Math.Abs(delta_xx) //xの値のゴール地点
                                    ] = two_set[i];                    //処理済み画像をPartの対応する部分に入れていく
                                }
                            }
                            for (int i = 0; i < Part_img.Count(); i++)
                            {
                                superimposed += Part_img[i];
                            }

                            Cv2.Threshold(superimposed, superimposed, 5, 255, ThresholdType.ToZero);//パラメータ見ないといけない。

                            superimposed.SubMat(0 + 3
                                                , 440 + 3
                                                , 0
                                                , 512).CopyTo(superimposed);    //1枚目の画像の大きさ、場所で切り取る
                        }



                        if (delta_xx < 0 && delta_yy < 0)//画像の左上への移動
                        {
                            for (int i = 0; i < two_set.Count; i++)
                            {
                                if (i == 0 || i == 1)
                                {
                                    Part_img[i][
                                        0 + 3
                                        , 440 + 3
                                        , 0 + 3
                                        , 512 + 3
                                    ] = two_set[i];     //処理済み画像をPartの対応する部分に入れていく
                                }
                                else if (i == 2 || i == 3)
                                {
                                    Part_img[i][
                                        0 + 3 - 1       //yの値のスタート地点
                                        , 440 + 3 - 1   //yの値のゴール地点
                                        , 0 + 3 - 1     //xの値のスタート地点
                                        , 512 + 3 - 1   //xの値のゴール地点
                                    ] = two_set[i];     //処理済み画像をPartの対応する部分に入れていく
                                }
                                else if (i == 4 || i == 5)
                                {
                                    Part_img[i][
                                        0 + 3 - 2       //yの値のスタート地点
                                        , 440 + 3 - 2   //yの値のゴール地点
                                        , 0 + 3 - 2     //xの値のスタート地点
                                        , 512 + 3 - 2   //xの値のゴール地点
                                    ] = two_set[i];     //処理済み画像をPartの対応する部分に入れていく
                                }
                                else if (i == 6 || i == 7)
                                {
                                    Part_img[i][
                                        0 + 3 - 3           //yの値のスタート地点
                                        , 440 + 3 - 3       //yの値のゴール地点
                                        , 0 + 3 - 3         //xの値のスタート地点
                                        , 512 + 3 - 3       //xの値のゴール地点
                                    ] = two_set[i];         //処理済み画像をPartの対応する部分に入れていく
                                }
                            }
                            for (int i = 0; i < Part_img.Count(); i++)
                            {
                                superimposed += Part_img[i];
                            }

                            Cv2.Threshold(superimposed, superimposed, 5, 255, ThresholdType.ToZero);//パラメータ見ないといけない。

                            superimposed.SubMat(0 + 3
                                                , 440 + 3
                                                , 0 + 3
                                                , 512 + 3).CopyTo(superimposed);    //1枚目の画像の大きさ、場所で切り取る
                        }


                        if (delta_xx < 0 && delta_yy >= 0)//画像の左下への移動
                        {
                            for (int i = 0; i < two_set.Count; i++)
                            {
                                if (i == 0 || i == 1)
                                {
                                    Part_img[i][
                                        0
                                        , 440
                                        , 0 + 3
                                        , 512 + 3
                                    ] = two_set[i];     //処理済み画像をPartの対応する部分に入れていく
                                }
                                else if (i == 2 || i == 3)
                                {
                                    Part_img[i][
                                        0 + Math.Abs(delta_yy)     //yの値のスタート地点
                                        , 440 + Math.Abs(delta_yy) //yの値のゴール地点
                                        , 0 + 3 - 1                //xの値のスタート地点
                                        , 512 + 3 - 1              //xの値のゴール地点
                                    ] = two_set[i];                //処理済み画像をPartの対応する部分に入れていく
                                }
                                else if (i == 4 || i == 5)
                                {
                                    Part_img[i][
                                        0 + 2 * Math.Abs(delta_yy)     //yの値のスタート地点
                                        , 440 + 2 * Math.Abs(delta_yy) //yの値のゴール地点
                                        , 0 + 3 - 2                    //xの値のスタート地点
                                        , 512 + 3 - 2                  //xの値のゴール地点
                                    ] = two_set[i];                    //処理済み画像をPartの対応する部分に入れていく
                                }
                                else if (i == 6 || i == 7)
                                {
                                    Part_img[i][
                                        0 + 3 * Math.Abs(delta_yy)     //yの値のスタート地点
                                        , 440 + 3 * Math.Abs(delta_yy) //yの値のゴール地点
                                        , 0 + 3 - 3                    //xの値のスタート地点
                                        , 512 + 3 - 3                  //xの値のゴール地点
                                    ] = two_set[i];                    //処理済み画像をPartの対応する部分に入れていく
                                }
                            }
                            for (int i = 0; i < Part_img.Count(); i++)
                            {
                                superimposed += Part_img[i];
                            }

                            Cv2.Threshold(superimposed, superimposed, 5, 255, ThresholdType.ToZero);//パラメータ見ないといけない。

                            superimposed.SubMat(0
                                                , 440
                                                , 0 + 3
                                                , 512 + 3).CopyTo(superimposed);    //1枚目の画像の大きさ、場所で切り取る
                        }

                        Mat one1 = Mat.Ones(y0 - 20, 512, MatType.CV_8UC1);//視野の中心からどれだけの窓を開けるか
                        Mat one2 = Mat.Ones(41, x0 - 20, MatType.CV_8UC1);
                        Mat one3 = Mat.Ones(41, 491 - x0, MatType.CV_8UC1);
                        Mat one4 = Mat.Ones(419 - y0, 512, MatType.CV_8UC1);

                        superimposed[0, y0 - 20, 0, 512]             = one1 * 0;
                        superimposed[y0 - 20, y0 + 21, 0, x0 - 20]   = one2 * 0;
                        superimposed[y0 - 20, y0 + 21, x0 + 21, 512] = one3 * 0;
                        superimposed[y0 + 21, 440, 0, 512]           = one4 * 0;//中心から○μmの正方形以外は黒くする。

                        superimposed.ImWrite("C:\\set\\superimposed25_1.bmp");

                        using (CvMemStorage storage = new CvMemStorage())
                        {
                            using (CvContourScanner scanner = new CvContourScanner(superimposed.ToIplImage(), storage, CvContour.SizeOf, ContourRetrieval.Tree, ContourChain.ApproxSimple))
                            {
                                foreach (CvSeq <CvPoint> c in scanner)
                                {
                                    CvMoments mom = new CvMoments(c, false);
                                    if (c.ElemSize < 2)
                                    {
                                        continue;
                                    }
                                    if (mom.M00 == 0.0)
                                    {
                                        continue;
                                    }
                                    double mx   = mom.M10 / mom.M00;
                                    double my   = mom.M01 / mom.M00;
                                    mm     koko = new mm();
                                    koko.white_x    = mx;
                                    koko.white_y    = my;
                                    koko.white_kido = mom.M00;
                                    koko.white_dx   = delta_xx;
                                    koko.white_dy   = delta_yy;
                                    white_area.Add(koko);
                                    stage.WriteLine(String.Format("mx={0:f2} , my={1:f2} , dx={2:f2} , dy={3:f2} , M={4:f2}", mx, my, delta_xx, delta_yy, mom.M00));
                                }
                            }
                        }
                        Part_img.Clear();
                    } //pixel移動x
                }     //pixel移動y
            }
            if (white_area.Count > 0)
            {
                double center_x  = 0;
                double center_y  = 0;
                double center_dx = 0;
                double center_dy = 0;
                double kido_sum  = 0;
                for (int i = 0; i < white_area.Count; i++)
                {
                    kido_sum  += white_area[i].white_kido;
                    center_x  += white_area[i].white_x * white_area[i].white_kido;
                    center_y  += white_area[i].white_y * white_area[i].white_kido;
                    center_dx += white_area[i].white_dx * white_area[i].white_kido;
                    center_dy += white_area[i].white_dy * white_area[i].white_kido;
                }
                center_x  = center_x / kido_sum;
                center_y  = center_y / kido_sum;
                center_dx = center_dx / kido_sum;
                center_dy = center_dy / kido_sum;

                int c_o_g_x;
                int c_o_g_y;
                if (center_x >= 0)
                {
                    c_o_g_x = (int)(center_x + 0.5);
                }
                else
                {
                    c_o_g_x = (int)(center_x - 0.5);
                }

                if (center_x >= 0)
                {
                    c_o_g_y = (int)(center_y + 0.5);
                }
                else
                {
                    c_o_g_y = (int)(center_y - 0.5);
                }

                int dx_pixel = c_o_g_x - x0;
                int dy_pixel = c_o_g_y - y0;

                double dx_micron = dx_pixel * 0.265625 / 1000;
                double dy_micron = dy_pixel * 0.265625 / 1000;

                double now_x2 = mc.GetPoint().X;
                double now_y2 = mc.GetPoint().Y;
                mc.MovePointXY(now_x2 - dx_micron, now_y2 + dy_micron);//pixelの軸とstageの軸の関係から
                mc.Join();

                over_dx = center_dx;
                over_dy = center_dy;
            }
        }
예제 #11
0
        private void BeamFollower(Track myTrack, int mod, int pl, bool dubflag)
        {
            MotorControler mc      = MotorControler.GetInstance(parameterManager);
            Camera         camera  = Camera.GetInstance();
            Surface        surface = Surface.GetInstance(parameterManager);
            Vector3        initial = mc.GetPoint();

            string uptxt = string.Format(@"c:\GTR_test\bpm\{0}_{1}_up.txt", 10, 10);

            BeamDetection(uptxt, true);
            List <Point2d> BPM_pix = new List <Point2d>();//beamのピクセル座標を格納するListである。

            string line;

            System.IO.StreamReader file = new System.IO.StreamReader(uptxt);
            while ((line = file.ReadLine()) != null)
            {
                string[] data = line.Split(' ');
                double   sx   = double.Parse(data[0]);
                double   sy   = double.Parse(data[1]);
                BPM_pix.Add(new Point2d(sx, sy));
            }
            file.Close();

            //ここからは、画像の中心(256,220)に近いいくつかのbeamの座標を選択し、それを追跡するように修正する。
            List <Point2d> some_bpm = new List <Point2d>();
            List <Point3d> point    = new List <Point3d>();
            List <Point2d> point_10 = new List <Point2d>();

            for (int i = 0; i < BPM_pix.Count(); i++)
            {
                List <Point3d> point2 = new List <Point3d>();
                for (int r = 0; r < BPM_pix.Count(); r++)
                {
                    Point3d p  = new Point3d();
                    double  dx = BPM_pix[i].X - BPM_pix[r].X;
                    double  dy = BPM_pix[i].Y - BPM_pix[r].Y;
                    double  dr = Math.Sqrt(dx * dx + dy * dy);

                    if (dr < 7)
                    {//この7という数字は、windowsizeが一辺10ピクセルのため、全体で見た時に7ピクセル離れていれば良いだろうと判断し、このようにした。
                        p.X = 10;
                        p.Y = 10;
                        p.Z = 10;
                        point2.Add(p);
                    }
                }//for r

                if (point2.Count() == 1)
                {
                    Point2d bem = new Point2d();
                    bem.X = BPM_pix[i].X;
                    bem.Y = BPM_pix[i].Y;
                    point_10.Add(bem);
                }
            }//for i

            //ここまで
            int bemcount = 0;

            if (point_10.Count() >= 5)
            {
                bemcount = 5;
            }
            else
            {
                bemcount = point_10.Count();
            }

            for (int i = 0; i < bemcount; i++)//ここで、領域における分け方も含めてbeamを選択できるようにする。
            {
                some_bpm.Add(new Point2d(point_10[i].X, point_10[i].Y));
            }
            //ただ、とりあえずこれで作動はするであろう形になった。

            List <List <pozi> > LBeam     = new List <List <pozi> >();
            List <List <pozi> > LBeam_Low = new List <List <pozi> >();

            List <pozi>    c2        = new List <pozi>();
            List <Point2d> PM_result = some_bpm;//パターンマッチの結果から取得したbeamのpixel座標

            for (int a = 0; a < PM_result.Count(); a++)
            {
                pozi c3 = new pozi();
                c3.img.X = initial.X;
                c3.img.Y = initial.Y;
                c3.img.Z = initial.Z;

                c3.peak = PM_result[a];

                double firstx = c3.img.X - (c3.peak.X - 256) * 0.000267;
                double firsty = c3.img.Y + (c3.peak.Y - 220) * 0.000267;
                double firstz = c3.img.Z;

                c3.stage = new Point3d(firstx, firsty, firstz);

                c2.Add(c3);
            }
            LBeam.Add(c2);//第一層目でのbeamの情報をぶち込む。

            //for up layer

            int number_of_images = 7;
            int hits             = 4;

            double Sh    = 0.5 / (surface.UpTop - surface.UpBottom);
            double theta = Math.Atan(0);

            double dz;
            double dz_price_img = (6 * Math.Cos(theta) / Sh) / 1000;
            double dz_img       = dz_price_img * (-1);

            string datarootdirpath = string.Format(@"C:\GTR_test\{0}", myTrack.IdString);//Open forder to store track information

            System.IO.DirectoryInfo mydir = System.IO.Directory.CreateDirectory(datarootdirpath);

            List <OpenCvSharp.CPlusPlus.Point2d> Msdxdy = new List <OpenCvSharp.CPlusPlus.Point2d>();  //stage移動のための角度を入れるList。

            Msdxdy.Add(new OpenCvSharp.CPlusPlus.Point2d(0.0, 0.0));                                   //最初はbeamが垂直に原子核乾板に照射されていると考えて(0.0,0.0)を入れた。

            List <OpenCvSharp.CPlusPlus.Point3d> LStage  = new List <OpenCvSharp.CPlusPlus.Point3d>(); //ImageTakeingで撮影した画像の最後の画像の座標を入れるList。
            List <OpenCvSharp.CPlusPlus.Point3d> LCenter = new List <OpenCvSharp.CPlusPlus.Point3d>(); //検出したbemaのずれから算出した、本来の画像の中心点のstage座標。

            List <List <ImageTaking> > UpTrackInfo = new List <List <ImageTaking> >();

            //for down layer................................................................
            //            tl.Rec("down");
            double Sh_low;

            Sh_low = 0.5 / (surface.LowTop - surface.LowBottom);

            List <OpenCvSharp.CPlusPlus.Point2d> Msdxdy_Low = new List <OpenCvSharp.CPlusPlus.Point2d>();
            List <OpenCvSharp.CPlusPlus.Point3d> LStage_Low = new List <OpenCvSharp.CPlusPlus.Point3d>();

            List <OpenCvSharp.CPlusPlus.Point3d> LCenter_Low = new List <OpenCvSharp.CPlusPlus.Point3d>();//検出したbemaのずれから算出した、本来の画像の中心点のstage座標。

            List <List <ImageTaking> > LowTrackInfo = new List <List <ImageTaking> >();


            dz_price_img = (6 * Math.Cos(theta) / Sh) / 1000;
            dz_img       = dz_price_img * (-1);
            dz           = dz_img;
            int gotobase   = 0;
            int not_detect = 0;

            for (int i = 0; gotobase < 1; i++)
            {
                Vector3 initialpos = mc.GetPoint();
                double  moverange  = (number_of_images - 1) * dz_img;
                double  predpoint  = moverange + initialpos.Z;

                if (predpoint < surface.UpBottom)
                {
                    gotobase = 1;

                    dz = surface.UpBottom - initialpos.Z + (number_of_images - 1) * dz_price_img;
                }

                //gotobase = 1のときは、移動して画像を撮影するようにする。
                if (i != 0)//このままでOK
                {
                    Vector3 dstpoint = new Vector3(
                        LCenter[i - 1].X + Msdxdy[i].X * dz * Sh,
                        LCenter[i - 1].Y + Msdxdy[i].Y * dz * Sh,
                        LCenter[i - 1].Z + dz
                        );
                    mc.MovePoint(dstpoint);
                    mc.Join();
                }

                List <ImageTaking> LiITUpMid = TakeSequentialImage( //image taking
                    Msdxdy[i].X * Sh,                               //Dx
                    Msdxdy[i].Y * Sh,                               //Dy
                    dz_img,                                         //Dz
                    number_of_images);                              //number of images


                LStage.Add(new OpenCvSharp.CPlusPlus.Point3d(
                               LiITUpMid[number_of_images - 1].StageCoord.X,
                               LiITUpMid[number_of_images - 1].StageCoord.Y,
                               LiITUpMid[number_of_images - 1].StageCoord.Z
                               ));                                                                                     //撮影した画像の最後の画像の座標を LStage に代入する。

                LiITUpMid[number_of_images - 1].img.ImWrite(datarootdirpath + string.Format(@"\img_l_up_{0}.png", i)); //最後の画像だけ別で保存。
                UpTrackInfo.Add(LiITUpMid);                                                                            //撮影した画像すべてを UpTrackInfo に代入。

                List <Mat> binimages = new List <Mat>();                                                               //撮影した画像に対して画像処理をかける。
                for (int t = 0; t <= number_of_images - 1; t++)
                {
                    Mat bin = (Mat)DogContrastBinalize(LiITUpMid[t].img, 31, 60);
                    Cv2.Dilate(bin, bin, new Mat());
                    binimages.Add(bin);
                }

                List <pozi>    beam_data   = new List <pozi>();    //各stepごとで検出したbeamそれぞれのデータを格納する。
                List <Point2d> MSDXDY_BEAM = new List <Point2d>(); //それぞれのbeamから算出した角度を格納するList。
                for (int r = 0; r < LBeam[0].Count(); r++)         //検出したbeamの数だけ処理を行うようにする。
                {
                    pozi beam_pozi = new pozi();
                    beam_pozi.img = LStage[i];//画像の撮影場所を格納する。

                    //trackを重ねる処理を入れる。
                    Point2d beam_peak = TrackDetection_verold(//シフトしないようにして、処理を行うようにしよう。
                        binimages,
                        (int)LBeam[i][r].peak.X,
                        (int)LBeam[i][r].peak.Y,
                        0,           //shiftx もともと 3
                        0,           //shifty もともと 3
                        4,
                        10,          //windowsize もともと 90
                        hits, true); // true);

                    if (beam_peak.X == -1 & beam_peak.Y == -1)
                    {//検出できなかった時にどのような処理を行うのかを考えたほうがいいだろうな。
                        mc.Join();
                        not_detect = 1;

                        //goto not_detect_track; とりあえずコメントアウトしておく
                    }

                    beam_pozi.peak.X = beam_peak.X;
                    beam_pozi.peak.Y = beam_peak.Y;

                    double firstx = beam_pozi.img.X - (beam_pozi.peak.X - 256) * 0.000267;
                    double firsty = beam_pozi.img.Y + (beam_pozi.peak.Y - 220) * 0.000267;
                    double firstz = beam_pozi.img.Z;

                    beam_pozi.stage = new Point3d(firstx, firsty, firstz);

                    beam_data.Add(beam_pozi);
                }//r_loop

                Point2d Ms_esti = new Point2d();
                double  Ms_x    = new double();
                double  Ms_y    = new double();

                int pix_dX = new int();
                int pix_dY = new int();

                LBeam.Add(beam_data);

                for (int k = 0; k < LBeam[i].Count(); k++)
                {
                    if (i == 0)
                    {
                        MSDXDY_BEAM.Add(new OpenCvSharp.CPlusPlus.Point2d(Msdxdy[i].X, Msdxdy[i].Y));
                    }
                    else
                    {
                        if (i == 1)
                        {
                            Point3d LTrack_ghost = new Point3d();
                            double  dzPrev       = (LBeam[i][k].stage.Z - surface.UpTop) * Sh;
                            double  Lghost_x     = LBeam[i][k].stage.X - Msdxdy[i].X * dzPrev;
                            double  Lghost_y     = LBeam[i][k].stage.Y - Msdxdy[i].Y * dzPrev;
                            LTrack_ghost = new Point3d(Lghost_x, Lghost_y, surface.UpTop);                                                          //上側乳剤層上面にtrackがあるならどの位置にあるかを算出する。

                            OpenCvSharp.CPlusPlus.Point2d Tangle = ApproximateStraight(Sh, LTrack_ghost, LBeam[i][k].stage, LBeam[i + 1][k].stage); //ここを2点で行うようにする。
                            MSDXDY_BEAM.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle.X, Tangle.Y));
                        }
                        else
                        {
                            OpenCvSharp.CPlusPlus.Point2d Tangle = ApproximateStraight(Sh, LBeam[i - 1][k].stage, LBeam[i][k].stage, LBeam[i + 1][k].stage);
                            MSDXDY_BEAM.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle.X, Tangle.Y));
                        }
                    }
                }

                for (int q = 0; q < MSDXDY_BEAM.Count(); q++) //ここで個々のbeamの角度を平均してstageの移動角度を算出する。
                {
                    Ms_x += MSDXDY_BEAM[q].X;
                    Ms_y += MSDXDY_BEAM[q].Y;

                    //pix_dX += (int)LBeam[i][q - 1].peak.X - (int)LBeam[i][q].peak.X;//q - 1 がおかしい。ここで何をしたかったのかを思い出そう。
                    //pix_dY += (int)LBeam[i][q - 1].peak.Y - (int)LBeam[i][q].peak.Y;//予想した地点とのピクセルのズレかな?

                    pix_dX += (int)LBeam[i + 1][q].peak.X - (int)LBeam[i][q].peak.X;
                    pix_dY += (int)LBeam[i + 1][q].peak.Y - (int)LBeam[i][q].peak.Y;
                }
                Ms_x    = Ms_x / MSDXDY_BEAM.Count();
                Ms_y    = Ms_y / MSDXDY_BEAM.Count();
                Ms_esti = new Point2d(Ms_x, Ms_y);
                Msdxdy.Add(Ms_esti);//算出した角度をぶち込む。
                //LBeam.Add(beam_data);

                pix_dX = pix_dX / MSDXDY_BEAM.Count(); //ずれたピクセル量
                pix_dY = pix_dY / MSDXDY_BEAM.Count(); //ずれたピクセル量
                double cenX = LStage[i].X - pix_dX * 0.000267;
                double cenY = LStage[i].Y + pix_dY * 0.000267;
                double cenZ = LStage[i].Z;

                LCenter.Add(new Point3d(cenX, cenY, cenZ));//検出したそれぞれのbeamのズレから算出したパターンマッチの際の中心座標(stage)。
            }//for i-loop

            //baseまたぎ
            int lcen_counter   = LCenter.Count();
            int msdxdy_counter = Msdxdy.Count();

            mc.MovePoint(
                LCenter[lcen_counter - 1].X + Msdxdy[msdxdy_counter - 1].X * (surface.LowTop - surface.UpBottom),
                LCenter[lcen_counter - 1].Y + Msdxdy[msdxdy_counter - 1].Y * (surface.LowTop - surface.UpBottom),
                surface.LowTop
                );
            mc.Join();

            //////ここから下gelの処理
            Msdxdy_Low.Add(new OpenCvSharp.CPlusPlus.Point2d(Msdxdy[msdxdy_counter - 1].X, Msdxdy[msdxdy_counter - 1].Y));
            int lbeam_counter = LBeam.Count();

            LBeam_Low.Add(LBeam[lbeam_counter - 1]);

            //今までのtrack追跡プログラムとは異なる角度等の使い方をする。
            dz_price_img = (6 * Math.Cos(theta) / Sh_low) / 1000;
            dz_img       = dz_price_img * (-1);
            dz           = dz_img;

            int goto_dgel = 0;

            for (int i = 0; goto_dgel < 1; i++)
            {
                ///////移動して画像処理をしたときに、下gelの下に入らないようにする。
                Vector3 initialpos = mc.GetPoint();
                double  moverange  = (number_of_images - 1) * dz_img;
                double  predpoint  = moverange + initialpos.Z;

                if (predpoint < surface.LowBottom)//もしもbaseに入りそうなら、8枚目の画像がちょうど下gelを撮影するようにdzを調整する。
                {
                    goto_dgel = 1;

                    dz = surface.LowBottom - initialpos.Z + (number_of_images - 1) * dz_price_img;
                }
                ////////

                //goto_dgel == 1のときは、移動して画像を撮影するようにする。
                if (i != 0)
                {
                    Vector3 dstpoint = new Vector3(
                        LCenter_Low[i - 1].X + Msdxdy_Low[i].X * dz * Sh_low,
                        LCenter_Low[i - 1].Y + Msdxdy_Low[i].Y * dz * Sh_low,
                        LCenter_Low[i - 1].Z + dz
                        );
                    mc.MovePoint(dstpoint);
                    mc.Join();
                }

                //今までのtrack追跡プログラムとは異なる角度等の使い方をする。
                List <ImageTaking> LiITLowMid = TakeSequentialImage(
                    Msdxdy_Low[i].X * Sh_low,
                    Msdxdy_Low[i].Y * Sh_low,
                    dz_img,
                    number_of_images);

                //画像・座標の記録
                LStage_Low.Add(new OpenCvSharp.CPlusPlus.Point3d(
                                   LiITLowMid[number_of_images - 1].StageCoord.X,
                                   LiITLowMid[number_of_images - 1].StageCoord.Y,
                                   LiITLowMid[number_of_images - 1].StageCoord.Z));

                LiITLowMid[number_of_images - 1].img.ImWrite(datarootdirpath + string.Format(@"\img_l_low_{0}.png", i));

                LowTrackInfo.Add(LiITLowMid);//撮影した8枚の画像と、撮影した位置を記録する。

                //撮影した画像をここで処理する。
                List <Mat> binimages = new List <Mat>();
                for (int t = 0; t <= number_of_images - 1; t++)
                {
                    Mat bin = (Mat)DogContrastBinalize(LiITLowMid[t].img, 31, 60);
                    Cv2.Dilate(bin, bin, new Mat());
                    binimages.Add(bin);
                }

                List <pozi>    beam_data_low   = new List <pozi>();    //各stepごとで検出したbeamそれぞれのデータを格納する。
                List <Point2d> MSDXDY_BEAM_LOW = new List <Point2d>(); //それぞれのbeamから算出した角度を格納するList。
                for (int r = 0; r < LBeam_Low[0].Count(); r++)
                {
                    pozi beam_pozi = new pozi();
                    beam_pozi.img = LStage_Low[i];//画像の撮影場所を格納する。

                    //trackを重ねる処理を入れる。
                    Point2d beam_peak = TrackDetection_verold(
                        binimages,
                        (int)LBeam_Low[i][r].peak.X,
                        (int)LBeam_Low[i][r].peak.Y,
                        0,
                        0,
                        4,
                        10,
                        hits);// true);

                    if (beam_peak.X == -1 & beam_peak.Y == -1)
                    {
                        mc.Join();
                        not_detect = 1;

                        //goto not_detect_track; とりあえずコメントアウトしておく
                    }

                    beam_pozi.peak.X = beam_peak.X;
                    beam_pozi.peak.Y = beam_peak.Y;

                    double firstx = beam_pozi.img.X - (beam_pozi.peak.X - 256) * 0.000267;
                    double firsty = beam_pozi.img.Y + (beam_pozi.peak.Y - 220) * 0.000267;
                    double firstz = beam_pozi.img.Z;

                    beam_pozi.stage = new Point3d(firstx, firsty, firstz);

                    beam_data_low.Add(beam_pozi);
                }//r_loop

                Point2d Ms_esti = new Point2d();
                double  Ms_x    = new double();
                double  Ms_y    = new double();

                int pix_dX = new int();
                int pix_dY = new int();

                LBeam_Low.Add(beam_data_low);
                for (int k = 0; k < LBeam_Low[i].Count(); k++)
                {
                    if (i == 0)
                    {
                        OpenCvSharp.CPlusPlus.Point2d Tangle_l = ApproximateStraightBase(
                            Sh,
                            Sh_low,
                            LBeam[lbeam_counter - 2][k].stage,
                            LBeam[lbeam_counter - 1][k].stage,
                            LBeam_Low[i][k].stage,
                            surface);
                        MSDXDY_BEAM_LOW.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle_l.X, Tangle_l.Y));
                    }
                    else if (i == 1)
                    {
                        OpenCvSharp.CPlusPlus.Point2d Tangle_l = ApproximateStraightBase(
                            Sh,
                            Sh_low,
                            LBeam[lbeam_counter - 1][k].stage,
                            LBeam_Low[i - 1][k].stage,
                            LBeam_Low[i][k].stage,
                            surface);
                        MSDXDY_BEAM_LOW.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle_l.X, Tangle_l.Y));
                    }
                    else
                    {
                        OpenCvSharp.CPlusPlus.Point2d Tangle_l = ApproximateStraight(
                            Sh_low,
                            LBeam_Low[i - 2][k].stage,
                            LBeam_Low[i - 1][k].stage,
                            LBeam_Low[i][k].stage);
                        MSDXDY_BEAM_LOW.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle_l.X, Tangle_l.Y));
                    }
                }//roop_k

                for (int q = 0; q < MSDXDY_BEAM_LOW.Count(); q++) //ここで個々のbeamの角度を平均してstageの移動角度を算出する。
                {
                    Ms_x += MSDXDY_BEAM_LOW[q].X;
                    Ms_y += MSDXDY_BEAM_LOW[q].Y;

                    pix_dX += (int)LBeam_Low[i + 1][q].peak.X - (int)LBeam_Low[i][q].peak.X;
                    pix_dY += (int)LBeam_Low[i + 1][q].peak.Y - (int)LBeam_Low[i][q].peak.Y;
                }

                Ms_x    = Ms_x / MSDXDY_BEAM_LOW.Count();
                Ms_y    = Ms_y / MSDXDY_BEAM_LOW.Count();
                Ms_esti = new Point2d(Ms_x, Ms_y);
                Msdxdy_Low.Add(Ms_esti);//算出した角度をぶち込む。


                pix_dX = pix_dX / MSDXDY_BEAM_LOW.Count(); //ずれたピクセル量
                pix_dY = pix_dY / MSDXDY_BEAM_LOW.Count(); //ずれたピクセル量
                double cenX = LStage_Low[i].X - pix_dX * 0.000267;
                double cenY = LStage_Low[i].Y + pix_dY * 0.000267;
                double cenZ = LStage_Low[i].Z;

                LCenter_Low.Add(new Point3d(cenX, cenY, cenZ));//検出したそれぞれのbeamのズレから算出したパターンマッチの際の中心座標(stage)。
            }//i_loop

            //
            int lcen_low_count = LCenter_Low.Count();

            mc.MovePointXY(LCenter_Low[lcen_low_count - 1].X, LCenter_Low[lcen_low_count - 1].Y);

            mc.Join();

            //検出に失敗した場合は、ループを抜けてここに来る。

            //file write out up_gel
            string       txtfileName_sh_up = datarootdirpath + string.Format(@"\Sh_up.txt");
            StreamWriter twriter_sh_up     = File.CreateText(txtfileName_sh_up);

            twriter_sh_up.WriteLine("{0}", Sh);
            twriter_sh_up.Close();

            //file write out
            string       txtfileName_t_info_up = datarootdirpath + string.Format(@"\location_up.txt");
            StreamWriter twriter_t_info_up     = File.CreateText(txtfileName_t_info_up);

            for (int i = 0; i < UpTrackInfo.Count; i++)
            {
                for (int t = 0; t < UpTrackInfo[i].Count; t++)
                {
                    UpTrackInfo[i][t].img.ImWrite(datarootdirpath + string.Format(@"\img_t_info_up_{0}-{1}.png", i, t));
                    Vector3 p = UpTrackInfo[i][t].StageCoord;
                    twriter_t_info_up.WriteLine("{0} {1} {2} {3} {4}", i, t, p.X, p.Y, p.Z);
                }
            }
            twriter_t_info_up.Close();

            string       txtfileName_lbeam = datarootdirpath + string.Format(@"\lbeam_up.txt");
            StreamWriter twriter_lbeam     = File.CreateText(txtfileName_lbeam);

            for (int i = 0; i < LBeam.Count(); i++)
            {
                for (int r = 0; r < LBeam[i].Count(); r++)
                {
                    twriter_lbeam.WriteLine("{0} {1} BeamPeak: {2} {3} LBeam(Stage): {4} {5} {6}",
                                            i,
                                            r,
                                            LBeam[i][r].peak.X,
                                            LBeam[i][r].peak.Y,
                                            LBeam[i][r].stage.X,
                                            LBeam[i][r].stage.Y,
                                            LBeam[i][r].stage.Z);
                }
            }
            twriter_lbeam.Close();

            string       txtfileName_LCenter = datarootdirpath + string.Format(@"\LCenter_up.txt");
            StreamWriter twriter_LCenter     = File.CreateText(txtfileName_LCenter);

            for (int i = 0; i < LCenter.Count(); i++)
            {
                twriter_LCenter.WriteLine("{0} {1} {2}", LCenter[i].X, LCenter[i].Y, LCenter[i].Z);
            }
            twriter_LCenter.Close();

            string       txtfileName_msdxdy = datarootdirpath + string.Format(@"\msdxdy.txt");
            StreamWriter twriter_msdxdy     = File.CreateText(txtfileName_msdxdy);

            for (int i = 0; i < Msdxdy.Count(); i++)
            {
                OpenCvSharp.CPlusPlus.Point2d p = Msdxdy[i];
                twriter_msdxdy.WriteLine("{0} {1} {2}", i, p.X, p.Y);
            }
            twriter_msdxdy.Close();

            //file write out low_gel
            string       txtfileName_sh_low = datarootdirpath + string.Format(@"\Sh_low.txt");
            StreamWriter twriter_sh_low     = File.CreateText(txtfileName_sh_low);

            twriter_sh_low.WriteLine("{0}", Sh_low);
            twriter_sh_low.Close();

            string       txtfileName_t_info_low = datarootdirpath + string.Format(@"\location_low.txt");
            StreamWriter twriter_t_info_low     = File.CreateText(txtfileName_t_info_low);

            for (int i = 0; i < LowTrackInfo.Count; i++)
            {
                for (int t = 0; t < LowTrackInfo[i].Count; t++)
                {
                    LowTrackInfo[i][t].img.ImWrite(datarootdirpath + string.Format(@"\img_t_info_low_{0}-{1}.png", i, t));
                    Vector3 p = LowTrackInfo[i][t].StageCoord;
                    twriter_t_info_low.WriteLine("{0} {1} {2} {3} {4}", i, t, p.X, p.Y, p.Z);
                }
            }
            twriter_t_info_low.Close();


            string       txtfileName_lbeam_low = datarootdirpath + string.Format(@"\lbeam_low.txt");
            StreamWriter twriter_lbeam_low     = File.CreateText(txtfileName_lbeam_low);

            for (int i = 0; i < LBeam_Low.Count(); i++)
            {
                for (int r = 0; r < LBeam_Low[i].Count(); r++)
                {
                    twriter_lbeam_low.WriteLine("{0} {1} BeamPeak: {2} {3} LBeam(Stage): {4} {5} {6}",
                                                i,
                                                r,
                                                LBeam_Low[i][r].peak.X,
                                                LBeam_Low[i][r].peak.Y,
                                                LBeam_Low[i][r].stage.X,
                                                LBeam_Low[i][r].stage.Y,
                                                LBeam_Low[i][r].stage.Z);
                }
            }
            twriter_lbeam_low.Close();


            string       txtfileName_LCenter_low = datarootdirpath + string.Format(@"\LCenter_low.txt");
            StreamWriter twriter_LCenter_low     = File.CreateText(txtfileName_LCenter_low);

            for (int i = 0; i < LCenter_Low.Count(); i++)
            {
                twriter_LCenter_low.WriteLine("{0} {1} {2}", LCenter_Low[i].X, LCenter_Low[i].Y, LCenter_Low[i].Z);
            }
            twriter_LCenter_low.Close();

            string       txtfileName_msdxdy_low = datarootdirpath + string.Format(@"\msdxdy_low.txt");
            StreamWriter twriter_msdxdy_low     = File.CreateText(txtfileName_msdxdy_low);

            for (int i = 0; i < Msdxdy_Low.Count(); i++)
            {
                OpenCvSharp.CPlusPlus.Point2d p = Msdxdy_Low[i];
                twriter_msdxdy_low.WriteLine("{0} {1} {2}", i, p.X, p.Y);
            }
            twriter_msdxdy_low.Close();
        }
예제 #12
0
        }//BeamDetection

        private void task()
        {
            MotorControler mc           = MotorControler.GetInstance(parameterManager);
            Camera         camera       = Camera.GetInstance();
            TracksManager  tm           = parameterManager.TracksManager;
            Track          myTrack      = tm.GetTrack(tm.TrackingIndex);
            Surface        surface      = Surface.GetInstance(parameterManager);
            Stopwatch      sw           = new Stopwatch();
            CoordManager   coordManager = new CoordManager(parameterManager);


            //CoordManager coordManager;
            int mod = parameterManager.ModuleNo;
            int pl  = parameterManager.PlateNo;

            sw.Start();

            try
            {
                //MotorControler mc = MotorControler.GetInstance(parameterManager);
                GridMark nearestMark = coordManager.GetTheNearestGridMark(mc.GetPoint());
                System.Diagnostics.Debug.WriteLine(String.Format("{0},  {1}", nearestMark.x, nearestMark.y));
                mc.MovePointXY(nearestMark.x, nearestMark.y);
                mc.Join();
            }
            catch (GridMarkNotFoundException ex)
            {
                System.Diagnostics.Debug.WriteLine(String.Format("{0}", ex.ToString()));
            }
            try
            {
                // MotorControler mc = MotorControler.GetInstance(parameterManager);
                IGridMarkRecognizer GridMarkRecognizer = coordManager;
                mc.SetSpiralCenterPoint();
                Led     led          = Led.GetInstance();
                Vector2 encoderPoint = new Vector2(-1, -1);
                encoderPoint.X = mc.GetPoint().X;
                encoderPoint.Y = mc.GetPoint().Y;//おこられたのでしかたなくこうする 吉田20150427
                Vector2 viewerPoint = new Vector2(-1, -1);

                bool continueFlag = true;
                while (continueFlag)
                {
                    led.AdjustLight(parameterManager);
                    viewerPoint = GridMarkRecognizer.SearchGridMarkx50();
                    if (viewerPoint.X < 0 || viewerPoint.Y < 0)
                    {
                        System.Diagnostics.Debug.WriteLine(String.Format("grid mark not found"));
                        mc.MoveInSpiral(true);
                        mc.Join();
                        continueFlag = (mc.SpiralIndex < 30);
                    }
                    else
                    {
                        System.Diagnostics.Debug.WriteLine(String.Format("******** {0}  {1}", viewerPoint.X, viewerPoint.Y));
                        encoderPoint = coordManager.TransToEmulsionCoord(viewerPoint);
                        mc.MovePointXY(encoderPoint);
                        mc.Join();
                        continueFlag = false;
                    }
                } // while

                //重心検出と移動を2回繰り返して、グリッドマークを視野中心にもっていく
                mc.MovePointXY(encoderPoint);
                mc.Join();
                viewerPoint  = GridMarkRecognizer.SearchGridMarkx50();
                encoderPoint = coordManager.TransToEmulsionCoord(viewerPoint);
                mc.MovePointXY(encoderPoint);
                mc.Join();
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine("exception");
            }

            //.........Get value of HFDX and HFDY................//

            try
            {
                Vector3  CurrentCenterPoint = mc.GetPoint();
                GridMark nearestMark        = coordManager.GetTheNearestGridMark(CurrentCenterPoint);
                System.Diagnostics.Debug.WriteLine(String.Format("{0},  {1}", CurrentCenterPoint.X, CurrentCenterPoint.Y));
                coordManager.HFDX = CurrentCenterPoint.X - nearestMark.x;
                coordManager.HFDY = CurrentCenterPoint.Y - nearestMark.y;
            }
            catch (EntryPointNotFoundException ex)
            {
                MessageBox.Show("エントリポイントが見当たりません。 " + ex.Message);
                System.Diagnostics.Debug.WriteLine("エントリポイントが見当たりません。 " + ex.Message);
            }

            //....... Move to track place after revised with shift value.....//

            double dstx = myTrack.MsX + coordManager.HFDX;
            double dsty = myTrack.MsY + coordManager.HFDY;

            mc.MovePointXY(dstx, dsty, delegate {
                //stage.WriteLine(Properties.Strings.MovingComplete);
            });

            mc.Join();

            Led led_ = Led.GetInstance();

            led_.AdjustLight(parameterManager);
            Thread.Sleep(500); //Wait for 5s

            //////////////////////////////Surfacerecog/////////////////////////////////////.................................../////////

            try
            {
                if (mc.IsMoving)
                {
                    MessageBoxResult r = MessageBox.Show(
                        Properties.Strings.SurfaceException01,
                        Properties.Strings.Abort + "?",
                        MessageBoxButton.YesNo);
                    if (r == MessageBoxResult.Yes)
                    {
                        mc.AbortMoving();
                    }
                    else
                    {
                        return;
                    }
                }

                //Surface surface = Surface.GetInstance(parameterManager);
                if (surface.IsActive)
                {
                    MessageBoxResult r = MessageBox.Show(
                        Properties.Strings.SurfaceException02,
                        Properties.Strings.Abort + "?",
                        MessageBoxButton.YesNo);
                    if (r == MessageBoxResult.Yes)
                    {
                        surface.Abort();
                    }
                    else
                    {
                        return;
                    }
                }

                try
                {
                    surface.Start(true);
                }
                catch (Exception ex)
                {
                    MessageBox.Show(ex.Message, Properties.Strings.Error);
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message, Properties.Strings.Error);
            }
            mc.Join();

            //...............Beam Pattern Matching..................////////////////

            try
            {
                string datarootdirpath        = string.Format(@"C:\test\bpm\{0}", mod);
                System.IO.DirectoryInfo mydir = System.IO.Directory.CreateDirectory(datarootdirpath);
                string[] sp    = myTrack.IdString.Split('-');
                string   uptxt = string.Format(@"c:\test\bpm\{0}\{1}-{2}-{3}-{4}_up.txt", mod, mod, pl, sp[0], sp[1]);
                string   dwtxt = string.Format(@"c:\test\bpm\{0}\{1}-{2}-{3}-{4}_dw.txt", mod, mod, pl - 1, sp[0], sp[1]);
                BeamDetection(uptxt, true);

                BeamPatternMatch bpm = new BeamPatternMatch(8, 200);
                bpm.ReadTrackDataTxtFile(dwtxt, false);

                bpm.ReadTrackDataTxtFile(uptxt, true);
                bpm.DoPatternMatch();

                //stage.WriteLine(String.Format("pattern match dx,dy = {0}, {1}", bpm.GetPeakX() * 0.2625 * 0.001, bpm.GetPeakY() * 0.2625 * 0.001));
                Vector3 BfPoint = mc.GetPoint();
                mc.MoveDistance(bpm.GetPeakX() * 0.2625 * 0.001, VectorId.X);
                mc.Join();
                mc.MoveDistance(-bpm.GetPeakY() * 0.2625 * 0.001, VectorId.Y);
                mc.Join();
                Led led = Led.GetInstance();
                led.AdjustLight(parameterManager);
                Vector3 AfPoint = mc.GetPoint();
                //stage.WriteLine(String.Format("Move dx,dy = {0}, {1}", BfPoint.X - AfPoint.X, BfPoint.Y - AfPoint.Y));
            }
            catch (ArgumentOutOfRangeException)
            {
                MessageBox.Show("ID numver is not existed。 ");
            }
            catch (System.Exception)
            {
                MessageBox.Show("No beam battern。 ");
            }
            // }
            /////////////////////////////SurfaceRecog////////////////////////////////////////////////////////////////////////////

            Vector3 initial = mc.GetPoint();///////initialpoint

            double Sh = 0.5 / (surface.UpTop - surface.UpBottom);

            //ここから角度によって撮影間隔を変更するように書き換える。
            double tansi = Math.Sqrt(myTrack.MsDX * myTrack.MsDX + myTrack.MsDY * myTrack.MsDY);
            double theta = Math.Atan(tansi);
            //絶対値の大きさを入れる。dzはマイナスの値になるようにする。
            double dz;

            double dz_price_img = (6 * Math.Cos(theta) / Sh) / 1000;
            double dz_img       = dz_price_img * (-1);
            //
            string datarootdirpathw = string.Format(@"C:\test\{0}", myTrack.IdString);

            System.IO.DirectoryInfo mydir_ = System.IO.Directory.CreateDirectory(datarootdirpathw);
            //

            //必要なlistをまとめる
            //List<ImageTaking> LiITUpTrack = new List<ImageTaking>();

            List <OpenCvSharp.CPlusPlus.Point2d> Msdxdy = new List <OpenCvSharp.CPlusPlus.Point2d>();

            Msdxdy.Add(new OpenCvSharp.CPlusPlus.Point2d(myTrack.MsDX, myTrack.MsDY));

            List <OpenCvSharp.CPlusPlus.Point3d> LStage = new List <OpenCvSharp.CPlusPlus.Point3d>();
            List <OpenCvSharp.CPlusPlus.Point>   LPeak  = new List <OpenCvSharp.CPlusPlus.Point>(); //i番目の画像で実際に見つかったトラックの座標。pixel座標で視野中心からの差分。
            List <Point3d> LTrack = new List <Point3d>();                                           //i番目の画像で実際に見つかったトラックの座標のステージ座標

            List <List <ImageTaking> > UpTrackInfo = new List <List <ImageTaking> >();

            //エラー防止のために下ゲルの処理の際に必要なListをここに移動した。
            double Sh_low;

            Sh_low = 0.5 / (surface.LowTop - surface.LowBottom);

            List <OpenCvSharp.CPlusPlus.Point2d> Msdxdy_Low = new List <OpenCvSharp.CPlusPlus.Point2d>();

            //List<ImageTaking> LiITLowMid = new List<ImageTaking>();
            List <OpenCvSharp.CPlusPlus.Point3d> LStage_Low = new List <OpenCvSharp.CPlusPlus.Point3d>();
            List <OpenCvSharp.CPlusPlus.Point>   LPeak_Low  = new List <OpenCvSharp.CPlusPlus.Point>(); //i番目の画像で実際に見つかったトラックの座標。pixel座標で視野中心からの差分。
            List <Point3d>             LTrack_Low           = new List <Point3d>();                     //i番目の画像で実際に見つかったトラックの座標のステージ座標
            List <List <ImageTaking> > LowTrackInfo         = new List <List <ImageTaking> >();


            //今までのtrack追跡プログラムとは異なる角度等の使い方をする。
            dz_price_img = (6 * Math.Cos(theta) / Sh) / 1000;
            dz_img       = dz_price_img * (-1);
            dz           = dz_img;

            int gotobase   = 0;
            int not_detect = 0;

            for (int i = 0; gotobase < 1; i++)
            {
                ///////移動して画像処理をしたときに、baseの中に入らないようにする。
                Vector3 initialpos = mc.GetPoint();
                double  moverange  = 7 * dz_img;
                double  predpoint  = moverange + initialpos.Z;

                if (predpoint < surface.UpBottom)//もしもbaseに入りそうなら、8枚目の画像がちょうどbaseを撮影するようにdzを調整する。
                {
                    gotobase = 1;

                    dz = surface.UpBottom - initialpos.Z + 7 * dz_price_img;
                }
                ////////

                //gotobase = 1のときは、移動して画像を撮影するようにする。
                if (i != 0)
                {
                    Vector3 dstpoint = new Vector3(
                        LTrack[i - 1].X + Msdxdy[i].X * dz * Sh,
                        LTrack[i - 1].Y + Msdxdy[i].Y * dz * Sh,
                        LTrack[i - 1].Z + dz
                        );
                    mc.MovePoint(dstpoint);
                    mc.Join();
                }

                List <ImageTaking> LiITUpMid = TakeSequentialImage(
                    Msdxdy[i].X * Sh,
                    Msdxdy[i].Y * Sh,
                    dz_img,
                    8);

                ////画像の保存、座標の保存。
                LStage.Add(new OpenCvSharp.CPlusPlus.Point3d(LiITUpMid[7].StageCoord.X, LiITUpMid[7].StageCoord.Y, LiITUpMid[7].StageCoord.Z));
                LiITUpMid[7].img.ImWrite(datarootdirpathw + string.Format(@"\img_l_up_{0}.bmp", i));

                UpTrackInfo.Add(LiITUpMid);//撮影した8枚の画像と、撮影した位置を記録する。

                //撮影した画像をここで処理する。
                List <Mat> binimages = new List <Mat>();
                for (int t = 0; t <= 7; t++)
                {
                    Mat bin = (Mat)DogContrastBinalize(LiITUpMid[t].img);

                    double xx = myTrack.MsDX * myTrack.MsDX;
                    double yy = myTrack.MsDY * myTrack.MsDY;
                    if (Math.Sqrt(xx + yy) >= 0.4)
                    {
                        Cv2.Dilate(bin, bin, new Mat());
                    }
                    Cv2.Dilate(bin, bin, new Mat());
                    binimages.Add(bin);
                }

                //trackを重ねる処理を入れる。
                Point2d pixel_cen = TrackDetection(binimages, 256, 220, 3, 3, 4, 90, 3);//画像の8枚目におけるtrackのpixel座標を算出する。

                if (pixel_cen.X == -1 & pixel_cen.Y == -1)
                {
                    //追跡に失敗した時に最後に検出したtrack座標に移動してから、追跡に失敗した地点の画像を撮影するようにする。
                    //mc.MovePoint(LTrack[i - 1].X, LTrack[i - 1].Y, LTrack[i - 1].Z);
                    mc.Join();

                    not_detect = 1;
                    goto not_detect_track;
                }

                //検出したpixel座標をstage座標に変換するなどlistに追加する。

                LPeak.Add(new OpenCvSharp.CPlusPlus.Point(pixel_cen.X - 256, pixel_cen.Y - 220));

                double firstx = LStage[i].X - LPeak[i].X * 0.000267;
                double firsty = LStage[i].Y + LPeak[i].Y * 0.000267;
                double firstz = LStage[i].Z;
                LTrack.Add(new Point3d(firstx, firsty, firstz));
                //

                //上側乳剤層上面の1回目のtrack検出によって、次のtrackの位置を検出する角度を求める。
                //その角度が、1回目のtrack検出の結果によって大きな角度にならないように調整をする。

                if (i == 0)
                {
                    Msdxdy.Add(new OpenCvSharp.CPlusPlus.Point2d(myTrack.MsDX, myTrack.MsDY));
                }
                else if (i == 1)
                {
                    List <Point3d> LTrack_ghost = new List <Point3d>();
                    double         dzPrev       = (LStage[0].Z - surface.UpTop) * Sh;
                    double         Lghost_x     = LTrack[0].X - Msdxdy[i].X * dzPrev;
                    double         Lghost_y     = LTrack[0].Y - Msdxdy[i].Y * dzPrev;
                    LTrack_ghost.Add(new Point3d(Lghost_x, Lghost_y, surface.UpTop));//上側乳剤層上面にtrackがあるならどの位置にあるかを算出する。

                    string       txtfileName_ltrackghost = datarootdirpathw + string.Format(@"\LTrack_ghost.txt");
                    StreamWriter twriter_ltrackghost     = File.CreateText(txtfileName_ltrackghost);
                    twriter_ltrackghost.WriteLine("{0} {1} {2}", LTrack_ghost[0].X, LTrack_ghost[0].Y, LTrack_ghost[0].Z);
                    twriter_ltrackghost.Close();

                    OpenCvSharp.CPlusPlus.Point2d Tangle = ApproximateStraight(Sh, LTrack_ghost[0], LTrack[0], LTrack[1]);
                    Msdxdy.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle.X, Tangle.Y));
                }
                else
                {
                    OpenCvSharp.CPlusPlus.Point2d Tangle = ApproximateStraight(Sh, LTrack[i - 2], LTrack[i - 1], LTrack[i]);
                    Msdxdy.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle.X, Tangle.Y));
                }
            }//for i-loop

            //baseまたぎ
            int ltrack_counter = LTrack.Count();
            int msdxdy_counter = Msdxdy.Count();

            mc.MovePoint(
                LTrack[ltrack_counter - 1].X + Msdxdy[msdxdy_counter - 1].X * (surface.LowTop - surface.UpBottom),
                LTrack[ltrack_counter - 1].Y + Msdxdy[msdxdy_counter - 1].Y * (surface.LowTop - surface.UpBottom),
                surface.LowTop
                );
            mc.Join();

            //////ここから下gelの処理
            Msdxdy_Low.Add(new OpenCvSharp.CPlusPlus.Point2d(Msdxdy[msdxdy_counter - 1].X, Msdxdy[msdxdy_counter - 1].Y));

            //今までのtrack追跡プログラムとは異なる角度等の使い方をする。
            dz_price_img = (6 * Math.Cos(theta) / Sh_low) / 1000;
            dz_img       = dz_price_img * (-1);
            dz           = dz_img;

            int goto_dgel = 0;

            for (int i = 0; goto_dgel < 1; i++)
            {
                ///////移動して画像処理をしたときに、下gelの下に入らないようにする。
                Vector3 initialpos = mc.GetPoint();
                double  moverange  = 7 * dz_img;
                double  predpoint  = moverange + initialpos.Z;

                if (predpoint < surface.LowBottom)//もしもbaseに入りそうなら、8枚目の画像がちょうど下gelを撮影するようにdzを調整する。
                {
                    goto_dgel = 1;

                    dz = surface.LowBottom - initialpos.Z + 7 * dz_price_img;
                }
                ////////

                //goto_dgel == 1のときは、移動して画像を撮影するようにする。
                if (i != 0)
                {
                    Vector3 dstpoint = new Vector3(
                        LTrack_Low[i - 1].X + Msdxdy_Low[i].X * dz * Sh_low,
                        LTrack_Low[i - 1].Y + Msdxdy_Low[i].Y * dz * Sh_low,
                        LTrack_Low[i - 1].Z + dz
                        );
                    mc.MovePoint(dstpoint);
                    mc.Join();
                }

                //今までのtrack追跡プログラムとは異なる角度等の使い方をする。
                List <ImageTaking> LiITLowMid = TakeSequentialImage(
                    Msdxdy[i].X * Sh_low,
                    Msdxdy[i].Y * Sh_low,
                    dz_img,
                    8);

                //画像・座標の記録
                LStage_Low.Add(new OpenCvSharp.CPlusPlus.Point3d(LiITLowMid[7].StageCoord.X, LiITLowMid[7].StageCoord.Y, LiITLowMid[7].StageCoord.Z));
                LiITLowMid[7].img.ImWrite(datarootdirpathw + string.Format(@"\img_l_low_{0}.bmp", i));

                LowTrackInfo.Add(LiITLowMid);//撮影した8枚の画像と、撮影した位置を記録する。

                //撮影した画像をここで処理する。
                List <Mat> binimages = new List <Mat>();
                for (int t = 0; t <= 7; t++)
                {
                    Mat bin = (Mat)DogContrastBinalize(LiITLowMid[t].img);

                    double xx = myTrack.MsDX * myTrack.MsDX;
                    double yy = myTrack.MsDY * myTrack.MsDY;
                    if (Math.Sqrt(xx + yy) >= 0.4)
                    {
                        Cv2.Dilate(bin, bin, new Mat());
                    }
                    Cv2.Dilate(bin, bin, new Mat());
                    binimages.Add(bin);
                }
                //trackを重ねる処理を入れる。
                Point2d pixel_cen = TrackDetection(binimages, 256, 220, 3, 3, 4, 90, 3);//画像の8枚目におけるtrackのpixel座標を算出する。

                //もし検出に失敗した場合はループを抜ける。
                if (pixel_cen.X == -1 & pixel_cen.Y == -1)
                {
                    //mc.MovePoint(LTrack_Low[i - 1].X, LTrack_Low[i - 1].Y, LTrack_Low[i - 1].Z);
                    mc.Join();

                    not_detect = 1;
                    goto not_detect_track;
                }
                //

                //検出したpixel座標をstage座標に変換するなどlistに追加する。
                LPeak_Low.Add(new OpenCvSharp.CPlusPlus.Point(pixel_cen.X - 256, pixel_cen.Y - 220));

                double firstx = LStage_Low[i].X - LPeak_Low[i].X * 0.000267;
                double firsty = LStage_Low[i].Y + LPeak_Low[i].Y * 0.000267;
                double firstz = LStage_Low[i].Z;
                LTrack_Low.Add(new Point3d(firstx, firsty, firstz));
                //

                //ここからは、最小二乗法で角度を算出するプログラムである。
                //上側乳剤層上面の1回目のtrack検出によって、次のtrackの位置を検出する角度を求める。
                //その角度が、1回目のtrack検出の結果によって大きな角度にならないように調整をする。
                if (i == 0)
                {
                    OpenCvSharp.CPlusPlus.Point2d Tangle_l = ApproximateStraightBase(Sh, Sh_low, LTrack[ltrack_counter - 2], LTrack[ltrack_counter - 1], LTrack_Low[i], surface);
                    Msdxdy_Low.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle_l.X, Tangle_l.Y));
                }
                else if (i == 1)
                {
                    OpenCvSharp.CPlusPlus.Point2d Tangle_l = ApproximateStraightBase(Sh, Sh_low, LTrack[ltrack_counter - 1], LTrack_Low[i - 1], LTrack_Low[i], surface);
                    Msdxdy_Low.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle_l.X, Tangle_l.Y));
                }
                else
                {
                    OpenCvSharp.CPlusPlus.Point2d Tangle_l = ApproximateStraight(Sh_low, LTrack_Low[i - 2], LTrack_Low[i - 1], LTrack_Low[i]);
                    Msdxdy_Low.Add(new OpenCvSharp.CPlusPlus.Point2d(Tangle_l.X, Tangle_l.Y));
                }
            }//i_loop

            //
            int ltrack_low_count = LTrack_Low.Count();

            mc.MovePointXY(LTrack_Low[ltrack_low_count - 1].X, LTrack_Low[ltrack_low_count - 1].Y);

            mc.Join();

            //検出に失敗した場合は、ループを抜けてここに来る。
            not_detect_track :;//検出に失敗したと考えられる地点で画像を取得し、下ゲル下面まで移動する。(現在は下ゲル下面とするが、今後変更する可能性有。)

            if (not_detect != 0)
            {
                //写真撮影
                List <ImageTaking> NotDetect = TakeSequentialImage(
                    Msdxdy[0].X * Sh,
                    Msdxdy[0].Y * Sh,
                    0,
                    1);

                string       txtfileName_t_not_detect = datarootdirpathw + string.Format(@"\not_detect.txt");
                StreamWriter twriter_t_not_detect     = File.CreateText(txtfileName_t_not_detect);
                for (int i = 0; i < NotDetect.Count; i++)
                {
                    NotDetect[i].img.ImWrite(datarootdirpathw + string.Format(@"\img_t_not_detect.bmp"));
                    Vector3 p = NotDetect[i].StageCoord;
                    twriter_t_not_detect.WriteLine("{0} {1} {2} {3}", i, p.X, p.Y, p.Z);
                }
                twriter_t_not_detect.Close();

                mc.MovePointZ(surface.LowBottom);

                mc.Join();
            }


            //file write out up_gel
            string       txtfileName_sh_up = datarootdirpathw + string.Format(@"\Sh_up.txt");
            StreamWriter twriter_sh_up     = File.CreateText(txtfileName_sh_up);

            twriter_sh_up.WriteLine("{0}", Sh);
            twriter_sh_up.Close();

            //file write out
            string       txtfileName_t_info_up = datarootdirpathw + string.Format(@"\location_up.txt");
            StreamWriter twriter_t_info_up     = File.CreateText(txtfileName_t_info_up);

            for (int i = 0; i < UpTrackInfo.Count; i++)
            {
                for (int t = 0; t < UpTrackInfo[i].Count; t++)
                {
                    UpTrackInfo[i][t].img.ImWrite(datarootdirpathw + string.Format(@"\img_t_info_up_{0}-{1}.bmp", i, t));
                    Vector3 p = UpTrackInfo[i][t].StageCoord;
                    twriter_t_info_up.WriteLine("{0} {1} {2} {3} {4}", i, t, p.X, p.Y, p.Z);
                }
            }
            twriter_t_info_up.Close();

            string       txtfileName_lpeak = datarootdirpathw + string.Format(@"\lpeak_up.txt");
            StreamWriter twriter_lpeak     = File.CreateText(txtfileName_lpeak);

            for (int i = 0; i < LPeak.Count(); i++)
            {
                OpenCvSharp.CPlusPlus.Point p = LPeak[i];
                twriter_lpeak.WriteLine("{0} {1} {2}", i, p.X, p.Y);
            }
            twriter_lpeak.Close();

            string       txtfileName_ltrack = datarootdirpathw + string.Format(@"\ltrack_up.txt");
            StreamWriter twriter_ltrack     = File.CreateText(txtfileName_ltrack);

            for (int i = 0; i < LTrack.Count(); i++)
            {
                OpenCvSharp.CPlusPlus.Point3d p = LTrack[i];
                twriter_ltrack.WriteLine("{0} {1} {2} {3}", i, p.X, p.Y, p.Z);
            }
            twriter_ltrack.Close();

            string       txtfileName_msdxdy = datarootdirpathw + string.Format(@"\msdxdy.txt");
            StreamWriter twriter_msdxdy     = File.CreateText(txtfileName_msdxdy);

            for (int i = 0; i < Msdxdy.Count(); i++)
            {
                OpenCvSharp.CPlusPlus.Point2d p = Msdxdy[i];
                twriter_msdxdy.WriteLine("{0} {1} {2}", i, p.X, p.Y);
            }
            twriter_msdxdy.Close();


            //file write out low_gel
            string       txtfileName_sh_low = datarootdirpathw + string.Format(@"\Sh_low.txt");
            StreamWriter twriter_sh_low     = File.CreateText(txtfileName_sh_low);

            twriter_sh_low.WriteLine("{0}", Sh_low);
            twriter_sh_low.Close();

            string       txtfileName_t_info_low = datarootdirpathw + string.Format(@"\location_low.txt");
            StreamWriter twriter_t_info_low     = File.CreateText(txtfileName_t_info_low);

            for (int i = 0; i < LowTrackInfo.Count; i++)
            {
                for (int t = 0; t < LowTrackInfo[i].Count; t++)
                {
                    LowTrackInfo[i][t].img.ImWrite(datarootdirpathw + string.Format(@"\img_t_info_low_{0}-{1}.bmp", i, t));
                    Vector3 p = LowTrackInfo[i][t].StageCoord;
                    twriter_t_info_low.WriteLine("{0} {1} {2} {3} {4}", i, t, p.X, p.Y, p.Z);
                }
            }
            twriter_t_info_low.Close();

            string       txtfileName_lpeak_low = datarootdirpathw + string.Format(@"\lpeak_low.txt");
            StreamWriter twriter_lpeak_low     = File.CreateText(txtfileName_lpeak_low);

            for (int i = 0; i < LPeak_Low.Count(); i++)
            {
                OpenCvSharp.CPlusPlus.Point p = LPeak_Low[i];
                twriter_lpeak_low.WriteLine("{0} {1} {2}", i, p.X, p.Y);
            }
            twriter_lpeak_low.Close();

            string       txtfileName_ltrack_low = datarootdirpathw + string.Format(@"\ltrack_low.txt");
            StreamWriter twriter_ltrack_low     = File.CreateText(txtfileName_ltrack_low);

            for (int i = 0; i < LTrack_Low.Count(); i++)
            {
                OpenCvSharp.CPlusPlus.Point3d p = LTrack_Low[i];
                twriter_ltrack_low.WriteLine("{0} {1} {2} {3}", i, p.X, p.Y, p.Z);
            }
            twriter_ltrack_low.Close();

            string       txtfileName_msdxdy_low = datarootdirpathw + string.Format(@"\msdxdy_low.txt");
            StreamWriter twriter_msdxdy_low     = File.CreateText(txtfileName_msdxdy_low);

            for (int i = 0; i < Msdxdy_Low.Count(); i++)
            {
                OpenCvSharp.CPlusPlus.Point2d p = Msdxdy_Low[i];
                twriter_msdxdy_low.WriteLine("{0} {1} {2}", i, p.X, p.Y);
            }
            twriter_msdxdy_low.Close();//*/

            //.................Taking Photo in buttom of down layer.......................//

            try
            {
                string[] sp                    = myTrack.IdString.Split('-');
                string   dwtxt                 = string.Format(@"c:\test\bpm\{0}\{1}-{2}-{3}-{4}_dw.txt", mod, mod, pl, sp[0], sp[1]);
                string   datarootdirpath       = string.Format(@"C:\test\bpm\{0}", mod);
                System.IO.DirectoryInfo mydir  = System.IO.Directory.CreateDirectory(datarootdirpath);
                System.IO.DirectoryInfo mydir2 = System.IO.Directory.CreateDirectory(datarootdirpath);
                BeamDetection(dwtxt, false);
            }
            catch (ArgumentOutOfRangeException)
            {
                MessageBox.Show("ID is not exist ");
            }

            //...............Move to top surfacr of upper layer........................//
            try
            {
                Vector3 cc = mc.GetPoint();
                double  Zp = surface.UpTop;
                mc.MoveTo(new Vector3(cc.X, cc.Y, Zp));
                mc.Join();
            }
            catch (ArgumentOutOfRangeException)
            {
                MessageBox.Show("Cannot move to top surface of upperlayer ");
            }

            // .......................................................................//

            sw.Stop();
            string[] sp1     = myTrack.IdString.Split('-');
            string   logtxt_ = string.Format(@"c:\test\bpm\{0}\{1}-{2}_log_.txt", mod, mod, pl);
            //string log_ = string.Format("{0} \n", sw.Elapsed);
            string       log_ = string.Format("{0} {1} {2} \n", sp1[0], sp1[1], sw.Elapsed);
            StreamWriter swr  = new StreamWriter(logtxt_, true, Encoding.ASCII);

            swr.Write(log_);
            swr.Close();
        }
예제 #13
0
            /// <summary>
            /// allTracksButtonのドロップダウンメニューにトラックの一覧を追加して,初期化します.
            /// </summary>
            public void InitializeTracksItemMenu()
            {
                TracksManager tracksManager = parameterManager.TracksManager;

                // トラックデータが読み込まれていないなど,TracksManagerが初期化されていない場合は終了する.
                if (!tracksManager.IsInitialized)
                {
                    nextTrackButton.IsEnabled = false;
                    allTracksButton.IsEnabled = false;
                    return;
                }

                // トラックが存在しない場合はnextTrackButton及びallTracksButtonを無効にする.
                // トラックが一つのみの場合はallTracksButtonのみを無効にする.
                if (tracksManager.NumOfTracks == 0)
                {
                    nextTrackButton.IsEnabled = false;
                }
                if (tracksManager.NumOfTracks <= 1)
                {
                    allTracksButton.IsEnabled = false;
                }

                // allTracksButtonのドロップダウンメニューの中身を初期化する
                tracksItem = new ComboBoxItem[tracksManager.NumOfTracks];
                for (int i = 0; i < tracksItem.Length; ++i)
                {
                    Track myTrack = tracksManager.GetTrack(i);
                    tracksItem[i]                     = new ComboBoxItem();
                    tracksItem[i].Content             = myTrack.IdString;
                    tracksItem[i].HorizontalAlignment = HorizontalAlignment.Left;
                    tracksItem[i].Selected           += delegate(object o, RoutedEventArgs e2) {
                        /* ComboBoxItemのClickイベントハンドラを追加 */
                        // 該当トラックの座標までモータ移動させる.
                        // ただし,モータが移動中の場合はその移動を中止させるかをユーザーに尋ねる.
                        MotorControler mc = MotorControler.GetInstance(parameterManager);
                        if (mc.IsMoving)
                        {
                            if (askAbortMotorMoving())
                            {
                                mc.AbortMoving();
                                if (stage != null)
                                {
                                    stage.WriteLine(Properties.Strings.MotorStop01);
                                }
                            }
                            else
                            {
                                // ユーザーがモータ動作を中止しない場合は終了
                                return;
                            }
                        }

                        // モータの移動動作
                        stage.WriteLine(Properties.Strings.Moving);
#if !NoHardware
                        double dstx = myTrack.MsX + window.CoordManager.HFDX;
                        double dsty = myTrack.MsY + window.CoordManager.HFDY;
                        mc.MovePointXY(dstx, dsty, delegate {
                            stage.WriteLine(Properties.Strings.MovingComplete);
                        });
#endif
                        // 追跡中のトラック番号を更新
                        tracksManager.TrackingIndex = myTrack.Index;

                        // 移動済みのメニュー項目は太字にするようにセット
                        ComboBoxItem thisItem = o as ComboBoxItem;
                        thisItem.FontWeight = FontWeights.Bold;
                        UpdateTrackInfo();
                    };
                    allTracksButton.Items.Add(tracksItem[i]);
                }
            }
예제 #14
0
        /// <summary>
        /// searchThreadが行う処理です.このメソッドを直接呼び出さないでください.
        /// </summary>
        private void searchThread_Task()
        {
            if (Started != null)
            {
                Started(this, new EventArgs());
            }

            Camera camera = Camera.GetInstance();

            camera.Start();

            for (int i = coordManager.DefinedGridMarkNum; i < CoordManager.AllGridMarksNum; ++i)
            {
                GridMarkPoint  presentMark   = utility.NextPoint;
                Vector2        nextGridCoord = utility.GetGridMarkCoord(presentMark);
                MotorControler mc            = MotorControler.GetInstance(parameterManager);
                Led            led           = Led.GetInstance();

                mc.MovePointXY(nextGridCoord, new Action(delegate {
                    mc.SetSpiralCenterPoint();
                }));
                mc.Join();

                Surface surface      = Surface.GetInstance(parameterManager);
                bool    moveContinue = true;

                mc.MoveDistance(0.1, VectorId.Z);
                mc.Join();
                Thread.Sleep(100);

                led.AdjustLight(parameterManager);

                while (moveContinue)
                {
                    mc.MoveDistance(-0.01, VectorId.Z);
                    mc.Join();
                    byte[] b    = camera.ArrayImage;
                    Mat    mat0 = new Mat(440, 512, MatType.CV_8U, b);
                    Mat    mat  = mat0.Clone();
                    Cv2.GaussianBlur(mat, mat, Cv.Size(3, 3), -1);
                    //mat.ImWrite(String.Format(@"c:\img\{0}_g.bmp",
                    //    System.DateTime.Now.ToString("yyyyMMdd_HHmmss_fff")));
                    Mat gau = mat.Clone();
                    Cv2.GaussianBlur(gau, gau, Cv.Size(31, 31), -1);
                    Cv2.Subtract(gau, mat, mat);
                    Cv2.Threshold(mat, mat, 10, 255, ThresholdType.Binary);
                    int brightness = Cv2.CountNonZero(mat);
                    //mat.ImWrite(String.Format(@"c:\img\{0}_t.bmp",
                    //    System.DateTime.Now.ToString("yyyyMMdd_HHmmss_fff")));

                    moveContinue = (brightness < 15000);
                }

                led.AdjustLight(parameterManager);


                /* グリッドマークを検出する */
                // 入力画像にて,グリッドマーク検出を行う.
                // 見つからなかった場合はNumOfSpiralSearchの回数分だけらせん移動を行い探す.
                bool continueFlag = true;
                while (continueFlag)
                {
                    GridMarkEventArgs eventArgs = new GridMarkEventArgs();
                    eventArgs.GridMarkPoint = presentMark;
                    try {
                        Vector2 viewerPoint  = GridMarkRecognizer.SearchGridMark();
                        Vector2 encoderPoint = coordManager.TransToEmulsionCoord(viewerPoint);
                        mc.MovePointXY(encoderPoint);
                        mc.Join();
                        Thread.Sleep(100);
                        viewerPoint  = GridMarkRecognizer.SearchGridMark();
                        encoderPoint = coordManager.TransToEmulsionCoord(viewerPoint);
                        mc.MovePointXY(encoderPoint);
                        mc.Join();
                        Thread.Sleep(100);

                        coordManager.SetGridMark(encoderPoint, presentMark, camera.ArrayImage);

                        continueFlag = false;
                        if (Found != null)
                        {
                            Vector3 position = mc.GetPoint();
                            eventArgs.ViewerPoint   = new Vector2(position.X, position.Y);
                            eventArgs.EncoderPoint  = encoderPoint;
                            eventArgs.GridMarkPoint = presentMark;
                            Found(this, eventArgs);
                        }
                    } catch (GridMarkNotFoundException) {
                        mc.MoveInSpiral(true);
                        continueFlag = (mc.SpiralIndex < NumOfSpiralSearch);
                        if (!continueFlag && NotFound != null)
                        {
                            NotFound(this, eventArgs);
                        }
                    }
                } // while
            }     // for

            // 座標系の生成
            coordManager.CreateCoordSystem();
        }