Exemple #1
0
        /**********************************************************
        * processing of the images taken from the cameras after remapping them,
        * in order to find the drone and send a command to it.
        * Update of the flight plan.
        * Show processed frames in windows.
        * Create video from cameras images.
        **********************************************************/
        public void VideoProcessingLoop(VideoCapture capture_l, Mat backgroundFrame_l, VideoCapture capture_r, Mat backgroundFrame_r, Mat rmapx1, Mat rmapy1, Mat rmapx2, Mat rmapy2, Rectangle Rec1, Rectangle Rec2)
        {
            // Statistics timers
            var stopwatch = new Stopwatch();              // Used to measure video processing performance
            var stopwatch_3d_calculate = new Stopwatch(); // Used to measure 3D calculation performance

            int frameNumber = 0;
            Mat videoFrame  = new Mat();

            Mat cropLeft;
            Mat cropRight;

            #region Flight Plan
            int           targetCnt = 1;
            StereoPoint3D drone     = new StereoPoint3D();
            Point3D       target;

            foreach (string line in flightPlanFile)
            {
                string[] coordinate = line.Split(',');
                double   x          = Convert.ToDouble(coordinate[0]);
                double   y          = Convert.ToDouble(coordinate[1]);
                double   z          = Convert.ToDouble(coordinate[2]);
                flightPlan.Enqueue(new Point3D(x, y, z));
            }
            target = flightPlan.Dequeue();
            #endregion

            while (true)// Loop video
            {
                frameNumber++;
                Console.WriteLine("************************************Start Frame*************************************\n");

                // Getting next frame(null is returned if no further frame exists)
                rawFrame_l = capture_l.QueryFrame();
                rawFrame_r = capture_r.QueryFrame();

                // Rectify frames using rmap and crop according to roi (from calibration)
                CvInvoke.Remap(rawFrame_l, rawFrame_l, rmapx1, rmapy1, Inter.Linear);
                CvInvoke.Remap(rawFrame_r, rawFrame_r, rmapx2, rmapy2, Inter.Linear);

                // Save video file of both cameras stream (images needs to be the same size)
                if (saveCameraVideo)
                {
                    CvInvoke.HConcat(rawFrame_l, rawFrame_r, videoFrame);
                    videoWrite.Write(videoFrame);
                }

                // Crop images according to ROI
                cropLeft  = new Mat(rawFrame_l, Rec1);
                cropRight = new Mat(rawFrame_r, Rec2);
                //cropLeft = rawFrame_l.Clone();
                //cropRight = rawFrame_r.Clone();
                rawFrame_l = cropLeft;
                rawFrame_r = cropRight;

                // Process frame image to find drone location in the frame
                stopwatch.Restart();// Frame processing calculate - Start
                //ProcessFrame(backgroundFrame_l, backgroundFrame_r, Threshold, ErodeIterations, DilateIterations);
                ProcessFrame(Threshold);
                stopwatch.Stop();// Frame processing calculate - End

                // Calculate drone 3D coordinate
                drone.CalculateCoordinate3D(point_center_l.X, point_center_r.X, point_center_r.Y);
                X_3d = drone.GetX3D();
                Y_3d = drone.GetY3D();
                Z_3d = drone.GetZ3D();
                Console.WriteLine($"Frame Number: {frameNumber}\n");

                // Check drone position accodring to target and update drone command
                rmt.InstructionCalculate(drone, ref target);

                // check and update if needed target coordinate
                if (target.arrived)
                {
                    System.Media.SystemSounds.Beep.Play();
                    targetCnt++;
                    if (flightPlan.Count > 0)
                    {
                        target = flightPlan.Dequeue();
                    }
                    else
                    {
                        rmt.SendCommand("land");
                        CvInvoke.WaitKey(10000);
                        Environment.Exit(0);
                    }
                }
                // Write data to Frame
                WriteFrameInfo(stopwatch.ElapsedMilliseconds, frameNumber, targetCnt);

                // Show all frame processing stages
                ShowWindowsWithImageProcessingStages();

                // Enable program exit from keyboard
                int key = CvInvoke.WaitKey(5);
                // Close program if Esc key was pressed
                if (key == 27)
                {
                    rmt.SendCommand("land");
                    CvInvoke.WaitKey(5000);
                    videoWrite.Dispose();
                    Environment.Exit(0);
                }
                Console.WriteLine("************************************End Frame*************************************\n\n");
            }
        }