/**********************************************************
* 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");
}
}
