Ejemplo n.º 1
0
        public void Estimate(List <Vector2> pts)
        {
            // http://www.dtcenter.org/sites/default/files/community-code/met/docs/write-ups/circle_fit.pdf
            float a;
            float b;
            float x2;
            float y2;
            float uc;
            float vc;

            Matrix22 m1;
            Matrix22 m2;
            Matrix22 m3;

            float sum_x2  = 0;
            float sum_y2  = 0;
            float sum_xy  = 0;
            float sum_x3  = 0;
            float sum_y3  = 0;
            float sum_xy2 = 0;
            float sum_yx2 = 0;

            Vector2        origin       = Matrix.Mean(pts);
            List <Vector2> centered_pts = Matrix.Minus(pts, origin);

            foreach (Vector2 pt in centered_pts)
            {
                x2       = pt.x * pt.x;
                y2       = pt.y * pt.y;
                sum_x2  += x2;
                sum_y2  += y2;
                sum_xy  += pt.x * pt.y;
                sum_x3  += x2 * pt.x;
                sum_y3  += y2 * pt.y;
                sum_xy2 += pt.x * y2;
                sum_yx2 += pt.y * x2;
            }

            a = (sum_x3 + sum_xy2) / 2;
            b = (sum_y3 + sum_yx2) / 2;

            // Solve 2-dimensional linear equation using Cramer’s rule:
            // https://www.geeksforgeeks.org/system-linear-equations-three-variables-using-cramers-rule/
            m1 = new Matrix22(sum_x2, sum_xy, sum_xy, sum_y2);
            m2 = new Matrix22(a, sum_xy, b, sum_y2);
            m3 = new Matrix22(sum_x2, a, sum_xy, b);

            uc = m2.Det() / m1.Det();
            vc = m3.Det() / m1.Det();

            Center = new Vector2(uc, vc).Add(origin);
            R      = (float)Math.Sqrt(uc * uc + vc * vc + (sum_x2 + sum_y2) / centered_pts.Count);
        }
Ejemplo n.º 2
0
        /// <summary>
        /// Calculate multivariate normal probablity density function.
        /// Hardcoded for Vector2 and Matrix22 datatype.
        /// Use double precision to increase robustness.
        /// </summary>
        /// <param name="pt"></param>
        /// <param name="miu"></param>
        /// <param name="covariance"></param>
        /// <returns></returns>
        private static double MultiNormalPDF(Vector2 pt, Gaussian_2D gaussian)
        {
            Vector2  miu        = gaussian.miu;
            Matrix22 covariance = gaussian.Sigma;

            Vector2  x_minus_miu = pt.Minus(miu);
            Matrix22 inv_cov     = covariance.Inverse();

            //Break Matrix22 into two rows (Vector2) for dot product
            Vector2 row1 = new Vector2(inv_cov.m00, inv_cov.m01);
            Vector2 row2 = new Vector2(inv_cov.m10, inv_cov.m11);

            float dot_row1 = row1.Dot(x_minus_miu);
            float dot_row2 = row2.Dot(x_minus_miu);

            //Assemble two rows
            Vector2 temp_v = new Vector2(dot_row1, dot_row2);

            double numerator = Math.Exp(-x_minus_miu.Dot(temp_v) / 2);
            double denom     = 2 * Math.PI * Math.Sqrt(covariance.Det());

            return(numerator / denom);
        }