예제 #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");
            }
        }
예제 #2
0
파일: Tello.cs 프로젝트: aram89g/VisionApp
    /***********************************************************
    * inputs: (X,Y,Z) coordinates of the drone
    * updates drone commands according to drone and target position.
    ***********************************************************/
    public void InstructionCalculate(StereoPoint3D drone, ref Point3D target)
    {
        double delta = 0.1;
        //int stepX = 20;
        //int stepY = 20;
        //int stepZ = 20;

        double X = drone.GetX3D();
        double Y = drone.GetY3D();
        double Z = drone.GetZ3D();

        bool xValid = false;
        bool yValid = false;
        bool zValid = false;

        Console.WriteLine($"Drone Coordinates:  [X: {drone.GetX3D()}, Y: {drone.GetY3D()}, Z: {drone.GetZ3D()}]\n");
        Console.WriteLine($"Target Coordinates:  [X: {target.GetX()}, Y: {target.GetY()}, Z: {target.GetZ()}]\n");

        // set PID input and reference
        pidX.Input    = drone.GetX3D();
        pidX.Setpoint = target.GetX();

        pidY.Input    = drone.GetY3D();
        pidY.Setpoint = target.GetY();

        pidZ.Input    = drone.GetZ3D();
        pidZ.Setpoint = target.GetZ();

        Console.WriteLine($"PID X:  [Kp: {pidX.Kp}, Ki: {pidX.Ki}, Kd: {pidX.Kd}]\n");
        Console.WriteLine($"PID Y:  [Kp: {pidY.Kp}, Ki: {pidY.Ki}, Kd: {pidY.Kd}]\n");
        Console.WriteLine($"PID Z:  [Kp: {pidZ.Kp}, Ki: {pidZ.Ki}, Kd: {pidZ.Kd}]\n");

        // check if drone is at target
        xValid = OnTarget(pidX.Setpoint, pidX.Input, delta);
        yValid = OnTarget(pidY.Setpoint, pidY.Input, delta);
        zValid = OnTarget(pidZ.Setpoint, pidZ.Input, delta);

        // calculate PID output
        stateX = pidX.PerformPID();
        stateY = pidY.PerformPID();
        stateZ = pidZ.PerformPID();

        if (!double.IsNaN(stateX) && !double.IsNaN(stateY) && !double.IsNaN(stateZ))
        {
            left_right   = (int)(stateX * 100);
            down_up      = -1 * (int)(stateY * 100);
            back_forward = (int)(stateZ * 100);

            //if (!xValid)
            //    left_right = (int)(stateX * 100);
            //if (!yValid)
            //    down_up = -1*(int)(stateY * 100);
            //if (!zValid)
            //    back_forward = (int)(stateZ * 100);

            Console.WriteLine("PID values:\n");
            Console.WriteLine($"right_left_mov = {left_right}\n");
            Console.WriteLine($"up_down = {down_up}\n");
            Console.WriteLine($"forward_back = {back_forward}\n");

            if (xValid && yValid && zValid)
            {
                target.arrived = true;
                Console.WriteLine("On Target, all axes\n");
            }

            // Send the RC command to the drone
            SendRcControl();
        }



        /*
         *
         * // Horizontal axis
         * if ((X < (target.GetX() + delta)) && (X > (target.GetX() - delta)))
         * {
         *  left_right = 0;
         *  xValid = true;
         * }
         * else if (X < target.GetX())
         * {
         *  left_right = stepX;
         *  Console.WriteLine("goes right");
         * }
         * else
         * {
         *  left_right = -stepX;
         *  Console.WriteLine("goes left");
         * }
         * // Vertical axis
         * if ((Y < (target.GetY() + delta)) && (Y > (target.GetY() - delta)))
         * {
         *  down_up = 0;
         *  yValid = true;
         * }
         * else if (Y < target.GetY())
         * {
         *  down_up = -stepY;
         *  Console.WriteLine("goes down");
         * }
         * else
         * {
         *  down_up = stepY;
         *  Console.WriteLine("goes up");
         * }
         * //Depth axis
         * if ((Z < (target.GetZ() + delta)) && (Z > (target.GetZ() - delta)))
         * {
         *  back_forward = 0;
         *  zValid = true;
         * }
         * else if (Z < target.GetZ())
         * {
         *  back_forward = stepZ;
         *  Console.WriteLine("goes forward");
         * }
         * else
         * {
         *  back_forward = -stepZ;
         *  Console.WriteLine("goes backwards");
         * }
         * if (xValid && yValid && zValid)
         * {
         *  target.arrived = true;
         * }
         *
         * // Send the RC command to the drone
         * SendRcControl();
         */
    }