// display the tracked objects and there location
private void DisplayTrackedObjects()
{
// clears the detected x y co-ordinate field to display co ordinates
DetectedXY.Text = string.Empty;
foreach (TrackedObject tracked in trackedObjects)
{
// display the detected co ordinates
PointF centre = tracked.getLastCircle().Center;
DetectedXY.Text = DetectedXY.Text + $"X: {centre.X}; Y: {centre.Y} ;";
//draw last circle and centre
objectImage.Draw(tracked.getLastCircle(), new Bgr(Color.Red), 4);
objectImage.Draw(new CircleF(tracked.getLastCircle().Center, 1), new Bgr(Color.Red), 4);
//draw path
List <PointF> path = tracked.GetPath();
// if path length 1 no need to plot
if (path.Count == 1)
{
continue;
}
// gets first point
PointF pointA = path[0];
//plots all pointd
foreach (PointF pointB in path)
{
LineSegment2DF line = new LineSegment2DF(pointA, pointB);
objectImage.Draw(line, new Bgr(Color.Red), 2);
pointA = pointB;
}
}
}
private void GetValue(IEnumerable <LineSegment2D> lines, LineSegment2DF endLine, LineSegment2D[] previousLines, LineSegment2D?targetLine, ref LineSegment2D?nextLine)
{
const double minDistanceThreshold = 20;
double minDistanceToEnd = double.MaxValue;
nextLine = null;
//targetPoint = null;
foreach (var line in lines)
{
if (previousLines.Contains(line))
{
continue;
}
if (line.P1 == targetLine.Value.P1 && line.P2 == targetLine.Value.P2)
{
continue;
}
double dist1 = MathExtension.FindDistanceBetweenSegments(targetLine.Value.P1, targetLine.Value.P2, line.P1, line.P2);
double minDistToEnd = MathExtension.FindDistanceBetweenSegments(line.P1, line.P2, endLine.P1, endLine.P2);
double currentDistToEnd = MathExtension.FindDistanceBetweenSegments(targetLine.Value.P1, targetLine.Value.P2, endLine.P1, endLine.P2);
if (dist1 < minDistanceThreshold && minDistToEnd < currentDistToEnd && minDistToEnd < minDistanceToEnd)
{
minDistanceToEnd = minDistToEnd;
nextLine = line;
}
}
}
/// <summary>
/// convert a series of points to LineSegment2D
/// </summary>
/// <param name="points">the array of points</param>
/// <param name="closed">if true, the last line segment is defined by the last point of the array and the first point of the array</param>
/// <returns>array of LineSegment2D</returns>
public static LineSegment2DF[] PolyLine(PointF[] points, bool closed)
{
LineSegment2DF[] res;
int length = points.Length;
if (closed)
{
res = new LineSegment2DF[length];
PointF lastPoint = points[length - 1];
for (int i = 0; i < res.Length; i++)
{
res[i] = new LineSegment2DF(lastPoint, points[i]);
lastPoint = points[i];
}
}
else
{
res = new LineSegment2DF[length - 1];
PointF lastPoint = points[0];
for (int i = 1; i < res.Length; i++)
{
res[i] = new LineSegment2DF(lastPoint, points[i]);
lastPoint = points[i];
}
}
return(res);
}
/// <summary>
/// Finds lines in the input image.
/// </summary>
/// <param name="image">Image to detect lines in.</param>
/// <returns>The detected line segments</returns>
public LineSegment2DF[] Detect(IInputArray image)
{
using (InputArray iaImage = image.GetInputArray())
using (Mat matLines = new Mat())
using (OutputArray oaLines = matLines.GetOutputArray())
{
// Process image
XImgprocInvoke.cveFastLineDetectorDetect(_ptr, iaImage, oaLines);
// Convert data in Mat to list of LineSegment2DF objects
float[] pointData = new float[matLines.Total.ToInt32() * matLines.ElementSize / 4];
LineSegment2DF[] lines = new LineSegment2DF[pointData.Length / 4];
matLines.CopyTo(pointData);
// Each line is represented by 4 floats
for (int i = 0; i < pointData.Length / 4; i++)
{
lines[i] = new LineSegment2DF(
new PointF(pointData[i * 4], pointData[(i * 4) + 1]),
new PointF(pointData[(i * 4) + 2], pointData[(i * 4) + 3]));
}
return(lines);
}
}
private void DrawRegionBoundary(Image <Bgr, Byte> image, int yPos)
{
PointF start = new PointF(0, yPos);
PointF end = new PointF(image.Width, yPos);
LineSegment2DF lineSegment = new LineSegment2DF(start, end);
image.Draw(lineSegment, new Bgr(Color.Red), 1);
}
internal void DrawOpticFluxLines(PointF previousFeatureLocation, PointF CurrentFeatureLocation, bool hasFullHistory, Image <Bgr, Byte> image)
{
if (hasFullHistory)
{
LineSegment2DF linusSegmentus = new LineSegment2DF(previousFeatureLocation, CurrentFeatureLocation);
image.Draw(linusSegmentus, new Bgr(System.Drawing.Color.Red), 1);
}
}
public static double angleBetween2Lines(LineSegment2DF line1, LineSegment2DF line2)
{
double angle1 = Math.Atan2(line1.P1.Y - line1.P2.Y,
line1.P1.X - line1.P2.X);
double angle2 = Math.Atan2(line2.P1.Y - line2.P2.Y,
line2.P1.X - line2.P2.X);
return(angle1 - angle2);
}
public void drawLine(Image <Hsv, byte> image, Rectangle bounds)
{
LineSegment2DF?testLine = getDrawnLine(bounds);
if (testLine != null)
{
LineSegment2DF line = (LineSegment2DF)testLine;
image.Draw(line, new Hsv(lineColor.Blue, lineColor.Green, lineColor.Red), lineThickness);
}
}
public void drawLine(Image <Gray, byte> image, Rectangle bounds)
{
LineSegment2DF?testLine = getDrawnLine(bounds);
if (testLine != null)
{
LineSegment2DF line = (LineSegment2DF)testLine;
image.Draw(line, new Gray(0.3 * lineColor.Red + 0.59 * lineColor.Green + 0.11 * lineColor.Blue), lineThickness);
}
}
public void drawLine(Image <Bgr, byte> image, Rectangle bounds)
{
LineSegment2DF?testLine = getDrawnLine(bounds);
if (testLine != null)
{
LineSegment2DF line = (LineSegment2DF)testLine;
image.Draw(line, lineColor, lineThickness);
}
}
private void DrawFlowVectors()
{
for (int i = 0; i < this.TrackedFeatures.Length; i++)
{
if (m_TrackingStatus[i] == 1)
{
LineSegment2DF lineSegment = new LineSegment2DF(this.PreviousFoundFeatures[i], this.TrackedFeatures[i]);
this.FlowImage.Draw(lineSegment, new Bgr(Color.Red), 1);
CircleF circle = new CircleF(this.TrackedFeatures[i], 2.0f);
this.FlowImage.Draw(circle, new Bgr(Color.Red), 1);
}
}
}
private void button2_Click(object sender, EventArgs e)
{
OpenFileDialog op = new OpenFileDialog();
if (op.ShowDialog() == DialogResult.OK)
{
Mat img = new Mat(op.FileName, Emgu.CV.CvEnum.LoadImageType.AnyColor);
imageBox1.Image = img;
}
//點，线，矩形，園，橢圓
MCvPoint3D32f mCvPoint3D32F = new MCvPoint3D32f(0, 0, 0);
PointF x = new PointF(0, 0);
PointF y = new PointF(1, 1);
LineSegment2DF lineSegment2DF = new LineSegment2DF(x, y);
//顏色
Rgb rgb = new Rgb(Color.Red);
Rgb red = new Rgb(255, 0, 0);
//類型轉換
Bitmap bitmap = new Bitmap(640, 480);
Image <Bgr, byte> image = new Image <Bgr, byte>(640, 480);
Mat mat = new Mat();
op = new OpenFileDialog();
if (op.ShowDialog() == DialogResult.OK)
{
Bitmap bitmap_new = new Bitmap(op.FileName);
Image <Bgr, byte> image_new = new Image <Bgr, byte>(op.FileName);
Mat mat_new = new Mat(op.FileName, Emgu.CV.CvEnum.LoadImageType.AnyColor);
pictureBox1.Image = bitmap_new;
pictureBox1.Image = image_new.ToBitmap();
//pictureBox1.Image = mat_new.Bitmap;
pictureBox1.Image = mat_new.ToImage <Bgr, byte>().Bitmap;
imageBox1.Image = new Image <Bgr, byte>(bitmap_new);
imageBox1.Image = image_new;
imageBox1.Image = mat_new;
}
Image <Bgr, byte> img = new Image <Bgr, byte>(200, 200, new Bgr(255, 255, 255));
Rectangle rectangle = new Rectangle(new Point(80, 80), new Size(40, 40));
CircleF circleF = new CircleF(new PointF(100, 100), 40);
string str = "I LOVE EmguCV";
Point str_location = new Point(0, 30);
img.Draw(rectangle, new Bgr(0, 255, 0), 2);
img.Draw(circleF, new Bgr(0, 0, 255), 3);
img.Draw(str, str_location, Emgu.CV.CvEnum.FontFace.HersheyComplexSmall, 1, new Bgr(0, 255, 0), 3);
imageBox1.Image = img;
}
public static double GetDistanceFromLine(PointF pt, LineSegment2DF line)
{
var a = line.P1;
var c = line.P2;
var b = pt;
// normalize points
var cn = new PointF(c.X - a.X, c.Y - a.Y);
var bn = new PointF(b.X - a.X, b.Y - a.Y);
double angle = Math.Atan2(bn.Y, bn.X) - Math.Atan2(cn.Y, cn.X);
double abLength = Math.Sqrt(bn.X * bn.X + bn.Y * bn.Y);
return(Math.Sin(angle) * abLength);
}
private PointF RelocateTooFarPoint(PointF feature, PointF center_point)
{
PointF middle = new PointF();
middle.X = (int)((feature.X + center_point.X) * 0.5);
middle.Y = (int)((feature.Y + center_point.Y) * 0.5);
LineSegment2DF line = new LineSegment2DF(feature, middle);
// colored_temp_image.Draw(new CircleF(feature, 3f), new Bgr(Color.Black), 2);
colored_temp_image.Draw(new CircleF(middle, 3f), new Bgr(0, 255, 0), 2);
colored_temp_image.Draw(line, new Bgr(Color.DeepPink), 2);
return(middle);
}
private bool Intersects2Df(LineSegment2DF thisLineSegment, LineSegment2DF otherLineSegment)
{
var firstLineSlopeX = thisLineSegment.P2.X - thisLineSegment.P1.X;
var firstLineSlopeY = thisLineSegment.P2.Y - thisLineSegment.P1.Y;
var secondLineSlopeX = otherLineSegment.P2.X - otherLineSegment.P1.X;
var secondLineSlopeY = otherLineSegment.P2.Y - otherLineSegment.P1.Y;
var s = (-firstLineSlopeY * (thisLineSegment.P1.X - otherLineSegment.P1.X) + firstLineSlopeX * (thisLineSegment.P1.Y - otherLineSegment.P1.Y)) / (-secondLineSlopeX * firstLineSlopeY + firstLineSlopeX * secondLineSlopeY);
var t = (secondLineSlopeX * (thisLineSegment.P1.Y - otherLineSegment.P1.Y) - secondLineSlopeY * (thisLineSegment.P1.X - otherLineSegment.P1.X)) / (-secondLineSlopeX * firstLineSlopeY + firstLineSlopeX * secondLineSlopeY);
if (s >= 0 && s <= 1 && t >= 0 && t <= 1)
{
// Collision detected
return(true);
}
return(false); // No collision
}
private double RefineAngle(double resultAngle, Image <Gray, byte> focus)
{
var rotatedNorthUp = focus.Rotate(resultAngle, new Gray(0));
// Re-rotate the frame now that N and S are vertical, so the centroids are in predictable
// locations no matter what the inital angle was.
var list = Utils.DetectAndFilterBlobs(rotatedNorthUp, 25, 250).OrderBy(b => b.Centroid.Y);
if (list.Count() >= 2)
{
var verticalLine = new LineSegment2DF(new PointF(focus.Size.Width / 2, 0), new PointF(focus.Size.Width / 2, focus.Size.Height));
var lineFromNorthToSouth = new LineSegment2DF(list.First().Centroid, list.Last().Centroid);
var skewAngle = Math2.angleBetween2Lines(lineFromNorthToSouth, verticalLine) * (180 / Math.PI);
// Divide by two since this is relative to the center of the line.
resultAngle -= skewAngle / 2;
}
return(resultAngle);
}
private void button1_Click(object sender, EventArgs e)
{
if (0 == _actionLineData.imageSrc)
{
OpenFileDialog lvse = new OpenFileDialog();
lvse.Title = "选择图片";
lvse.InitialDirectory = "";
lvse.Filter = "图片文件|*.bmp;*.jpg;*.jpeg;*.gif;*png";
lvse.FilterIndex = 1;
if (lvse.ShowDialog() == DialogResult.OK)
{
Mat mat = CvInvoke.Imread(lvse.FileName, Emgu.CV.CvEnum.ImreadModes.AnyColor);
try
{
_actionLine.run(new Image <Gray, byte>(mat.Bitmap));
}
catch (Exception ex)
{
MessageBox.Show(ex.Message);
}
}
}
else
{
_actionLine.ActionExcute();
}
_imageShow = _actionLine.imageResult.Clone();
if (_actionLine.imageResult.IsROISet)
{
Rectangle rect = new Rectangle(0, 0, _actionLine.imageInput.Width, _actionLine.imageInput.Height);
_imageShow.ROI = rect;
_imageShow.Draw(_actionLine.imageResult.ROI, new Gray(255), 3);
LineSegment2DF line = new LineSegment2DF(_actionLine.startPoint, _actionLine.endPoint);
_imageShow.Draw(line, new Gray(120), 1);
}
imageBox1.Image = _imageShow;
}
//Draw the points on the image. Input: 2d array with the information of the cells (cell num | pic num | x location | y location)
private void drawPoints(double[,] cellArray)
{
// Read the values from the 2D array and plot them on top of the image
int circleRadius = 2; //radius of the circles in the plot
int circleThickness = 2;
int lineThickness = 2;
Bgra cellColour = new Bgra(0, 0, 0, 255);
//generate random line colours for each different tracked cell
Random random = new Random();
Bgra lineColour = new Bgra(random.Next(256), random.Next(256), random.Next(256), 255);
int imageNum = 1;
foreach (var pointsImage in imagePointsList)
{
for (int i = 0; i < cellArray.GetLength(0); i++)
{
if (cellArray[i, 1] <= imageNum) //Draw the tracked points only up to the picture number in the series
{
//Draw a circle at the indicated spot
float centerX = (float)cellArray[i, 2];
float centerY = (float)cellArray[i, 3];
System.Drawing.PointF center = new System.Drawing.PointF(centerX, centerY);
CircleF circle = new CircleF(center, circleRadius);
pointsImage.Draw(circle, cellColour, circleThickness);
if (i >= 1 && cellArray[i, 0] == cellArray[i - 1, 0]) //draw a connecting line between two points of the same tracked cell
{
System.Drawing.PointF prevCenter = new System.Drawing.PointF((float)cellArray[i - 1, 2], (float)cellArray[i - 1, 3]);
LineSegment2DF line = new LineSegment2DF(prevCenter, center);
pointsImage.Draw(line, lineColour, lineThickness);
}
else
{
lineColour = new Bgra(random.Next(256), random.Next(256), random.Next(256), 255);
}
}
}
imageNum++;
}
}
private MKeyPoint GetNearestKeyPoint(double x, double y, VectorOfKeyPoint keyFeaturesVector)
{
var width = _capture.GetCaptureProperty(CapProp.FrameWidth);
var height = _capture.GetCaptureProperty(CapProp.FrameHeight);
var point = new PointF((float)(x * width), (float)(y * height));
var result = keyFeaturesVector.Enumerable().First();
var minDistanse = double.MaxValue;
foreach (var mKeyPoint in keyFeaturesVector.Enumerable())
{
var distance = new LineSegment2DF(mKeyPoint.Point, point).Length;
if (distance < minDistanse)
{
result = mKeyPoint;
minDistanse = distance;
}
}
return(result);
}
public static Image <Bgra, byte> DrawMaxSubmatrix(Image <Bgra, byte> Image, int[,] isTaken)
{
int x1, y1, width, height;
float boxW = (float)Image.Width / isTaken.GetLength(0);
float boxH = (float)Image.Height / isTaken.GetLength(1);
MaxSubmatrix(isTaken, out x1, out y1, out width, out height);
PointF upLeft = new PointF(x1 * boxW, y1 * boxH);
PointF bottumLeft = new PointF(x1 * boxW, y1 * boxH + boxH * height);
PointF upRight = new PointF(x1 * boxW + boxW * width, y1 * boxH);
PointF bottumRight = new PointF(x1 * boxW + boxW * width, y1 * boxH + boxH * height);
LineSegment2DF up = new LineSegment2DF(upLeft, upRight);
LineSegment2DF bottum = new LineSegment2DF(bottumLeft, bottumRight);
LineSegment2DF left = new LineSegment2DF(upLeft, bottumLeft);
LineSegment2DF right = new LineSegment2DF(upRight, bottumRight);
Image.Draw(up, new Bgra(0, 255, 0, 0), 2);
Image.Draw(bottum, new Bgra(0, 255, 0, 0), 2);
Image.Draw(left, new Bgra(0, 255, 0, 0), 2);
Image.Draw(right, new Bgra(0, 255, 0, 0), 2);
return(Image);
}
/// <summary>
/// Draw a line segment in the map
/// </summary>
/// <param name="line">The line to be draw</param>
/// <param name="color">The color for the line</param>
/// <param name="thickness">The thickness of the line</param>
public override void Draw(LineSegment2DF line, TColor color, int thickness)
{
base.Draw(new LineSegment2D(MapPointToImagePoint(line.P1), MapPointToImagePoint(line.P2)), color, thickness);
}
public static bool AnalyzeZebraCrossingTexture(int mainDirectionLineGroupId, Dictionary <LineQuantification, LinkedList <LineEquation> > linesHistogram, Image <Gray, byte> processingImg, Image <Bgr, byte> oriImg, Image <Bgr, byte> stasticDst, Image <Bgr, byte> drawScanLineImg)
{
//紀錄斑馬線之間白色連結起來的線段
List <LineSegment2DF> crossingConnectionlines = new List <LineSegment2DF>();
Point prePoint = new Point();
Point currentPoint = new Point();
Image <Bgr, byte> scanLineImg = oriImg.Clone();
IntensityPoint current, previous;
current = new IntensityPoint();
previous = new IntensityPoint();
int index = 0;
List <LineEquation> orderedLines = new List <LineEquation>();
//角度幾乎呈垂直,所以排序用x軸
if ((17 <= mainDirectionLineGroupId && mainDirectionLineGroupId <= 18) || (7 <= mainDirectionLineGroupId && mainDirectionLineGroupId <= 9))
{
var orderedMainLines = from line in linesHistogram[(LineQuantification)mainDirectionLineGroupId] orderby(line.Line.P1.X + line.Line.P2.X) / 2 select line;
foreach (LineEquation line in orderedMainLines)
{
orderedLines.Add(line);
}
}
else
{
var orderedMainLines = from line in linesHistogram[(LineQuantification)mainDirectionLineGroupId] orderby(line.Line.P1.Y + line.Line.P2.Y) / 2 select line;
foreach (LineEquation line in orderedMainLines)
{
orderedLines.Add(line);
}
}
foreach (LineEquation line in orderedLines)
{
int lineCenterY = (line.Line.P1.Y + line.Line.P2.Y) / 2;
int lineCenterX = (line.Line.P1.X + line.Line.P2.X) / 2;
if (!currentPoint.IsEmpty)
{
prePoint = currentPoint;
}
currentPoint = new Point(lineCenterX, lineCenterY);
//兩點 =>存放線條,並繪製
if (!currentPoint.IsEmpty && !prePoint.IsEmpty)
{
LineSegment2DF scanline = new LineSegment2DF(prePoint, currentPoint);
if (drawScanLineImg != null)
{
drawScanLineImg.Draw(scanline, Utilities.LineColors[index % Utilities.LineColors.Length], 2);
}
//記錄每一條線段
crossingConnectionlines.Add(scanline);
//Console.WriteLine("draw Line:direction ,x = " + scanline.Direction.X + "y =" + scanline.Direction.Y + ",point p1.x =" + prePoint.X + ",p1.y = " + prePoint.Y + ", p2.x =" + currentPoint.X + ",p2.y = " + currentPoint.Y);
}
//Console.WriteLine("-------------------------------------");
index++;
}
//統計黑白像素與判斷是否每條線段為白黑白的特徵
bool isBlackWhiteCrossing = DoBlackWhiteStatisticsByScanLine(crossingConnectionlines, processingImg, stasticDst);
if (isBlackWhiteCrossing && linesHistogram[(LineQuantification)mainDirectionLineGroupId].Count > 3)
{
//Console.WriteLine("共有" + linesHistogram[(LineQuantification)mainDirectionLineGroupId].Count + "相似斜率線段");
return(true);
}
else
{
return(false);
}
}
/// <summary>
/// Main method for processing the image
/// </summary>
/// <param name="input"></param>
private void ProcessFrame(Image <Bgr, Byte> input)
{
//input._EqualizeHist();
if (prevgrayframe == null)
{
prevgrayframe = input.Convert <Gray, Byte>();
preFeatures = prevgrayframe.GoodFeaturesToTrack(1000, 0.05, 5.0, 3);
prevgrayframe.FindCornerSubPix(preFeatures, new Size(5, 5), new Size(-1, -1), new MCvTermCriteria(25, 1.5d)); //This just increase the accuracy of the points
//mixChannel_src = input.PyrDown().PyrDown().Convert<Hsv, float>()[0];
return;
}
grayframe = input.Convert <Gray, Byte>();
//apply the Optical flow
Emgu.CV.OpticalFlow.PyrLK(prevgrayframe, grayframe, preFeatures[0], new Size(10, 10), 3, criteria, out curFeatures, out status, out error);
Image <Gray, float> FlowX = new Image <Gray, float>(grayframe.Size),
FlowY = new Image <Gray, float>(grayframe.Size),
FlowAngle = new Image <Gray, float>(grayframe.Size),
FlowLength = new Image <Gray, float>(grayframe.Size),
FlowResult = new Image <Gray, float>(grayframe.Size);
#region Farneback method to display movement in colour intensity
//Same as bellow CvInvoke method but a bit simpler
Emgu.CV.OpticalFlow.Farneback(prevgrayframe, grayframe, FlowX, FlowY, 0.5, 1, 10, 2, 5, 1.1, OPTICALFLOW_FARNEBACK_FLAG.USE_INITIAL_FLOW);
//CvInvoke.cvShowImage("FlowX", FlowX); //Uncomment to see in external window
//CvInvoke.cvShowImage("FlowY", FlowY);//Uncomment to see in external window
//CvInvoke.cvWaitKey(1); //Uncomment to see in external window (NOTE: You only need this line once)
//CvInvoke Method
//IntPtr Flow = CvInvoke.cvCreateImage(grayframe.Size, Emgu.CV.CvEnum.IPL_DEPTH.IPL_DEPTH_32F, 2);
//CvInvoke.cvCalcOpticalFlowFarneback(prevgrayframe, grayframe, Flow, 0.5, 1, 10, 2, 5, 1.1, OPTICALFLOW_FARNEBACK_FLAG.USE_INITIAL_FLOW);
//CvInvoke.cvSplit(Flow, FlowX, FlowY, IntPtr.Zero, IntPtr.Zero);
//CvInvoke.cvShowImage("FlowFX", FlowX); //Uncomment to see in external window
//CvInvoke.cvShowImage("FlowFY", FlowY); //Uncomment to see in external window
//CvInvoke.cvWaitKey(1); //Uncomment to see in external window (NOTE: You only need this line once)
#region All this is accomplished in the region bellow
// for (int i = 0; i < FlowX.Width; i++)
// {
// for (int j = 0; j < FlowX.Height; j++)
// {
// FlowLength.Data[j, i, 0] = (float)(Math.Sqrt((FlowX.Data[j, i, 0] * FlowX.Data[j, i, 0]) + (FlowY.Data[j, i, 0] * FlowY.Data[j, i, 0]))); //Gradient
// if (FlowLength.Data[j, i, 0] < 0)
// {
// FlowAngle.Data[j, i, 0] = (float)(Math.Atan2(FlowY.Data[j, i, 0], FlowX.Data[j, i, 0]) * 180 / Math.PI);
// }
// else
// {
// FlowAngle.Data[j, i, 0] = (float)(Math.Atan2(FlowY.Data[j, i, 0], (FlowX.Data[j, i, 0] * -1)) * 180 / Math.PI);
// }
// //FlowResult.Data[j, i, 0] = FlowAngle.Data[j, i, 0] * FlowLength.Data[j, i, 0];
// FlowResult.Data[j, i, 0] = FlowLength.Data[j, i, 0] * 5;
// }
// }
// Image<Bgr, Byte> Result = new Image<Bgr, Byte>(grayframe.Size);
// CvInvoke.ApplyColorMap(FlowResult.Convert<Gray, Byte>(), Result, ColorMapType.Hot);
//// CvInvoke.cvShowImage("Flow Angle", FlowAngle.Convert<Gray,Byte>());//Uncomment to see in external window
//// CvInvoke.cvShowImage("Flow Length", FlowLength.Convert<Gray, Byte>());//Uncomment to see in external window
// CvInvoke.cvShowImage("Flow Angle Colour", Result);//Uncomment to see in external window
#endregion
#region This code is much simpler
//Find the length for the whole array
FlowY = FlowY.Mul(FlowY); //Y*Y
FlowX = FlowX.Mul(FlowX); //X*X
FlowResult = FlowX + FlowY; //X^2 + Y^2
CvInvoke.cvSqrt(FlowResult, FlowResult); //SQRT(X^2 + Y^2)
//Apply a colour map.
Image <Bgr, Byte> Result = new Image <Bgr, Byte>(grayframe.Size); //store the result
CvInvoke.ApplyColorMap(FlowResult.Convert <Gray, Byte>() * 5, Result, ColorMapType.Hot); //Scale the FlowResult by a factor of 5 for a better visual difference
CvInvoke.cvShowImage("Flow Angle Colour II", Result); //Uncomment to see in external window
CvInvoke.cvWaitKey(1); //Uncomment to see in external window (NOTE: You only need this line once)
#endregion
#endregion
prevgrayframe = grayframe.Copy(); //copy current frame to previous
//Image<Gray, float> mixCahnnel_dest2 = Histo.BackProjectPatch<float>(new Image<Gray, float>[] { input.PyrDown().PyrDown().Convert<Hsv, float>()[0] }, new Size(1, 1), HISTOGRAM_COMP_METHOD.CV_COMP_BHATTACHARYYA, 1.0);
//CvInvoke.cvShowImage("BackProjection", mixCahnnel_dest2);
//CvInvoke.cvWaitKey(1); //Uncomment to see in external window (NOTE: You only need this line once)
for (int i = 0; i < curFeatures.Length; i++)
{
LineSegment2DF line = new LineSegment2DF(preFeatures[0][i], curFeatures[i]);
double dx = Math.Abs(line.P1.X - line.P2.X);
double dy = Math.Abs(line.P1.Y - line.P2.Y);
double l = Math.Sqrt(dx * dx + dy * dy);
double spinSize = 0.1 * l;
if (l > 5 && l < 100)
{
frame.Draw(line, new Bgr(Color.Red), 2);
double angle = Math.Atan2((double)line.P1.Y - line.P2.Y, (double)line.P1.X - line.P2.X);
Point Tip1 = new Point((int)(line.P2.X + spinSize * Math.Cos(angle + 3.1416 / 4)), (int)(line.P2.Y + spinSize * Math.Sin(angle + 3.1416 / 4)));
Point Tip2 = new Point((int)(line.P2.X + spinSize * Math.Cos(angle - 3.1416 / 4)), (int)(line.P2.Y + spinSize * Math.Sin(angle - 3.1416 / 4)));
LineSegment2DF line1 = new LineSegment2DF(Tip1, curFeatures[i]);
LineSegment2DF line2 = new LineSegment2DF(Tip2, curFeatures[i]);
frame.Draw(line1, new Bgr(Color.Blue), 2);
frame.Draw(line2, new Bgr(Color.Blue), 2);
}
//int range = 20;
//if (preFeatures[0][i].X > curFeatures[i].X - range && preFeatures[0][i].X < curFeatures[i].X + range) preFeatures[0][i].X = curFeatures[i].X;
//if (preFeatures[0][i].Y > curFeatures[i].Y - range && preFeatures[0][i].Y < curFeatures[i].Y + range) preFeatures[0][i].Y = curFeatures[i].Y;
}
preFeatures = prevgrayframe.GoodFeaturesToTrack(1000, 0.05, 5.0, 3);
prevgrayframe.FindCornerSubPix(preFeatures, new Size(5, 5), new Size(-1, -1), new MCvTermCriteria(25, 1.5d)); //This just increase the accuracy of the points
/*---------------------------------------------*/
DisplayImage(input.ToBitmap(), PCBX_Image); //thread safe display for camera cross thread errors
}
public static void ResetGameFromSavePointByMenu()
{
IsInGame = false;
new Thread(() =>
{
SystemManager.Instance.App.Controller.Press(Interop.XINPUT_GAMEPAD_BUTTONS.START, 10);
SystemManager.Instance.App.Controller.Flush();
while (!SystemManager.Instance.IndicatorHost.Menu.IsInMenu)
{
Thread.Sleep(100);
}
Thread.Sleep(800);
SystemManager.Instance.App.Controller.Press(Interop.XINPUT_GAMEPAD_BUTTONS.A, 10);
SystemManager.Instance.App.Controller.Flush();
Thread.Sleep(1200);
var location = SystemManager.Instance.IndicatorHost.Menu.Location;
while (location == default(PointF))
{
// Trace.WriteLine("wait for location");
Thread.Sleep(400);
location = SystemManager.Instance.IndicatorHost.Menu.Location;
}
SystemManager.Instance.App.Controller.Press(Interop.XINPUT_GAMEPAD_BUTTONS.B, 10);
SystemManager.Instance.App.Controller.Flush();
Thread.Sleep(900);
var line = new LineSegment2DF(Timeline.CurrentLocation, location);
Trace.WriteLine($"MOVE: {Math.Round(line.Length)} {Math.Round(Math2.GetPolarHeadingFromLine(line))}");
Trace.WriteLine($"Location: {location}");
// Trace.WriteLine("now in menu!");
while (!SystemManager.Instance.IndicatorHost.Menu.SelectedMenuItem.Contains("GAME"))
{
// Trace.WriteLine("SELECTED: " + SystemManager.Instance.IndicatorHost.Menu.SelectedMenuItem);
SystemManager.Instance.App.Controller.Press(Interop.XINPUT_GAMEPAD_BUTTONS.DPAD_LEFT, 10);
SystemManager.Instance.App.Controller.Flush();
Thread.Sleep(500);
}
// Trace.WriteLine("now in game!");
Thread.Sleep(200);
SystemManager.Instance.App.Controller.Press(Interop.XINPUT_GAMEPAD_BUTTONS.A, 10);
SystemManager.Instance.App.Controller.Flush();
// Trace.WriteLine("now game menu list");
while (SystemManager.Instance.IndicatorHost.Menu.SelectedGameMenuItem != "LOADGAME")
{
// Trace.WriteLine("SELECTED: " + SystemManager.Instance.IndicatorHost.Menu.SelectedGameMenuItem);
SystemManager.Instance.App.Controller.Press(Interop.XINPUT_GAMEPAD_BUTTONS.DPAD_DOWN, 10);
SystemManager.Instance.App.Controller.Flush();
Thread.Sleep(500);
}
// Trace.WriteLine("now save list");
SystemManager.Instance.App.Controller.Press(Interop.XINPUT_GAMEPAD_BUTTONS.A, 10);
SystemManager.Instance.App.Controller.Flush();
Thread.Sleep(800);
SystemManager.Instance.App.Controller.Press(Interop.XINPUT_GAMEPAD_BUTTONS.A, 10);
SystemManager.Instance.App.Controller.Flush();
Thread.Sleep(800);
SystemManager.Instance.App.Controller.Press(Interop.XINPUT_GAMEPAD_BUTTONS.A, 10);
SystemManager.Instance.App.Controller.Flush();
Thread.Sleep(800);
SystemManager.Instance.App.Controller.Press(Interop.XINPUT_GAMEPAD_BUTTONS.A, 10);
SystemManager.Instance.App.Controller.Flush();
// Trace.WriteLine("now game should be loading");
while (!SystemManager.Instance.IndicatorHost.Loading.IsLoading)
{
// Trace.WriteLine("wait for loading " + SystemManager.Instance.IndicatorHost.Loading.LoadingTextRead);
Thread.Sleep(1000);
}
// Trace.WriteLine("confirm loading!");
while (SystemManager.Instance.IndicatorHost.Loading.IsLoading)
{
// Trace.WriteLine("wait for no loading " + SystemManager.Instance.IndicatorHost.Loading.LoadingTextRead);
Thread.Sleep(1000);
}
SystemManager.Instance.FlightPlan.CurrentIndex = 0;
Reset();
Trace.WriteLine("GAME READY!");
Timeline.Begin();
Thread.Sleep(2000);
SystemManager.Instance.MCP.IAS = 120;
SystemManager.Instance.MCP.ALT = 1200;
SystemManager.Instance.MCP.AltitudeHold = true;
SystemManager.Instance.MCP.LNAV = true;
SystemManager.Instance.MCP.IASHold = true;
}).Start();
}
public static void UpdateLocationFromMenu()
{
IsInGame = false;
new Thread(() =>
{
//Trace.WriteLine("EnterMenu");
SystemManager.Instance.App.Controller.Press(Interop.XINPUT_GAMEPAD_BUTTONS.RIGHT_SHOULDER, 0);
SystemManager.Instance.App.Controller.Press(Interop.XINPUT_GAMEPAD_BUTTONS.LEFT_SHOULDER, 0);
SystemManager.Instance.App.Controller.Press(Interop.XINPUT_GAMEPAD_BUTTONS.START, 12);
SystemManager.Instance.App.Controller.Flush();
while (!SystemManager.Instance.IndicatorHost.Menu.IsInMenu)
{
Thread.Sleep(100);
}
// Trace.WriteLine("now in menu!");
Thread.Sleep(800);
SystemManager.Instance.App.Controller.Press(Interop.XINPUT_GAMEPAD_BUTTONS.A, 12);
SystemManager.Instance.App.Controller.Flush();
Thread.Sleep(1200);
// Trace.WriteLine("now in selected map");
var location = SystemManager.Instance.IndicatorHost.Menu.Location;
while (location == default(PointF))
{
// Trace.WriteLine("wait for location");
Thread.Sleep(400);
location = SystemManager.Instance.IndicatorHost.Menu.Location;
}
Thread.Sleep(700);
SystemManager.Instance.App.Controller.Press(Interop.XINPUT_GAMEPAD_BUTTONS.B, 18);
SystemManager.Instance.App.Controller.Flush();
Thread.Sleep(1000);
SystemManager.Instance.App.Controller.Press(Interop.XINPUT_GAMEPAD_BUTTONS.B, 18);
SystemManager.Instance.App.Controller.Flush();
var final_sleep = (int)(18 * (1000 / SystemManager.Instance.App.Controller.XInput_In.Fps));
var line = new LineSegment2DF(Timeline.CurrentLocation, location);
Trace.WriteLine($"MOVE: {Math.Round(line.Length)}");
Trace.WriteLine($"Location: {location} sleep={final_sleep}");
CurrentLocation = location;
for (var i = 1; i < 5; i++)
{
Data[LatestFrameId - i].Seconds = Duration.Elapsed.Add(TimeSpan.FromMilliseconds(final_sleep)).TotalSeconds;
Data[LatestFrameId - i].Location = CurrentLocation;
Data[LatestFrameId - i].Roll.Value = RollAvg;
Data[LatestFrameId - i].Pitch.Value = PitchAvg;
Data[LatestFrameId - i].IsLocationCalculated = true;
Data[LatestFrameId - i].IsResetLocation = true;
Data[LatestFrameId - i].IsDataComplete = true;
}
while (Duration.Elapsed.TotalSeconds < Data[LatestFrameId - 1].Seconds)
{
Thread.Sleep(1);
}
// Thread.Sleep(final_sleep);
// 500 is pretty ok!
SystemManager.Instance.Computer._rollPid.ClearError();
SystemManager.Instance.Computer._pitchPid.ClearError();
Resume();
}).Start();
}
private void FindSpine(LineSegment2D[] lines, RotatedRect rotatedRectangle, Image <Gray, Byte> img)
{
LineSegment2DF[] initialLines = new LineSegment2DF[2];
if (!rotatedRectangle.Size.IsEmpty)
{
//Use one of the smaller boundries from rotatedRect for initial detection
PointF[] vertices = rotatedRectangle.GetVertices();
PointF p1 = vertices[0];
PointF p2 = vertices[1];
PointF p3 = vertices[2];
PointF p4 = vertices[3];
if (p2.DistanceSquared(p1) < p2.DistanceSquared(p3))
{
//p1 and p2 are paired, p3 and p4 are paired
initialLines[0] = new LineSegment2DF(p1, p2);
initialLines[1] = new LineSegment2DF(p3, p4);
}
else
{
//p2 and p3 are paired, p1 and p4 are paired
initialLines[0] = new LineSegment2DF(p2, p3);
initialLines[1] = new LineSegment2DF(p1, p4);
}
}
else
{
//Use one of the image sides for intial detection
initialLines[0] = new LineSegment2DF(new PointF(0, 0), new PointF(0, img.Height - 1));
initialLines[1] = new LineSegment2DF(new PointF(img.Width - 1, 0), new PointF(img.Width - 1, img.Height - 1));
}
//Find closest line segment to initial line
double minDistance = double.MaxValue;
LineSegment2D?targetLine = null;
foreach (var line in lines)
{
double minDistance1 = MathExtension.MinDistanceFromLineToPoint(initialLines[0].P1, initialLines[0].P2, line.P1);
double minDistance2 = MathExtension.MinDistanceFromLineToPoint(initialLines[0].P1, initialLines[0].P2, line.P2);
double currentDist = minDistance1 < minDistance2 ? minDistance1 : minDistance2;
if (currentDist < minDistance)
{
minDistance = currentDist;
targetLine = line;
}
}
List <LineSegment2D> previousLines = new List <LineSegment2D>();
//We have our target line, try to traverse to the other side
LineSegment2D?nextLine = null;
Image <Bgr, Byte> moddedImage = img.Convert <Bgr, Byte>();
if (targetLine.HasValue)
{
previousLines.Add(targetLine.Value);
//We have a starting position, lets test it!
moddedImage.Draw(targetLine.Value, new Bgr(Color.Red), 2);
}
do
{
GetValue(lines, initialLines[1], previousLines.ToArray(), targetLine, ref nextLine);
if (nextLine.HasValue)
{
targetLine = nextLine;
previousLines.Add(nextLine.Value);
moddedImage.Draw(nextLine.Value, new Bgr(Color.Red), 2);
}
}while (nextLine.HasValue);
DisplayImage3 = ImageService.ToBitmapSource(moddedImage);
}
internal static double GetPolarHeadingFromLine(LineSegment2DF targetLine)
{
return(GetPolarHeadingFromLine(targetLine.P1, targetLine.P2));
}
public DetectResult DetectSurf(Image <Bgr, byte> imgToFind, Image <Bgr, byte> imgScene, out long matchTime)
{
using (var matches = new VectorOfVectorOfDMatch())
{
matchTime = 0;
try
{
Mat mask;
Mat homography;
VectorOfKeyPoint modelKeyPoints;
VectorOfKeyPoint observedKeyPoints;
FindMatch(imgToFind.Mat, imgScene.Mat, out matchTime, out modelKeyPoints, out observedKeyPoints, matches, out mask, out homography);
float sharpnessValue = this.MatchesCount(matches, mask);
//Draw the matched keypoints
//var result = imgScene.Mat;
//var result = new Mat();
//Features2DToolbox.DrawMatches(imgToFind.Mat, modelKeyPoints, imgScene.Mat, observedKeyPoints,
//matches, result, new MCvScalar(255, 255, 255), new MCvScalar(255, 255, 255), mask);
#region draw the projected region on the image
if (homography != null && sharpnessValue > 5)
{
//draw a rectangle along the projected model
var rect = new Rectangle(Point.Empty, imgToFind.Size);
var p1 = new PointF(rect.Left, rect.Bottom);
var p2 = new PointF(rect.Right, rect.Bottom);
var p3 = new PointF(rect.Right, rect.Top);
var p4 = new PointF(rect.Left, rect.Top);
var pts = new[] { p1, p2, p3, p4 };
pts = CvInvoke.PerspectiveTransform(pts, homography);
//check if any opposite lines intersect
//if so, then don't add to final results
//we should never have 2 opposite sides intersecting
var l1 = new LineSegment2DF(pts[0], pts[1]);
var l2 = new LineSegment2DF(pts[1], pts[2]);
var l3 = new LineSegment2DF(pts[2], pts[3]);
var l4 = new LineSegment2DF(pts[3], pts[0]);
if (pts.All(x => x.X >= 0 && x.Y >= 0)) // whole template should be on imageFrame
{
if (!(Intersects2Df(l1, l3) || Intersects2Df(l2, l4))) // opposite lines must not intersects
{
//var maxScale = 1.3;
//var scaleHorisontal = l1.Length > l3.Length ? l1.Length / l3.Length : l3.Length / l1.Length;
//var scaleVertical = l2.Length > l4.Length ? l2.Length / l4.Length : l4.Length / l2.Length;
//if (scaleHorisontal < maxScale && scaleVertical < maxScale)
{
//var points = Array.ConvertAll(pts, Point.Round);
//using (var vp = new VectorOfPoint(points))
//{
// CvInvoke.Polylines(result, vp, true, new MCvScalar(255, 0, 0, 255), 5);
//}
var minX = pts.Min(x => x.X);
var maxX = pts.Max(x => x.X);
var minY = pts.Min(x => x.Y);
var maxY = pts.Max(x => x.Y);
var width = (int)Math.Ceiling(maxX - minX);
var height = (int)Math.Ceiling(maxY - minY);
if ((minX + width <= imgScene.Width) && (minY + height <= imgScene.Height))
{
var imageRect = new Rectangle((int)Math.Ceiling(minX), (int)Math.Ceiling(minY), width, height);
// var imageBitmap = imgScene.Copy(imageRect);
return(new DetectResult
{
DetectedRectangle = imageRect,
// DetectedImage = imageBitmap,
SharpnessValue = sharpnessValue
});
}
}
}
}
}
#endregion
}
catch (Exception ex)
{
MessageBox.Show(ex.Message);
}
return(null);
}
}
/// <summary>
/// Draw a line segment in the map
/// </summary>
/// <param name="line">The line to be draw</param>
/// <param name="color">The color for the line</param>
/// <param name="thickness">The thickness of the line</param>
/// <param name="lineType">Line type</param>
/// <param name="shift">Number of fractional bits in the center coordinates and radius value</param>
public override void Draw(LineSegment2DF line, TColor color, int thickness, CvEnum.LineType lineType = CvEnum.LineType.EightConnected, int shift = 0)
{
base.Draw(new LineSegment2DF(MapPointToImagePoint(line.P1), MapPointToImagePoint(line.P2)), color, thickness, lineType, shift);
}
private double ReadRollAngleFromRingImage(Image <Gray, byte> img, Image <Bgr, byte> markupImage, DebugState debugState)
{
var circles = CvInvoke.HoughCircles(img, HoughType.Gradient, 2.0, 1.0, 1, 10, 50, 53);
if (circles.Length > 0)
{
var cir = circles[0];
var radius = cir.Radius;
Func <int, int, bool> check = (int cX, int cY) =>
{
if (cX < img.Width && cX > 0 &&
cY < img.Height && cY > 0)
{
var b = img[cY, cX];
return(b.Intensity > 0);
}
return(false);
};
Func <int, int, bool> check2 = (int cX, int cY) =>
{
return(check(cX - 1, cY) ||
check(cX + 1, cY) ||
check(cX - 3, cY) ||
check(cX + 3, cY) ||
check(cX - 5, cY) ||
check(cX + 5, cY) ||
check(cX, cY + 2) ||
check(cX, cY - 2));
};
// From 6PM to 12PM clockwise
PointF leftPoint = default(PointF);
for (double t = Math.PI / 2; t < Math.PI * 1.5; t += 0.05f)
{
var cX_ = (int)(radius * Math.Cos(t) + cir.Center.X);
var cY_ = (int)(radius * Math.Sin(t) + cir.Center.Y);
//CvInvoke.Circle(focus, new Point(cX_, cY_), 1, new Bgr(Color.Red).MCvScalar, 1);
if (check2(cX_, cY_))
{
leftPoint = new PointF(cX_, cY_);
break;
}
}
// From 6PM to 12PM counter-clockwise.
PointF rightPoint = default(PointF);
for (double t = Math.PI / 2; t > -0.5 * Math.PI; t -= 0.05f)
{
var cX_ = (int)(radius * Math.Cos(t) + cir.Center.X);
var cY_ = (int)(radius * Math.Sin(t) + cir.Center.Y);
//CvInvoke.Circle(focus, new Point(cX_, cY_), 1, new Bgr(Color.Green).MCvScalar, 1);
if (check2(cX_, cY_))
{
rightPoint = new PointF(cX_, cY_);
break;
}
}
if (leftPoint != default(PointF) && rightPoint != default(PointF))
{
CvInvoke.Line(markupImage, rightPoint.ToPoint(), leftPoint.ToPoint(), new Bgr(Color.Yellow).MCvScalar, 2);
var horizontalNeedleLine = new LineSegment2DF(leftPoint, rightPoint);
if (horizontalNeedleLine.Length < 88 || horizontalNeedleLine.Length > 140)
{
debugState.SetError($"ROLL: Dist {horizontalNeedleLine.Length}");
return(double.NaN);
}
var angle = Math2.GetPolarHeadingFromLine(horizontalNeedleLine) - 270;
// skew considered from other panels
return(angle + 1);
}
else
{
debugState.SetError($"ROLL: Couldn't find boundary");
}
}
else
{
debugState.SetError($"ROLL: boundary circles");
}
return(double.NaN);
}
