コード例 #1
0
ファイル: CoplanarPosit.cs プロジェクト: EnergonV/BestCS
        // 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);
        }
コード例 #2
0
        /// <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);
        }
コード例 #3
0
        /// <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);
        }