public PolygonSubType CheckPolygonSubType(List <IntPoint> corners)
{
PolygonSubType polygonSubType = PolygonSubType.Unknown;
PointsCloud.GetBoundingRectangle(corners, out IntPoint minXY, out IntPoint maxXY);
IntPoint intPoint = maxXY - minXY;
float num = lengthError * (float)(intPoint.X + intPoint.Y) / 2f;
if (corners.Count == 3)
{
float angleBetweenVectors = GeometryTools.GetAngleBetweenVectors(corners[0], corners[1], corners[2]);
float angleBetweenVectors2 = GeometryTools.GetAngleBetweenVectors(corners[1], corners[2], corners[0]);
float angleBetweenVectors3 = GeometryTools.GetAngleBetweenVectors(corners[2], corners[0], corners[1]);
if (System.Math.Abs(angleBetweenVectors - 60f) <= angleError && System.Math.Abs(angleBetweenVectors2 - 60f) <= angleError && System.Math.Abs(angleBetweenVectors3 - 60f) <= angleError)
{
polygonSubType = PolygonSubType.EquilateralTriangle;
}
else
{
if (System.Math.Abs(angleBetweenVectors - angleBetweenVectors2) <= angleError || System.Math.Abs(angleBetweenVectors2 - angleBetweenVectors3) <= angleError || System.Math.Abs(angleBetweenVectors3 - angleBetweenVectors) <= angleError)
{
polygonSubType = PolygonSubType.IsoscelesTriangle;
}
if (System.Math.Abs(angleBetweenVectors - 90f) <= angleError || System.Math.Abs(angleBetweenVectors2 - 90f) <= angleError || System.Math.Abs(angleBetweenVectors3 - 90f) <= angleError)
{
polygonSubType = ((polygonSubType == PolygonSubType.IsoscelesTriangle) ? PolygonSubType.RectangledIsoscelesTriangle : PolygonSubType.RectangledTriangle);
}
}
}
else if (corners.Count == 4)
{
float angleBetweenLines = GeometryTools.GetAngleBetweenLines(corners[0], corners[1], corners[2], corners[3]);
float angleBetweenLines2 = GeometryTools.GetAngleBetweenLines(corners[1], corners[2], corners[3], corners[0]);
if (angleBetweenLines <= angleError)
{
polygonSubType = PolygonSubType.Trapezoid;
if (angleBetweenLines2 <= angleError)
{
polygonSubType = PolygonSubType.Parallelogram;
if (System.Math.Abs(GeometryTools.GetAngleBetweenVectors(corners[1], corners[0], corners[2]) - 90f) <= angleError)
{
polygonSubType = PolygonSubType.Rectangle;
}
float num2 = corners[0].DistanceTo(corners[1]);
float num3 = corners[0].DistanceTo(corners[3]);
if (System.Math.Abs(num2 - num3) <= num)
{
polygonSubType = ((polygonSubType == PolygonSubType.Parallelogram) ? PolygonSubType.Rhombus : PolygonSubType.Square);
}
}
}
else if (angleBetweenLines2 <= angleError)
{
polygonSubType = PolygonSubType.Trapezoid;
}
}
return(polygonSubType);
}
// 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);
}
public List <IntPoint> OptimizeShape(List <IntPoint> shape)
{
List <IntPoint> list = new List <IntPoint>();
if (shape.Count <= 3)
{
list.AddRange(shape);
}
else
{
float num = 0f;
list.Add(shape[0]);
list.Add(shape[1]);
int num2 = 2;
int i = 2;
for (int count = shape.Count; i < count; i++)
{
list.Add(shape[i]);
num2++;
num = GeometryTools.GetAngleBetweenVectors(list[num2 - 2], list[num2 - 3], list[num2 - 1]);
if (num > maxAngleToKeep && (num2 > 3 || i < count - 1))
{
list.RemoveAt(num2 - 2);
num2--;
}
}
if (num2 > 3)
{
num = GeometryTools.GetAngleBetweenVectors(list[num2 - 1], list[num2 - 2], list[0]);
if (num > maxAngleToKeep)
{
list.RemoveAt(num2 - 1);
num2--;
}
if (num2 > 3)
{
num = GeometryTools.GetAngleBetweenVectors(list[0], list[num2 - 1], list[1]);
if (num > maxAngleToKeep)
{
list.RemoveAt(0);
}
}
}
}
return(list);
}
private float GetError(Point[] imagePoints, Matrix3x3 rotation, Vector3 translation)
{
Vector3 vector = rotation * modelPoints[0] + translation;
vector.X = vector.X * focalLength / vector.Z;
vector.Y = vector.Y * focalLength / vector.Z;
Vector3 vector2 = rotation * modelPoints[1] + translation;
vector2.X = vector2.X * focalLength / vector2.Z;
vector2.Y = vector2.Y * focalLength / vector2.Z;
Vector3 vector3 = rotation * modelPoints[2] + translation;
vector3.X = vector3.X * focalLength / vector3.Z;
vector3.Y = vector3.Y * focalLength / vector3.Z;
Vector3 vector4 = rotation * modelPoints[3] + translation;
vector4.X = vector4.X * focalLength / vector4.Z;
vector4.Y = vector4.Y * focalLength / vector4.Z;
Point[] array = new Point[4]
{
new Point(vector.X, vector.Y),
new Point(vector2.X, vector2.Y),
new Point(vector3.X, vector3.Y),
new Point(vector4.X, vector4.Y)
};
float angleBetweenVectors = GeometryTools.GetAngleBetweenVectors(imagePoints[0], imagePoints[1], imagePoints[3]);
float angleBetweenVectors2 = GeometryTools.GetAngleBetweenVectors(imagePoints[1], imagePoints[2], imagePoints[0]);
float angleBetweenVectors3 = GeometryTools.GetAngleBetweenVectors(imagePoints[2], imagePoints[3], imagePoints[1]);
float angleBetweenVectors4 = GeometryTools.GetAngleBetweenVectors(imagePoints[3], imagePoints[0], imagePoints[2]);
float angleBetweenVectors5 = GeometryTools.GetAngleBetweenVectors(array[0], array[1], array[3]);
float angleBetweenVectors6 = GeometryTools.GetAngleBetweenVectors(array[1], array[2], array[0]);
float angleBetweenVectors7 = GeometryTools.GetAngleBetweenVectors(array[2], array[3], array[1]);
float angleBetweenVectors8 = GeometryTools.GetAngleBetweenVectors(array[3], array[0], array[2]);
return((System.Math.Abs(angleBetweenVectors - angleBetweenVectors5) + System.Math.Abs(angleBetweenVectors2 - angleBetweenVectors6) + System.Math.Abs(angleBetweenVectors3 - angleBetweenVectors7) + System.Math.Abs(angleBetweenVectors4 - angleBetweenVectors8)) / 4f);
}
/// <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(IntPoint.ToPoint(corners[0]), IntPoint.ToPoint(corners[1]), IntPoint.ToPoint(corners[2]));
float angle2 = GeometryTools.GetAngleBetweenVectors(IntPoint.ToPoint(corners[1]), IntPoint.ToPoint(corners[2]), IntPoint.ToPoint(corners[0]));
float angle3 = GeometryTools.GetAngleBetweenVectors(IntPoint.ToPoint(corners[2]), IntPoint.ToPoint(corners[0]), IntPoint.ToPoint(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(IntPoint.ToPoint(corners[0]), IntPoint.ToPoint(corners[1]), IntPoint.ToPoint(corners[2]), IntPoint.ToPoint(corners[3]));
float angleBetween2ndPair = GeometryTools.GetAngleBetweenLines(IntPoint.ToPoint(corners[1]), IntPoint.ToPoint(corners[2]), IntPoint.ToPoint(corners[3]), IntPoint.ToPoint(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(IntPoint.ToPoint(corners[1]), IntPoint.ToPoint(corners[0]), IntPoint.ToPoint(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);
}
/// <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);
}
