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);
}