/// <summary>
/// Adds a fiducial to the known fiducials of this board
/// </summary>
/// <param name="id">the id or code of the fiducial</param>
/// <param name="topL">This is the top left corner (highest y value and lowest x) of this fiducial </param>
/// <param name="width">the width of the fiducial square pattern in meters</param>
public void Add(int id, Point3f topL, float width, Point3f xDir, Point3f yDir)
{
var nx = xDir.Normalize() * width;
var ny = yDir.Normalize() * width;
var corners = new List <Point3f>();
var topR = new Point3f(topL.X + nx.X, topL.Y + nx.Y, topL.Z + nx.Z);
var bottomR = new Point3f(topL.X + nx.X + ny.X, topL.Y + nx.Y + ny.Y, topL.Z + nx.Z + ny.Z);
var bottomL = new Point3f(topL.X + ny.X, topL.Y + ny.Y, topL.Z + ny.Z);
corners.Add(topL);
corners.Add(topR);
corners.Add(bottomR);
corners.Add(bottomL);
var center = new Point3f(corners.Average(c => c.X), corners.Average(c => c.Y), corners.Average(c => c.Z));
CenterDefinitions.Add(id, center);
CornerDefinitions.Add(id, corners);
}