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