// Calculate average error between real angles of the specified quadrilateral and angles of the
// quadrilateral which is the projection of currently estimated pose
private float GetError(Point[] imagePoints, Matrix3x3 rotation, Vector3 translation)
{
Vector3 v1 = rotation * modelPoints[0] + translation;
v1.X = v1.X * focalLength / v1.Z;
v1.Y = v1.Y * focalLength / v1.Z;
Vector3 v2 = rotation * modelPoints[1] + translation;
v2.X = v2.X * focalLength / v2.Z;
v2.Y = v2.Y * focalLength / v2.Z;
Vector3 v3 = rotation * modelPoints[2] + translation;
v3.X = v3.X * focalLength / v3.Z;
v3.Y = v3.Y * focalLength / v3.Z;
Vector3 v4 = rotation * modelPoints[3] + translation;
v4.X = v4.X * focalLength / v4.Z;
v4.Y = v4.Y * focalLength / v4.Z;
Point[] modeledPoints = new Point[4]
{
new Point(v1.X, v1.Y),
new Point(v2.X, v2.Y),
new Point(v3.X, v3.Y),
new Point(v4.X, v4.Y),
};
float ia1 = GeometryTools.GetAngleBetweenVectors(imagePoints[0], imagePoints[1], imagePoints[3]);
float ia2 = GeometryTools.GetAngleBetweenVectors(imagePoints[1], imagePoints[2], imagePoints[0]);
float ia3 = GeometryTools.GetAngleBetweenVectors(imagePoints[2], imagePoints[3], imagePoints[1]);
float ia4 = GeometryTools.GetAngleBetweenVectors(imagePoints[3], imagePoints[0], imagePoints[2]);
float ma1 = GeometryTools.GetAngleBetweenVectors(modeledPoints[0], modeledPoints[1], modeledPoints[3]);
float ma2 = GeometryTools.GetAngleBetweenVectors(modeledPoints[1], modeledPoints[2], modeledPoints[0]);
float ma3 = GeometryTools.GetAngleBetweenVectors(modeledPoints[2], modeledPoints[3], modeledPoints[1]);
float ma4 = GeometryTools.GetAngleBetweenVectors(modeledPoints[3], modeledPoints[0], modeledPoints[2]);
return((
System.Math.Abs(ia1 - ma1) +
System.Math.Abs(ia2 - ma2) +
System.Math.Abs(ia3 - ma3) +
System.Math.Abs(ia4 - ma4)
) / 4);
}
/// <summary>
/// Optimize specified shape.
/// </summary>
///
/// <param name="shape">Shape to be optimized.</param>
///
/// <returns>Returns final optimized shape, which may have reduced amount of points.</returns>
///
public List <IntPoint> OptimizeShape(List <IntPoint> shape)
{
// optimized shape
List <IntPoint> optimizedShape = new List <IntPoint>( );
if (shape.Count <= 3)
{
// do nothing if shape has 3 points or less
optimizedShape.AddRange(shape);
}
else
{
float angle = 0;
// add first 2 points to the new shape
optimizedShape.Add(shape[0]);
optimizedShape.Add(shape[1]);
int pointsInOptimizedHull = 2;
for (int i = 2, n = shape.Count; i < n; i++)
{
// add new point
optimizedShape.Add(shape[i]);
pointsInOptimizedHull++;
// get angle between 2 vectors, which start from the next to last point
angle = GeometryTools.GetAngleBetweenVectors(optimizedShape[pointsInOptimizedHull - 2],
optimizedShape[pointsInOptimizedHull - 3], optimizedShape[pointsInOptimizedHull - 1]);
if ((angle > maxAngleToKeep) &&
((pointsInOptimizedHull > 3) || (i < n - 1)))
{
// remove the next to last point
optimizedShape.RemoveAt(pointsInOptimizedHull - 2);
pointsInOptimizedHull--;
}
}
if (pointsInOptimizedHull > 3)
{
// check the last point
angle = GeometryTools.GetAngleBetweenVectors(optimizedShape[pointsInOptimizedHull - 1],
optimizedShape[pointsInOptimizedHull - 2], optimizedShape[0]);
if (angle > maxAngleToKeep)
{
optimizedShape.RemoveAt(pointsInOptimizedHull - 1);
pointsInOptimizedHull--;
}
if (pointsInOptimizedHull > 3)
{
// check the first point
angle = GeometryTools.GetAngleBetweenVectors(optimizedShape[0],
optimizedShape[pointsInOptimizedHull - 1], optimizedShape[1]);
if (angle > maxAngleToKeep)
{
optimizedShape.RemoveAt(0);
}
}
}
}
return(optimizedShape);
}
/// <summary>
/// Check sub type of a convex polygon.
/// </summary>
///
/// <param name="corners">Corners of the convex polygon to check.</param>
///
/// <returns>Return detected sub type of the specified shape.</returns>
///
/// <remarks><para>The method check corners of a convex polygon detecting
/// its subtype. Polygon's corners are usually retrieved using <see cref="IsConvexPolygon"/>
/// method, but can be any list of 3-4 points (only sub types of triangles and
/// quadrilateral are checked).</para>
///
/// <para>See <see cref="AngleError"/> and <see cref="LengthError"/> properties,
/// which set acceptable errors for polygon sub type checking.</para>
/// </remarks>
///
public PolygonSubType CheckPolygonSubType(List <IntPoint> corners)
{
PolygonSubType subType = PolygonSubType.Unknown;
// get bounding rectangle of the points list
IntPoint minXY, maxXY;
PointsCloud.GetBoundingRectangle(corners, out minXY, out maxXY);
// get cloud's size
IntPoint cloudSize = maxXY - minXY;
float maxLengthDiff = lengthError * (cloudSize.X + cloudSize.Y) / 2;
if (corners.Count == 3)
{
// get angles of the triangle
float angle1 = GeometryTools.GetAngleBetweenVectors(corners[0], corners[1], corners[2]);
float angle2 = GeometryTools.GetAngleBetweenVectors(corners[1], corners[2], corners[0]);
float angle3 = GeometryTools.GetAngleBetweenVectors(corners[2], corners[0], corners[1]);
// check for equilateral triangle
if ((Math.Abs(angle1 - 60) <= angleError) &&
(Math.Abs(angle2 - 60) <= angleError) &&
(Math.Abs(angle3 - 60) <= angleError))
{
subType = PolygonSubType.EquilateralTriangle;
}
else
{
// check for isosceles triangle
if ((Math.Abs(angle1 - angle2) <= angleError) ||
(Math.Abs(angle2 - angle3) <= angleError) ||
(Math.Abs(angle3 - angle1) <= angleError))
{
subType = PolygonSubType.IsoscelesTriangle;
}
// check for rectangled triangle
if ((Math.Abs(angle1 - 90) <= angleError) ||
(Math.Abs(angle2 - 90) <= angleError) ||
(Math.Abs(angle3 - 90) <= angleError))
{
subType = (subType == PolygonSubType.IsoscelesTriangle) ?
PolygonSubType.RectangledIsoscelesTriangle : PolygonSubType.RectangledTriangle;
}
}
}
else if (corners.Count == 4)
{
// get angles between 2 pairs of opposite sides
float angleBetween1stPair = GeometryTools.GetAngleBetweenLines(corners[0], corners[1], corners[2], corners[3]);
float angleBetween2ndPair = GeometryTools.GetAngleBetweenLines(corners[1], corners[2], corners[3], corners[0]);
// check 1st pair for parallelism
if (angleBetween1stPair <= angleError)
{
subType = PolygonSubType.Trapezoid;
// check 2nd pair for parallelism
if (angleBetween2ndPair <= angleError)
{
subType = PolygonSubType.Parallelogram;
// check angle between adjacent sides
if (Math.Abs(GeometryTools.GetAngleBetweenVectors(corners[1], corners[0], corners[2]) - 90) <= angleError)
{
subType = PolygonSubType.Rectangle;
}
// get length of 2 adjacent sides
float side1Length = (float)corners[0].DistanceTo(corners[1]);
float side2Length = (float)corners[0].DistanceTo(corners[3]);
if (Math.Abs(side1Length - side2Length) <= maxLengthDiff)
{
subType = (subType == PolygonSubType.Parallelogram) ?
PolygonSubType.Rhombus : PolygonSubType.Square;
}
}
}
else
{
// check 2nd pair for parallelism - last chence to detect trapezoid
if (angleBetween2ndPair <= angleError)
{
subType = PolygonSubType.Trapezoid;
}
}
}
return(subType);
}