private double CalculateResultFromAllResults(List <double> ret, DebugState debugState)
{
if (ret.Count == 0)
{
debugState.SetError("zero result count");
return(double.NaN);
}
ret.Sort();
var finalKnots = ret.Average();
if (ret.Count > 3)
{
finalKnots = ret[ret.Count / 2];
}
if (!IsValueInExpectedRange(finalKnots) && num_rejected_values < 10)
{
num_rejected_values++;
debugState.SetError($"REJECT: kt={finalKnots} lastValue={Timeline.Speed}");
return(double.NaN);
}
num_rejected_values = 0;
return(finalKnots);
}
public double ReadValue(IndicatorData data, DebugState debugState)
{
if (TryFindCircleInFullFrame(data, out var circle))
{
var focus = data.Frame.Copy(Math2.CropCircle(circle, 10));
debugState.Add(focus);
List <double> ret = new List <double>();
foreach (var line in GetLinesFromFocusImage(focus, circle, debugState))
{
CvInvoke.Line(focus, line.P1, line.P2, new Bgr(Color.Yellow).MCvScalar, 2);
var knots = ReadKnotsFromNeedleLine(line);
if (IsValueInExpectedRange(knots))
{
ret.Add(knots);
}
}
return(CalculateResultFromAllResults(ret, debugState));
}
else
{
debugState.SetError("No circles");
}
return(double.NaN);
}
public double ReadValue(IndicatorData data, DebugState debugState)
{
if (TryFindRollCircleInFullFrame(data, out CircleF rollIndicatorCicle))
{
var FocusRect = Math2.CropCircle(rollIndicatorCicle, 10);
var focus = data.Frame.SafeCopy(FocusRect);
debugState.Add(focus);
// Isolate the outside ring
Mat maskInnerAlt = new Mat(focus.Size, DepthType.Cv8U, 3);
maskInnerAlt.SetTo(new MCvScalar(1));
CvInvoke.Circle(maskInnerAlt, new Point(focus.Size.Width / 2, focus.Size.Height / 2), (int)(rollIndicatorCicle.Radius - (rollIndicatorCicle.Radius * 0.2)), new Bgr(Color.White).MCvScalar, -1);
CvInvoke.Circle(maskInnerAlt, new Point(focus.Size.Width / 2, focus.Size.Height / 2), (int)(rollIndicatorCicle.Radius - (rollIndicatorCicle.Radius * 0.275)), new Bgr(Color.Black).MCvScalar, -1);
var outerMovingRingOnly = focus.Copy(maskInnerAlt.ToImage <Gray, byte>());
var outerMovingRingWithoutBottom = outerMovingRingOnly.Copy(new Rectangle(0, 0, outerMovingRingOnly.Width, (int)(outerMovingRingOnly.Height))); // - (outerMovingRingOnly.Height * 0.29)
var ring_hsv_unfiltered = outerMovingRingWithoutBottom.Convert <Hsv, byte>().InRange(new Hsv(20, 0, 85), new Hsv(180, 255, 255));
var ring_hsv = Utils.RemoveBlobs(ring_hsv_unfiltered, 1, 500);
debugState.Add(outerMovingRingOnly);
debugState.Add(outerMovingRingWithoutBottom);
debugState.Add(ring_hsv);
debugState.Add(ring_hsv_unfiltered);
return(ReadRollAngleFromRingImage(ring_hsv, focus, debugState));
}
else
{
debugState.SetError($"ROLL: Main circles");
}
return(double.NaN);
}
public double ReadValue(IndicatorData data, DebugState debugState)
{
if (TryFindCircleInFullFrame(data, out var circle))
{
var focus = data.Frame.SafeCopy(Math2.CropCircle(circle, 15));
var vs_blackimg = focus.Convert <Hsv, byte>().DynLowInRange(dyn_lower, new Hsv(180, 255, 255));
debugState.Add(focus);
debugState.Add(vs_blackimg);
int margin = 10;
var vspeedMask = new Mat(focus.Size, DepthType.Cv8U, 3);
vspeedMask.SetTo(new MCvScalar(1));
CvInvoke.Circle(vspeedMask, Point.Round(new PointF(circle.Radius + margin, circle.Radius + margin)), (int)(circle.Radius - (circle.Radius * 0.1)), new Bgr(Color.White).MCvScalar, -1);
vs_blackimg = vs_blackimg.Copy(vspeedMask.ToImage <Gray, byte>());
var vspeed_inner_only = vs_blackimg.Copy(new Rectangle(0, 0, vs_blackimg.Width / 2, vs_blackimg.Height));
debugState.Add(vspeed_inner_only);
Rectangle center = GetCenterBoxFromImage(focus);
foreach (var line in CvInvoke.HoughLinesP(vspeed_inner_only, 1, Math.PI / 45.0, 20, 20, 14))
{
if (center.Contains(line.P1) || center.Contains(line.P2))
{
CvInvoke.Line(focus, line.P1, line.P2, new Bgr(Color.Yellow).MCvScalar, 2);
LineSegment2D needleLine;
if (center.Contains(line.P1))
{
needleLine = new LineSegment2D(line.P2, line.P1);
}
else
{
needleLine = new LineSegment2D(line.P1, line.P2);
}
var angle = (Math2.GetPolarHeadingFromLine(needleLine) - 270);
// bias up to account for skew
angle += 2.75;
if (line.Length > 63)
{
debugState.SetError($"Rejected length: {line.Length}");
}
return(angle);
}
}
}
return(double.NaN);
}
public double ReadValue(IndicatorData data, DebugState debugState)
{
if (TryFindCircleInFullFrame(data, out var circ))
{
var focusRect = Math2.CropCircle(circ, 15);
var focus = data.Frame.SafeCopy(focusRect);
var focusHsv = focus.Convert <Hsv, byte>();
var focusHsvText = focusHsv.DynLowInRange(dyn_lower, new Hsv(180, 255, 255));
var focusHsvTriangleMask = focusHsv.InRange(new Hsv(0, 0, 0), new Hsv(180, 140, 255));
var focusHsvTextOnly = focusHsvText.Copy(focusHsvTriangleMask);
debugState.Add(focus);
var blobs = Utils.DetectAndFilterBlobs(focusHsvTextOnly, 25, 250).
Where(b => b.Centroid.Y >= 5).OrderByDescending(b => b.Area).Take(4);
var focusHsvOnlyBlobs = Utils.RemoveAllButBlobs(focusHsvTextOnly, blobs);
debugState.Add(focusHsvOnlyBlobs);
var parts = GetPacksFromImage(focusHsvOnlyBlobs, blobs, debugState);
var ret = ComputeHeadingFromPacks(data.Id, parts, focus, debugState);
if (!double.IsNaN(ret))
{
// Adjust now that N and S are near-vertical and the blobs are more regular.
ret = RefineAngle(ret, focusHsvOnlyBlobs);
// Adjust based on the dots on the glareshield.
ret += GetGlareShieldSkewAngle(focusRect, data, debugState);
// Add a fixed cab skew to account for the vertical perspective.
ret -= 1;
ret = Math2.ClampAngle(ret);
// Proof for the debug info.
debugState.Add(focus.Rotate(ret, new Bgr(0, 0, 0)));
return(CheckResultValidity(ret, data.Id));
}
}
else
{
debugState.SetError("Couldn't find initial circle.");
}
return(double.NaN);
}
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);
}
double ComputeHeadingFromPacks(int frameId, IEnumerable <BlobPack> packs, Image <Bgr, Byte> compass_frame, DebugState debugState)
{
var frame = Timeline.Data[frameId];
var my_extended = new ExtendedData();
frame.Heading.ForIndicatorUse = my_extended;
var choices = new List <Tuple <double, double, string, Image <Gray, byte> > >();
double unused_angle = 0;
var p = packs.OrderByDescending(px => px.BlobRationAngle);
foreach (var pack in packs)
{
var b = pack.BlobBox;
b.Inflate(2, 2);
var small_angle = pack.BlobRationAngle;
var str = ResolveTextFromPreviousFrame(frameId, small_angle);
if (string.IsNullOrWhiteSpace(str))
{
str = GetText(pack.BlobImage, debugState);
}
if (str == "N" || str == "E" || str == "S" || str == "W")
{
small_angle = 360 - Math.Abs(small_angle);
double new_heading = 0;
switch (str)
{
case "N":
my_extended.LastN = pack.BlobRationAngle;
new_heading = small_angle;
CvInvoke.Rectangle(compass_frame, b, new Bgr(Color.Blue).MCvScalar, 1);
break;
case "E":
my_extended.LastE = pack.BlobRationAngle;
new_heading = (small_angle + 90);
CvInvoke.Rectangle(compass_frame, b, new Bgr(Color.Yellow).MCvScalar, 1);
break;
case "S":
my_extended.LastS = pack.BlobRationAngle;
new_heading = (small_angle + 180);
CvInvoke.Rectangle(compass_frame, b, new Bgr(Color.Red).MCvScalar, 1);
break;
case "W":
my_extended.LastW = pack.BlobRationAngle;
new_heading = (small_angle + 270);
CvInvoke.Rectangle(compass_frame, b, new Bgr(Color.Lime).MCvScalar, 1);
break;
}
new_heading = Math2.ClampAngle(new_heading);
choices.Add(new Tuple <double, double, string, Image <Gray, byte> >(new_heading, small_angle, str, pack.BlobImage));
}
else
{
unused_angle = pack.BlobRationAngle;
}
}
// Fill in exactly one missing quadrant.
if (choices.Count == 3 && packs.Count() == 4)
{
var letters = new List <string>()
{
"N", "E", "S", "W"
};
foreach (var c in choices)
{
letters.Remove(c.Item3);
}
var o_angle = unused_angle;
unused_angle = 360 - Math.Abs(unused_angle);
double new_heading = 0;
var str = letters.First();
switch (str)
{
case "N":
my_extended.LastN = o_angle;
new_heading = unused_angle;
break;
case "E":
my_extended.LastE = o_angle;
new_heading = (unused_angle + 90);
break;
case "S":
my_extended.LastS = o_angle;
new_heading = (unused_angle + 180);
break;
case "W":
my_extended.LastW = o_angle;
new_heading = (unused_angle + 270);
break;
}
new_heading = Math2.ClampAngle(new_heading);
choices.Add(new Tuple <double, double, string, Image <Gray, byte> >(new_heading, (int)unused_angle, str, null));
}
if (choices.Count == 4)
{
// Exclude invalid combinations
if (choices.Where(ct => ct.Item3 == "N").Count() > 1)
{
debugState.SetError("Bad N");
return(double.NaN);
}
if (choices.Where(ct => ct.Item3 == "E").Count() > 1)
{
debugState.SetError("Bad E");
return(double.NaN);
}
if (choices.Where(ct => ct.Item3 == "S").Count() > 1)
{
debugState.SetError("Bad S");
return(double.NaN);
}
if (choices.Where(ct => ct.Item3 == "W").Count() > 1)
{
debugState.SetError("Bad W");
return(double.NaN);
}
var p1 = Math2.AddAngles(choices[0].Item1, choices[1].Item1);
var p2 = Math2.AddAngles(choices[2].Item1, choices[3].Item1);
return(Math2.AddAngles(p1, p2));
}
else
{
debugState.SetError($"Bad choices {choices.Count}");
}
return(double.NaN);
}
private double GetGlareShieldSkewAngle(Rectangle focusRect, IndicatorData data, DebugState debugState)
{
var topDotRect = new Rectangle(focusRect.Left - 160, focusRect.Top - 5, 40, 40);
var bottomDotRect = new Rectangle(focusRect.Left - 170, focusRect.Bottom - 40, 60, 40);
var topPointBlobs = GetDotLocationFromFullFrame(data.Frame, focusRect, topDotRect, isTop: true);
var bottomPointBlobs = GetDotLocationFromFullFrame(data.Frame, focusRect, bottomDotRect, isTop: false);
double a = double.NaN;
if (topPointBlobs.Count > 0 && bottomPointBlobs.Count > 0)
{
var topPoint = topPointBlobs.First().Centroid.Add(topDotRect.Location);
var bottomPoint = bottomPointBlobs.First().Centroid.Add(bottomDotRect.Location);
a = Math2.GetPolarHeadingFromLine(topPoint, bottomPoint);
if (a > 180)
{
a = 360 - a;
a *= -1;
}
var lineRect = new Rectangle(focusRect.Left - 200, focusRect.Top - 50, 170, focusRect.Bottom + 50 - focusRect.Top);
var lineImg = data.Frame.Copy(lineRect);
var topInLineImg = new PointF(topPoint.X - lineRect.Location.X, topPoint.Y - lineRect.Location.Y);
var bottomInLineImg = new PointF(bottomPoint.X - lineRect.Location.X, bottomPoint.Y - lineRect.Location.Y);
var topRange = lineImg.Convert <Hsv, byte>(); //.InRange(new Hsv(HLow, SLow, VLow), new Hsv(HHigh, SHigh, VHigh));
CvInvoke.Line(lineImg, topInLineImg.ToPoint(), bottomInLineImg.ToPoint(), new Bgr(Color.Yellow).MCvScalar, 1);
debugState.Add(lineImg);
debugState.Add(topRange);
var dist = Math2.GetDistance(topPoint, bottomPoint);
if (Math.Abs(a) > 8 || dist < 110 || dist > 120)
{
var biasFrame = Timeline.LatestFrame(f => f.Heading.ForIndicatorUse == null ? double.NaN : ((ExtendedData)f.Heading.ForIndicatorUse).Bias, data.Id);
if (biasFrame != null)
{
a = ((ExtendedData)biasFrame.Heading.ForIndicatorUse).Bias;
}
else
{
debugState.SetError("Rejected due to dots angle out of bounds " + a);
return(double.NaN);
}
}
}
else
{
var biasFrame = Timeline.LatestFrame(f => f.Heading.ForIndicatorUse == null ? double.NaN : ((ExtendedData)f.Heading.ForIndicatorUse).Bias, data.Id);
if (biasFrame != null)
{
a = ((ExtendedData)biasFrame.Heading.ForIndicatorUse).Bias;
}
else
{
debugState.SetError("Rejected due to dots angle out of bounds " + a);
return(double.NaN);
}
}
((ExtendedData)Timeline.Data[data.Id].Heading.ForIndicatorUse).Bias = a;
return(a / 2);
}
public double ReadValue(IndicatorData data, DebugState debugState)
{
if (TryFindCircleInFullFrame(data, out var circle))
{
var focus = data.Frame.SafeCopy(Math2.CropCircle(circle, 15));
var vs_blackimg = focus.Convert <Hsv, byte>().DynLowInRange(dyn_lower, new Hsv(180, 255, 255)).PyrUp().PyrDown();
var markedup_frame = vs_blackimg.Convert <Bgr, byte>();
debugState.Add(focus);
debugState.Add(vs_blackimg);
debugState.Add(markedup_frame);
var cannyEdges3 = new Mat();
CvInvoke.Canny(vs_blackimg, cannyEdges3, 10, 120);
Mat dialatedCanny = new Mat();
CvInvoke.Dilate(cannyEdges3, dialatedCanny, null, new Point(-1, -1), 1, BorderType.Default, new Gray(0).MCvScalar);
Rectangle center = GetCenterBoxFromImage(focus);
CvInvoke.Rectangle(markedup_frame, center, new Bgr(Color.Red).MCvScalar, 1);
var lines = CvInvoke.HoughLinesP(dialatedCanny, 1, Math.PI / 45.0, 20, 16, 0).OrderByDescending(l => l.Length);
foreach (var line in lines)
{
CvInvoke.Line(markedup_frame, line.P1, line.P2, new Bgr(Color.Red).MCvScalar, 1);
if (center.Contains(line.P1) || center.Contains(line.P2))
{
LineSegment2D needleLine;
if (center.Contains(line.P1))
{
needleLine = new LineSegment2D(line.P2, line.P1);
}
else
{
needleLine = new LineSegment2D(line.P1, line.P2);
}
var angle = Math2.GetPolarHeadingFromLine(needleLine);
var hundreds = Math.Round((angle / 360) * 999);
CvInvoke.Line(focus, needleLine.P1, needleLine.P2, new Bgr(Color.Yellow).MCvScalar, 2);
var candidates = new List <double>();
for (var i = 0; i < 9; i++)
{
candidates.Add(hundreds + (i * 1000));
}
var ret = candidates.OrderBy(c => Math.Abs(c - Timeline.Altitude)).First();
if (!double.IsNaN(Timeline.Altitude))
{
var delta = Math.Abs(ret - Timeline.Altitude);
if (delta > 400)
{
debugState.SetError($"Bad value {delta} > 400");
return(double.NaN);
}
}
else
{
return(0);
}
return(ret);
}
}
}
return(double.NaN);
}
