public void ReadLineXml(XmlReader r, PointF scaling)
{
r.ReadStartElement();
float length = 0;
SegmentF line = SegmentF.Empty;
while (r.NodeType == XmlNodeType.Element)
{
switch (r.Name)
{
case "Length":
length = float.Parse(r.ReadElementContentAsString(), CultureInfo.InvariantCulture);
break;
case "Segment":
line = ParseSegment(r, scaling);
break;
case "Origin":
// Import from older format.
origin = XmlHelper.ParsePointF(r.ReadElementContentAsString());
if (float.IsNaN(origin.X) || float.IsNaN(origin.Y))
{
origin = PointF.Empty;
}
origin = origin.Scale(scaling.X, scaling.Y);
break;
case "Scale":
// Import and convert from older format.
// Create a fake line of 100 px horizontal at the origin.
float bakedScale = float.Parse(r.ReadElementContentAsString(), CultureInfo.InvariantCulture);
float lengthPixels = 100;
PointF start = origin;
PointF end = origin.Translate(lengthPixels, 0);
line = new SegmentF(start, end);
length = lengthPixels * bakedScale;
// The actual origin should be expressed in the calibrated plane coordinate system, which has its true origin at the A point of the quad.
origin = new PointF(0, length);
break;
default:
string unparsed = r.ReadOuterXml();
log.DebugFormat("Unparsed content in KVA XML: {0}", unparsed);
break;
}
}
r.ReadEndElement();
// Update mapping.
size = new SizeF(length, length);
quadImage = MakeQuad(line.Start, line.End);
mapping.Update(new QuadrilateralF(size.Width, size.Height), quadImage);
valid = quadImage.IsConvex;
initialized = true;
}
public override void Draw(Graphics canvas, DistortionHelper distorter, IImageToViewportTransformer transformer, bool selected, long currentTimestamp)
{
double opacityFactor = infosFading.GetOpacityFactor(currentTimestamp);
if (opacityFactor <= 0)
{
return;
}
QuadrilateralF quad = transformer.Transform(quadImage);
int alpha = (int)(opacityFactor * 255);
using (Pen p = styleHelper.GetPen(alpha, transformer.Scale))
{
p.EndCap = LineCap.Square;
if (styleHelper.PenShape == PenShape.Dash)
{
p.DashStyle = DashStyle.Dash;
}
canvas.DrawLine(p, quad.A, quad.B);
canvas.DrawLine(p, quad.B, quad.C);
canvas.DrawLine(p, quad.C, quad.D);
canvas.DrawLine(p, quad.D, quad.A);
}
}
/// <summary>
/// Takes a circle in image space and returns a cooresponding ellipse in image space.
/// This is an ill posed problem. The center is respected but the radius could be taken
/// anywhere around the circle in image space, and that yields different radii in world space.
/// We use the point along the X axis as the radius.
/// </summary>
public Ellipse GetEllipseFromCircle(PointF center, float radius, out PointF radiusLeftInImage, out PointF radiusRightInImage)
{
radiusLeftInImage = new PointF(center.X - radius, center.Y);
radiusRightInImage = new PointF(center.X + radius, center.Y);
if (calibratorType == CalibratorType.Line)
{
return(new Ellipse(center, radius, radius, 0));
}
// Rebuild the world-space circle based on center and radius alone.
PointF centerInWorld = GetPoint(center);
// Estimate the radius in world space.
// Get scalar will assumes reference direction to be X-axis in image space.
float radiusInWorld = GetScalar(radius);
// Get the intersection points of a horizontal diameter.
// This is used to draw a line from the center to the outline of the ellipse in perspective.
radiusLeftInImage = GetImagePoint(centerInWorld.Translate(-radiusInWorld, 0));
radiusRightInImage = GetImagePoint(centerInWorld.Translate(radiusInWorld, 0));
// Get the square enclosing the circle for mapping.
PointF a = GetImagePoint(centerInWorld.Translate(-radiusInWorld, -radiusInWorld));
PointF b = GetImagePoint(centerInWorld.Translate(radiusInWorld, -radiusInWorld));
PointF c = GetImagePoint(centerInWorld.Translate(radiusInWorld, radiusInWorld));
PointF d = GetImagePoint(centerInWorld.Translate(-radiusInWorld, radiusInWorld));
QuadrilateralF quadImage = new QuadrilateralF(a, b, c, d);
ProjectiveMapping mapping = new ProjectiveMapping();
mapping.Update(QuadrilateralF.CenteredUnitSquare, quadImage);
return(mapping.Ellipse());
}
public DrawingPlane(PointF origin, long timestamp, long averageTimeStampsPerFrame, DrawingStyle preset = null)
{
// Decoration
styleHelper.Color = Color.Empty;
styleHelper.GridDivisions = 8;
styleHelper.Perspective = true;
styleHelper.ValueChanged += StyleHelper_ValueChanged;
if (preset == null)
{
preset = ToolManager.GetStylePreset("Plane");
}
style = preset.Clone();
BindStyle();
infosFading = new InfosFading(timestamp, averageTimeStampsPerFrame);
infosFading.UseDefault = false;
infosFading.AlwaysVisible = true;
planeWidth = 100;
planeHeight = 100;
quadPlane = new QuadrilateralF(planeWidth, planeHeight);
mnuCalibrate.Click += new EventHandler(mnuCalibrate_Click);
mnuCalibrate.Image = Properties.Drawings.linecalibrate;
}
public void ReadPlaneXml(XmlReader r, PointF scale)
{
r.ReadStartElement();
while (r.NodeType == XmlNodeType.Element)
{
switch (r.Name)
{
case "Size":
size = XmlHelper.ParseSizeF(r.ReadElementContentAsString());
break;
case "Quadrilateral":
quadImage = ParseQuadrilateral(r, scale);
break;
case "Origin":
origin = XmlHelper.ParsePointF(r.ReadElementContentAsString());
break;
default:
string unparsed = r.ReadOuterXml();
log.DebugFormat("Unparsed content in KVA XML: {0}", unparsed);
break;
}
}
r.ReadEndElement();
mapping.Update(new QuadrilateralF(size.Width, size.Height), quadImage);
valid = quadImage.IsConvex;
initialized = true;
}
public void Update(QuadrilateralF plane, QuadrilateralF image)
{
double[,] squareToInput = MapSquareToQuad(plane);
double[,] squareToOutput = MapSquareToQuad(image);
if (squareToOutput == null)
{
return;
}
mapMatrix = MultiplyMatrix(squareToOutput, AdjugateMatrix(squareToInput));
unmapMatrix = AdjugateMatrix(mapMatrix);
Vector3 row0 = new Vector3((float)mapMatrix[0, 0], (float)mapMatrix[0, 1], (float)mapMatrix[0, 2]);
Vector3 row1 = new Vector3((float)mapMatrix[1, 0], (float)mapMatrix[1, 1], (float)mapMatrix[1, 2]);
Vector3 row2 = new Vector3((float)mapMatrix[2, 0], (float)mapMatrix[2, 1], (float)mapMatrix[2, 2]);
matrix = Matrix3x3.CreateFromRows(row0, row1, row2);
adjugate = matrix.Adjugate();
try
{
inverse = matrix.Inverse();
}
catch
{
// Singular matrix.
inverse = Matrix3x3.Identity;
}
}
public void CalibrationByPlane_Initialize(Guid id, SizeF size, QuadrilateralF quadImage)
{
calibrationDrawingId = id;
QuadrilateralF undistorted = distortionHelper.Undistort(quadImage);
calibrationPlane.Initialize(size, undistorted);
AfterCalibrationChanged();
}
public void Reset()
{
// Used on metadata over load.
planeIsConvex = true;
initialized = false;
quadImage = quadPlane.Clone();
}
public void UpdateMapping(SizeF size)
{
planeWidth = size.Width;
planeHeight = size.Height;
quadPlane = new QuadrilateralF(planeWidth, planeHeight);
projectiveMapping.Update(quadPlane, quadImage);
}
/// <summary>
/// Returns an undistorted quadrilateral.
/// </summary>
public QuadrilateralF Undistort(QuadrilateralF quad)
{
return(new QuadrilateralF(
Undistort(quad.A),
Undistort(quad.B),
Undistort(quad.C),
Undistort(quad.D)));
}
/// <summary>
/// Initialize the projective mapping from a line.
/// length: Real world length of the line.
/// a, b: Image coordinates of the line vertices.
/// </summary>
public void Initialize(float lengthWorld, PointF startImage, PointF endImage)
{
QuadrilateralF quadImage = MakeQuad(startImage, endImage);
SizeF sizeWorld = new SizeF(lengthWorld, lengthWorld);
Initialize(sizeWorld, quadImage);
perspective = false;
}
/// <summary>
/// Maps a quadrilateral from image coordinates to plane coordinates.
/// </summary>
public QuadrilateralF Backward(QuadrilateralF q)
{
PointF a = Backward(q.A);
PointF b = Backward(q.B);
PointF c = Backward(q.C);
PointF d = Backward(q.D);
return(new QuadrilateralF(a, b, c, d));
}
/// <summary>
/// Maps a quadrilateral from plane coordinates to image coordinates.
/// </summary>
public QuadrilateralF Forward(QuadrilateralF q)
{
PointF a = Forward(q.A);
PointF b = Forward(q.B);
PointF c = Forward(q.C);
PointF d = Forward(q.D);
return(new QuadrilateralF(a, b, c, d));
}
public QuadrilateralF Transform(QuadrilateralF quadrilateral)
{
Point a = Transform(quadrilateral.A);
Point b = Transform(quadrilateral.B);
Point c = Transform(quadrilateral.C);
Point d = Transform(quadrilateral.D);
return(new QuadrilateralF(a, b, c, d));
}
/// <summary>
/// Initialize the projective mapping from a line.
/// length: Real world length of the line.
/// a, b: Image coordinates of the line vertices.
/// </summary>
public void Initialize(float lengthWorld, PointF startImage, PointF endImage, CalibrationAxis calibrationAxis)
{
this.calibrationAxis = calibrationAxis;
QuadrilateralF quadImage = MakeQuad(startImage, endImage, calibrationAxis);
SizeF sizeWorld = new SizeF(lengthWorld, lengthWorld);
Initialize(sizeWorld, quadImage);
perspective = false;
}
// Get the transform matrix from unit square to quad.
private static double[,] MapSquareToQuad(QuadrilateralF quad)
{
double[,] sq = new double[3, 3];
double px, py;
px = quad[0].X - quad[1].X + quad[2].X - quad[3].X;
py = quad[0].Y - quad[1].Y + quad[2].Y - quad[3].Y;
if ((px < TOLERANCE) && (px > -TOLERANCE) &&
(py < TOLERANCE) && (py > -TOLERANCE))
{
// Input quadrilateral is a parallelogram, the mapping is affine.
sq[0, 0] = quad[1].X - quad[0].X;
sq[0, 1] = quad[2].X - quad[1].X;
sq[0, 2] = quad[0].X;
sq[1, 0] = quad[1].Y - quad[0].Y;
sq[1, 1] = quad[2].Y - quad[1].Y;
sq[1, 2] = quad[0].Y;
sq[2, 0] = 0.0;
sq[2, 1] = 0.0;
sq[2, 2] = 1.0;
}
else
{
// Projective mapping.
double dx1, dx2, dy1, dy2, del;
dx1 = quad[1].X - quad[2].X;
dx2 = quad[3].X - quad[2].X;
dy1 = quad[1].Y - quad[2].Y;
dy2 = quad[3].Y - quad[2].Y;
del = Det2(dx1, dx2, dy1, dy2);
if (del == 0.0)
{
return(null);
}
sq[2, 0] = Det2(px, dx2, py, dy2) / del;
sq[2, 1] = Det2(dx1, px, dy1, py) / del;
sq[2, 2] = 1.0;
sq[0, 0] = quad[1].X - quad[0].X + sq[2, 0] * quad[1].X;
sq[0, 1] = quad[3].X - quad[0].X + sq[2, 1] * quad[3].X;
sq[0, 2] = quad[0].X;
sq[1, 0] = quad[1].Y - quad[0].Y + sq[2, 0] * quad[1].Y;
sq[1, 1] = quad[3].Y - quad[0].Y + sq[2, 1] * quad[3].Y;
sq[1, 2] = quad[0].Y;
}
return(sq);
}
public QuadrilateralF Transform(QuadrilateralF quad)
{
return(new QuadrilateralF()
{
A = Transform(quad.A),
B = Transform(quad.B),
C = Transform(quad.C),
D = Transform(quad.D)
});
}
public FormCalibratePlane(CalibrationHelper calibrationHelper, DrawingPlane drawingPlane)
{
this.calibrationHelper = calibrationHelper;
this.drawingPlane = drawingPlane;
this.quadrilateral = drawingPlane.QuadImage;
InitializeComponent();
LocalizeForm();
InitializeValues();
}
private void DrawDiagonals(Graphics canvas, Pen pen, QuadrilateralF quadPlane, ProjectiveMapping projectiveMapping, DistortionHelper distorter, IImageToViewportTransformer transformer)
{
DrawDistortedLine(canvas, penEdges, quadPlane.A, quadPlane.B, projectiveMapping, distorter, transformer);
DrawDistortedLine(canvas, penEdges, quadPlane.B, quadPlane.C, projectiveMapping, distorter, transformer);
DrawDistortedLine(canvas, penEdges, quadPlane.C, quadPlane.D, projectiveMapping, distorter, transformer);
DrawDistortedLine(canvas, penEdges, quadPlane.D, quadPlane.A, projectiveMapping, distorter, transformer);
DrawDistortedLine(canvas, penEdges, quadPlane.A, quadPlane.C, projectiveMapping, distorter, transformer);
DrawDistortedLine(canvas, penEdges, quadPlane.B, quadPlane.D, projectiveMapping, distorter, transformer);
}
public void CalibrationByPlane_Update(QuadrilateralF quadImage)
{
QuadrilateralF undistorted = new QuadrilateralF(
distortionHelper.Undistort(quadImage.A),
distortionHelper.Undistort(quadImage.B),
distortionHelper.Undistort(quadImage.C),
distortionHelper.Undistort(quadImage.D));
calibrationPlane.Update(undistorted);
AfterCalibrationChanged();
}
public void CalibrationByPlane_Initialize(SizeF size, QuadrilateralF quadImage)
{
QuadrilateralF undistorted = new QuadrilateralF(
distortionHelper.Undistort(quadImage.A),
distortionHelper.Undistort(quadImage.B),
distortionHelper.Undistort(quadImage.C),
distortionHelper.Undistort(quadImage.D));
calibrationPlane.Initialize(size, undistorted);
AfterCalibrationChanged();
}
public void CalibrationByPlane_Update(Guid id, QuadrilateralF quadImage)
{
if (calibratorType != CalibratorType.Plane || id != calibrationDrawingId)
{
return;
}
QuadrilateralF undistorted = distortionHelper.Undistort(quadImage);
calibrationPlane.Update(undistorted);
AfterCalibrationChanged();
}
/// <summary>
/// Initialize the projective mapping.
/// </summary>
/// <param name="size">Real world dimension of the reference rectangle.</param>
/// <param name="quadImage">Image coordinates of the reference rectangle.</param>
public void Initialize(SizeF size, QuadrilateralF quadImage)
{
PointF originImage = initialized ? Untransform(PointF.Empty) : quadImage.D;
this.size = size;
this.quadImage = quadImage.Clone();
mapping.Update(new QuadrilateralF(size.Width, size.Height), quadImage);
SetOrigin(originImage);
this.initialized = true;
valid = quadImage.IsConvex;
}
/// <summary>
/// Updates the calibration coordinate system without changing the real-world scale of the rectangle or the user-defined origin.
/// Quadrilateral variant.
/// </summary>
public void Update(QuadrilateralF quadImage)
{
if (!initialized || size.IsEmpty)
{
valid = false;
return;
}
this.quadImage = quadImage.Clone();
mapping.Update(new QuadrilateralF(size.Width, size.Height), quadImage);
valid = quadImage.IsConvex;
}
/// <summary>
/// Initialize the projective mapping from a quadrilateral.
/// size: Real world dimension of the reference rectangle.
/// quadImage: Image coordinates of the reference rectangle.
/// </summary>
public void Initialize(SizeF sizeWorld, QuadrilateralF quadImage)
{
//PointF originImage = initialized ? Untransform(PointF.Empty) : quadImage.D;
PointF originImage = quadImage.D;
this.size = sizeWorld;
this.quadImage = quadImage.Clone();
mapping.Update(new QuadrilateralF(size.Width, size.Height), quadImage);
origin = mapping.Backward(originImage);
this.initialized = true;
valid = quadImage.IsConvex;
perspective = !quadImage.IsAxisAlignedRectangle;
}
public DrawingRectangle(PointF origin, long timestamp, long averageTimeStampsPerFrame, DrawingStyle preset = null, IImageToViewportTransformer transformer = null)
{
quadImage = new QuadrilateralF(origin, origin.Translate(50, 0), origin.Translate(50, 50), origin.Translate(0, 50));
styleHelper.Color = Color.Empty;
styleHelper.LineSize = 1;
styleHelper.PenShape = PenShape.Solid;
if (preset == null)
{
preset = ToolManager.GetStylePreset("Rectangle");
}
style = preset.Clone();
BindStyle();
infosFading = new InfosFading(timestamp, averageTimeStampsPerFrame);
}
private static CoordinateSystemGrid FindForLineCalibration(CalibrationHelper calibrationHelper)
{
CoordinateSystemGrid grid = new CoordinateSystemGrid();
RectangleF imageBounds = new RectangleF(PointF.Empty, calibrationHelper.ImageSize);
// The clip window is an inflated version of the image to account for distortion.
RectangleF clipWindow = imageBounds.CenteredScale(1.3f);
// Create a fake plane to act as the user-defined projected plane.
QuadrilateralF quadImage = new QuadrilateralF(imageBounds.Deflate(2.0f));
PointF a = calibrationHelper.GetPointFromRectified(quadImage.A);
PointF b = calibrationHelper.GetPointFromRectified(quadImage.B);
PointF d = calibrationHelper.GetPointFromRectified(quadImage.D);
RectangleF plane = new RectangleF(0, 0, b.X - a.X, a.Y - d.Y);
// Define the extended plane (for vanishing point replacement and drawing stop condition) as the reprojection of the whole image.
QuadrilateralF extendedPlane = ReprojectImageBounds(calibrationHelper, new QuadrilateralF(imageBounds));
CalibrationPlane calibrator = new CalibrationPlane();
calibrator.Initialize(plane.Size, quadImage);
PointF originImage = calibrationHelper.GetOrigin();
PointF originRectified = originImage;
if (calibrationHelper.DistortionHelper != null && calibrationHelper.DistortionHelper.Initialized)
{
originRectified = calibrationHelper.DistortionHelper.Undistort(originImage);
}
calibrator.SetOrigin(originRectified);
// From this point on we are mostly in the same situation as for plane calibration.
// stepping size is the same in both directions.
int targetSteps = 15;
float width = extendedPlane.B.X - extendedPlane.A.X;
float step = RangeHelper.FindUsableStepSize(width, targetSteps);
CreateVerticalGridLines(grid, 0, -step, calibrator, clipWindow, plane, extendedPlane, true, false, PointF.Empty);
CreateVerticalGridLines(grid, step, step, calibrator, clipWindow, plane, extendedPlane, true, false, PointF.Empty);
CreateHorizontalGridLines(grid, 0, -step, calibrator, clipWindow, plane, extendedPlane, true, false, PointF.Empty);
CreateHorizontalGridLines(grid, step, step, calibrator, clipWindow, plane, extendedPlane, true, false, PointF.Empty);
return(grid);
}
public override void Draw(Graphics canvas, DistortionHelper distorter, IImageToViewportTransformer transformer, bool selected, long currentTimestamp)
{
double opacityFactor = infosFading.GetOpacityTrackable(trackingTimestamps, currentTimestamp);
if (opacityFactor <= 0)
{
return;
}
QuadrilateralF quad = transformer.Transform(quadImage);
bool drawEdgesOnly = !planeIsConvex || (!styleHelper.Perspective && !quadImage.IsAxisAlignedRectangle);
using (penEdges = styleHelper.GetPen(opacityFactor, 1.0))
using (SolidBrush br = styleHelper.GetBrush(opacityFactor))
{
foreach (PointF p in quad)
{
canvas.FillEllipse(br, p.Box(4));
}
if (!drawEdgesOnly)
{
if (distorter != null && distorter.Initialized)
{
QuadrilateralF undistortedQuadImage = distorter.Undistort(quadImage);
projectiveMapping.Update(quadPlane, undistortedQuadImage);
}
else
{
projectiveMapping.Update(quadPlane, quadImage);
}
DrawGrid(canvas, penEdges, projectiveMapping, distorter, transformer);
}
else
{
// Non convex quadrilateral or non rectangle 2d grid: only draw the edges.
canvas.DrawLine(penEdges, quad.A, quad.B);
canvas.DrawLine(penEdges, quad.B, quad.C);
canvas.DrawLine(penEdges, quad.C, quad.D);
canvas.DrawLine(penEdges, quad.D, quad.A);
}
}
}
public override void Draw(Graphics canvas, DistortionHelper distorter, IImageToViewportTransformer transformer, bool selected, long currentTimestamp)
{
double opacityFactor = infosFading.GetOpacityFactor(currentTimestamp);
if (opacityFactor <= 0)
{
return;
}
QuadrilateralF quad = transformer.Transform(quadImage);
using (penEdges = styleHelper.GetPen(opacityFactor, 1.0))
using (SolidBrush br = styleHelper.GetBrush(opacityFactor))
{
foreach (PointF p in quad)
{
canvas.FillEllipse(br, p.Box(4));
}
if (planeIsConvex)
{
if (distorter != null && distorter.Initialized)
{
QuadrilateralF undistortedQuadImage = distorter.Undistort(quadImage);
projectiveMapping.Update(quadPlane, undistortedQuadImage);
}
else
{
projectiveMapping.Update(quadPlane, quadImage);
}
//DrawDiagonals(canvas, penEdges, quadPlane, projectiveMapping, distorter, transformer);
DrawGrid(canvas, penEdges, projectiveMapping, distorter, transformer);
}
else
{
// Non convex quadrilateral: only draw the edges.
canvas.DrawLine(penEdges, quad.A, quad.B);
canvas.DrawLine(penEdges, quad.B, quad.C);
canvas.DrawLine(penEdges, quad.C, quad.D);
canvas.DrawLine(penEdges, quad.D, quad.A);
}
}
}
private QuadrilateralF ParseQuadrilateral(XmlReader r, PointF scale)
{
r.ReadStartElement();
PointF a = PointF.Empty;
PointF b = PointF.Empty;
PointF c = PointF.Empty;
PointF d = PointF.Empty;
while (r.NodeType == XmlNodeType.Element)
{
switch (r.Name)
{
case "A":
a = XmlHelper.ParsePointF(r.ReadElementContentAsString());
break;
case "B":
b = XmlHelper.ParsePointF(r.ReadElementContentAsString());
break;
case "C":
c = XmlHelper.ParsePointF(r.ReadElementContentAsString());
break;
case "D":
d = XmlHelper.ParsePointF(r.ReadElementContentAsString());
break;
default:
string unparsed = r.ReadOuterXml();
log.DebugFormat("Unparsed content in KVA XML: {0}", unparsed);
break;
}
}
QuadrilateralF quad = new QuadrilateralF(a, b, c, d);
quad.Scale(scale.X, scale.Y);
r.ReadEndElement();
return(quad);
}