Ejemplo n.º 1
0
        // Here define A,B, and H Matrice
        private Joint kalmanFilterFull(int pos)
        {
            // Prepare
            //GeneralMatrix r = GeneralMatrix.Identity(3, 3);
            //R = r.MultiplyEquals(0.01);
            // Set r from Deviation Standard
            GeneralMatrix r = GeneralMatrix.Identity(3, 3);

            r.SetElement(0, 0, Rv[pos, 0]); // Variance Base on Joint Type , X
            r.SetElement(1, 1, Rv[pos, 1]); // Variance Base on Joint Type , Z
            r.SetElement(2, 2, Rv[pos, 2]); // Variance Base on Joint Type , Y
            R = r;
            // Set estimation from coordinate average in frame buffer
            GeneralMatrix Z = GeneralMatrix.Random(3, 1);

            Z.SetElement(0, 0, jointAveragePart[pos, 0]);
            Z.SetElement(1, 0, jointAveragePart[pos, 1]);
            Z.SetElement(2, 0, jointAveragePart[pos, 2]);

            // Predict
            GeneralMatrix Xp = F * Xk[pos];
            GeneralMatrix Pp = F * P[pos] * F.Transpose() + Q;

            // Measurement update ( Correction )
            K       = Pp * H.Transpose() * (H * Pp * H.Transpose() + R).Inverse();
            Xk[pos] = Xp + (K * (Z - (H * Xp)));

            GeneralMatrix I = GeneralMatrix.Identity(Pp.RowDimension, Pp.ColumnDimension);

            P[pos] = (I - (K * H)) * Pp;

            return(convertMatrixTojoint(Xk[pos]));
        }
Ejemplo n.º 2
0
        /// <summary>
        /// Distance in metres between two points expressed as longitude/latitude
        /// </summary>
        /// <param name="longLat0">longitude and latitude for first point</param>
        /// <param name="longLat1">longitude and latitude for second point</param>
        /// <returns></returns>
        public static double DistancePointToPointLongLat(LongLat longLat0, LongLat longLat1)
        {
            // use spherical coordinates: rho, phi, theta
            const double rho = 6378200; // earth radius in metres

            double sinPhi0   = Math.Sin(0.5 * Math.PI + longLat0.Latitude / 180.0 * Math.PI);
            double cosPhi0   = Math.Cos(0.5 * Math.PI + longLat0.Latitude / 180.0 * Math.PI);
            double sinTheta0 = Math.Sin(longLat0.Longitude / 180.0 * Math.PI);
            double cosTheta0 = Math.Cos(longLat0.Longitude / 180.0 * Math.PI);

            double sinPhi1   = Math.Sin(0.5 * Math.PI + longLat1.Latitude / 180.0 * Math.PI);
            double cosPhi1   = Math.Cos(0.5 * Math.PI + longLat1.Latitude / 180.0 * Math.PI);
            double sinTheta1 = Math.Sin(longLat1.Longitude / 180.0 * Math.PI);
            double cosTheta1 = Math.Cos(longLat1.Longitude / 180.0 * Math.PI);

            var p0 = new GeneralMatrix(3, 1);

            p0.SetElement(0, 0, rho * sinPhi0 * cosTheta0);
            p0.SetElement(1, 0, rho * sinPhi0 * sinTheta0);
            p0.SetElement(2, 0, rho * cosPhi0);

            var p1 = new GeneralMatrix(3, 1);

            p1.SetElement(0, 0, rho * sinPhi1 * cosTheta1);
            p1.SetElement(1, 0, rho * sinPhi1 * sinTheta1);
            p1.SetElement(2, 0, rho * cosPhi1);

            double distance = DistancePointToPoint(p0, p1);

            return(distance);
        }
Ejemplo n.º 3
0
        /******************* Method Kalman Filter ******************/

        // Variable A,B, and H we will asumtion this is a constant value
        // Most probably is 1.
        private Joint kalmanFilter(GeneralMatrix z, int pos)
        {
            // Prepare
            Joint         join = new Joint();
            GeneralMatrix r    = GeneralMatrix.Identity(3, 3);

            r.SetElement(0, 0, Rv[pos, 0]); // Variance Base on Joint Type , X
            r.SetElement(1, 1, Rv[pos, 1]); // Variance Base on Joint Type , Z
            r.SetElement(2, 2, Rv[pos, 2]); // Variance Base on Joint Type , Y
            R = r;

            // Predict
            GeneralMatrix Xp = Xk[pos];
            GeneralMatrix Pp = P[pos];

            // Measurement Update (correction
            GeneralMatrix s = Pp + R;

            K       = Pp * s.Inverse(); // Calculate Kalman Gain
            Xk[pos] = Xp + (K * (z - Xp));
            GeneralMatrix I = GeneralMatrix.Identity(Pp.RowDimension, Pp.ColumnDimension);

            P[pos] = (I - K) * Pp;

            estimationX = Xk[pos].GetElement(0, 0);
            estimationY = Xk[pos].GetElement(1, 0);
            estimationZ = Xk[pos].GetElement(2, 0);

            join.Position.X = (float)estimationX;
            join.Position.Y = (float)estimationY;
            join.Position.Z = (float)estimationZ;

            return(join);
        }
Ejemplo n.º 4
0
        /// <summary>
        /// (
        /// </summary>
        /// <param name="matrix"></param>
        /// <returns></returns>
        public static GeneralMatrix ExpandUtility(GeneralMatrix matrix)
        {
            double val = 0.0;
            int    n   = matrix.RowDimension;
            int    m   = matrix.ColumnDimension;

            if (n != m)
            {
                throw new ArgumentException("Criteria matrix must be symmetrical");
            }

            GeneralMatrix newMatrix = matrix.Transpose();

            //for all transposed elements calculate their inverse values
            //set diagonal elements to 0
            for (int i = 0; i < n; i++)
            {
                for (int j = 0; j <= i; j++)
                {
                    val = newMatrix.GetElement(i, j);
                    if (val == 0.0)
                    {
                        throw new ArgumentException("Criteria comparison values van't be 0");
                    }
                    newMatrix.SetElement(i, j, 1 / val);
                    if (i == j)
                    {
                        newMatrix.SetElement(i, j, 0);
                    }
                }
            }
            //add transposed, inverse matrix to the original one
            //create fully expanded matrix
            return(newMatrix.Add(matrix));
        }
Ejemplo n.º 5
0
            public void Visit(Compiled.Gp elem)
            {
                GeneralMatrix cur = new GeneralMatrix(1, elem.Terms.Length);

                for (int i = 0; i < elem.Terms.Length; ++i)
                {
                    cur.SetElement(0, i, ValueOf(elem.Terms[i]));
                }
                elem.Value = elem.Gpr.Evaluate(cur).GetElement(0, 0);

                for (int i = 0; i < elem.Inputs.Length; ++i)
                {
                    if (i < elem.dc)
                    {
                        //elem.Inputs[i].Weight = elem.Gpr.PartialDerivative(cur, i);
                        cur.SetElement(0, i, cur.GetElement(0, i) + 0.1);
                        elem.Inputs[i].Weight = (elem.Gpr.Evaluate(cur).GetElement(0, 0) - elem.Value) / 0.1;
                        cur.SetElement(0, i, cur.GetElement(0, i) - 0.1);
                    }
                    else
                    {
                        elem.Inputs[i].Weight = 0;
                    }
                }


                //Here Starts a dirty Hack

                /*double sum=0;
                 * for(int n=0; n<elem.Inputs.Length; n++) {
                 *      sum += elem.Inputs[n].Weight;
                 * }
                 * if(sum < 0.00001) {
                 *      double val = elem.Value;
                 *
                 *      double minDist = Double.MaxValue;
                 *      int iMin=-1;
                 *      double dist = 0, tmp;
                 *      for(int n=0; n<elem.X.RowDimension; n++) {
                 *              if(elem.Gpr.Y.GetElement(0, n) > val) {
                 *                      dist = 0;
                 *                      for(int m=0; m<elem.X.ColumnDimension; m++) {
                 *                              tmp = elem.X.GetElement(n, m) - cur.GetElement(0, m);
                 *                              dist += tmp*tmp;
                 *                      }
                 *                      if(dist < minDist) {
                 *                              minDist = dist;
                 *                              iMin = n;
                 *                      }
                 *              }
                 *      }
                 *      if(iMin>=0) {
                 *              for(int m=0; m<elem.X.ColumnDimension; m++) {
                 *                      elem.Inputs[m].Weight = elem.X.GetElement(m, iMin) - cur.GetElement(0, m);
                 *              }
                 *      }
                 * }*/
            }
Ejemplo n.º 6
0
        /******************* End Function for calculate Angle  ******************/
        #endregion

        #region Support Function
        private GeneralMatrix convertJoinToMatrix(Joint j)
        {
            GeneralMatrix matrix = GeneralMatrix.Identity(3, 1);

            matrix.SetElement(0, 0, j.Position.X);
            matrix.SetElement(1, 0, j.Position.Y);
            matrix.SetElement(2, 0, j.Position.Z);
            return(matrix);
        }
Ejemplo n.º 7
0
        public static GeneralMatrix To3x1Matrix(PointD pointD)
        {
            GeneralMatrix m = new GeneralMatrix(3, 1);

            m.SetElement(0, 0, pointD.X);
            m.SetElement(1, 0, pointD.Y);
            m.SetElement(2, 0, 1);
            return(m);
        }
Ejemplo n.º 8
0
        public static GeneralMatrix To3x1Matrix(LongLat longLat)
        {
            GeneralMatrix m = new GeneralMatrix(3, 1);

            m.SetElement(0, 0, longLat.Longitude);
            m.SetElement(1, 0, longLat.Latitude);
            m.SetElement(2, 0, 1);
            return(m);
        }
Ejemplo n.º 9
0
        private QFactorResult calculateLeastSquareLorenz()
        {
            int          i, j = 0;
            int          f0_index;
            PointPairInt range;
            int          n;

            // Preprocessing (removing data points which are far away from transmittance peak)
            f0_index = findCenterFrequencyIndex();
            if (f0_index < 0 || f0_index > pointList.Count)
            {
                return(new QFactorResult());   // return "no result" if center frequency not found.
            }
            range = findBandwidthIndexes(f0_index, 3.0F);
            if (range.I0 == int.MaxValue)
            {
                return(new QFactorResult());   // return "no result" if center frequency not found.
            }
            n = range.I1 - range.I0;
            if (n < 5)
            {       // When we have very small amount of points, turn off Q-calculations completely - we don't want to have useless results!
                    // We need to return the center frequency which is already found.
                return(new QFactorResult(pointList[f0_index].frequency, n));
            }

            GeneralMatrix X = new GeneralMatrix(n, 3);  // vandermonde matrix with measured frequency points
            GeneralMatrix Y = new GeneralMatrix(n, 3);  // vector with measured power levels

            for (i = 0; i < pointList.Count; i++)
            {
                if (i > range.I0 && i <= range.I1)
                {
                    X.SetElement(j, 0, Math.Pow(pointList[i].frequency, 2));
                    X.SetElement(j, 1, pointList[i].frequency);
                    X.SetElement(j, 2, 1);
                    Y.SetElement(j, 0, Math.Pow(10, -pointList[i].gain / 10));   // measured power is converted to linear form and inverted (1/x)
                    j++;
                }
            }
            QRDecomposition QR   = new QRDecomposition(X);
            GeneralMatrix   beta = QR.Solve(Y);             // Linear regression used to find beta coefficients

            double A = beta.GetElement(0, 0);
            double B = beta.GetElement(1, 0);
            double C = beta.GetElement(2, 0);

            double F0 = -B / (2 * A);
            double Q  = -B / (2 * Math.Sqrt(4 * A * C - B * B));
            double P0 = -1 / ((B * B) / (4 * A) - C);

            P0 = 10 * Math.Log10(P0);

            //return new QFactorResult(Q, F0, F0 - 0.5 * F0 / Q, F0 + 0.5 * F0 / Q, P0-3, P0-3);
            return(new QFactorResult(Q, F0, P0, F0 / Q, n));
        }
Ejemplo n.º 10
0
        public Point GetActualPosition(Point p)
        {
            GeneralMatrix mat = new GeneralMatrix(3, 1);

            mat.SetElement(0, 0, (double)p.X);
            mat.SetElement(1, 0, (double)p.Y);
            mat.SetElement(2, 0, 1.0);
            GeneralMatrix ret = m_transform.Inverse().Multiply(mat);

            return(new Point((int)ret.GetElement(0, 0), (int)ret.GetElement(1, 0)));
        }
Ejemplo n.º 11
0
        public PointF GetActualDisplayPosition(PointF p)
        {
            GeneralMatrix mat = new GeneralMatrix(3, 1);

            mat.SetElement(0, 0, p.X);
            mat.SetElement(1, 0, p.Y);
            mat.SetElement(2, 0, 1.0);
            GeneralMatrix ret = m_transform.Multiply(mat);

            return(new PointF((float)ret.GetElement(0, 0), (float)ret.GetElement(1, 0)));
        }
Ejemplo n.º 12
0
        public void TestNorm1()
        {
            GeneralMatrix _gm = new GeneralMatrix(2, 2);

            _gm.SetElement(0, 0, 1);
            _gm.SetElement(0, 1, 2);
            _gm.SetElement(1, 0, 3);
            _gm.SetElement(1, 1, 4);

            Assert.AreEqual(6, _gm.Norm1());
        }
Ejemplo n.º 13
0
        public void TestTranspose()
        {
            GeneralMatrix _gm = new GeneralMatrix(2, 2);

            _gm.SetElement(0, 0, 1);
            _gm.SetElement(0, 1, 2);
            _gm.SetElement(1, 0, 3);
            _gm.SetElement(1, 1, 4);
            GeneralMatrix _ngm = _gm.Transpose();

            Assert.AreEqual(1, 0, 2);
        }
Ejemplo n.º 14
0
        /// <summary>
        ///
        /// </summary>
        private void CalculatePriorities()
        {
            PrioritiesSelector selector = new PrioritiesSelector();

            selector.ComputePriorities(_criteria);

            //first (zero-th) element of the lambda matrix is the
            //consistency ratio factor for the selection matrix
            _lambdas.SetElement(0, 0, selector.ConsistencyRatio);

            _orderedCriteria = selector.CalculatedMatrix;
        }
Ejemplo n.º 15
0
        /// <summary>
        ///
        /// </summary>
        /// <param name="p0">First point on route</param>
        /// <param name="q0">First point on map</param>
        /// <param name="p1">Second point on route</param>
        /// <param name="q1">Second point on map</param>
        /// <param name="p2">Third point on route</param>
        /// <param name="q2">Third point on map</param>
        /// <param name="fallbackMatrix">Matrix to use if calculation fails due to singular matrix</param>
        /// <returns></returns>
        public static GeneralMatrix CalculateTransformationMatrix(PointD p0, PointD q0, PointD p1, PointD q1, PointD p2, PointD q2, GeneralMatrix fallbackMatrix)
        {
            try
            {
                var m = new GeneralMatrix(3, 3);
                m.SetElement(0, 0, p0.X);
                m.SetElement(0, 1, p0.Y);
                m.SetElement(0, 2, 1.0);
                m.SetElement(1, 0, p1.X);
                m.SetElement(1, 1, p1.Y);
                m.SetElement(1, 2, 1.0);
                m.SetElement(2, 0, p2.X);
                m.SetElement(2, 1, p2.Y);
                m.SetElement(2, 2, 1.0);

                var v1 = new GeneralMatrix(3, 1);
                v1.SetElement(0, 0, q0.X);
                v1.SetElement(1, 0, q1.X);
                v1.SetElement(2, 0, q2.X);
                var t1 = m.Inverse() * v1;

                var v2 = new GeneralMatrix(3, 1);
                v2.SetElement(0, 0, q0.Y);
                v2.SetElement(1, 0, q1.Y);
                v2.SetElement(2, 0, q2.Y);
                var t2 = m.Inverse() * v2;

                var v3 = new GeneralMatrix(3, 1);
                v3.SetElement(0, 0, 1.0);
                v3.SetElement(1, 0, 1.0);
                v3.SetElement(2, 0, 1.0);
                var t3 = m.Inverse() * v3;

                var t = new GeneralMatrix(3, 3);
                t.SetElement(0, 0, t1.GetElement(0, 0));
                t.SetElement(0, 1, t1.GetElement(1, 0));
                t.SetElement(0, 2, t1.GetElement(2, 0));
                t.SetElement(1, 0, t2.GetElement(0, 0));
                t.SetElement(1, 1, t2.GetElement(1, 0));
                t.SetElement(1, 2, t2.GetElement(2, 0));
                t.SetElement(2, 0, t3.GetElement(0, 0));
                t.SetElement(2, 1, t3.GetElement(1, 0));
                t.SetElement(2, 2, t3.GetElement(2, 0));

                return(t);
            }
            catch (Exception)
            {
                return((GeneralMatrix)fallbackMatrix.Clone());
            }
        }
Ejemplo n.º 16
0
        public GeneralMatrix QuantizePolar(int grid_x, int grid_y)
        {
            // create a general matrix full of 0s
            GeneralMatrix mesh = new GeneralMatrix(grid_y, grid_x, 0.0);

            if (_points.Count == 0)
            {
                return(mesh);
            }

            double stepX = 2.0 * Math.PI / grid_x; // X is the theta coordinate
            double stepY = 1.0 / grid_y;           // Y is the R coordinate

            foreach (GenericPoint pt in _points)
            {
                int    x_index = (int)Math.Floor((pt.Theta(_polarCenter) + Math.PI) / stepX);
                double r       = pt.R(_polarCenter, _averageDist);
                double t       = pt.Theta(_polarCenter);
                int    y_index = (int)Math.Floor(pt.R(_polarCenter, _averageDist) / stepY);

                if (y_index < grid_y && x_index < grid_x)
                {
                    mesh.SetElement(y_index, x_index, 1.0);
                }
            }

#if FALSE
            Console.WriteLine("Quantized polar");
            printGeneralMatrix(ref mesh);
#endif

            return(mesh);
        }
Ejemplo n.º 17
0
        /// <summary>
        /// Calculates and saves the distance transform map for the screen coordinates.
        /// Uses _sMesh and saves to _sDTM.
        /// A distance transform map is a matrix in which every entry represents the
        /// distance from that point to the nearest point with ink.
        /// </summary>
        private void ScreenDistanceTransform()
        {
            List <Coord> indices = IndexList(_sMesh);

            _sDTM = new GeneralMatrix(GRID_Y_SIZE, GRID_X_SIZE, 0.0);

            for (int i = 0; i < GRID_Y_SIZE; i++)
            {
                for (int j = 0; j < GRID_X_SIZE; j++)
                {
                    Coord  cp      = new Coord(j, i);
                    double mindist = double.PositiveInfinity;

                    foreach (Coord pt in indices)
                    {
                        double distan = cp.distanceTo(pt);
                        if (distan < mindist)
                        {
                            mindist = distan;
                        }
                    }
                    _sDTM.SetElement(i, j, mindist);
                }
            }
        }
Ejemplo n.º 18
0
        /// <summary>
        /// Gets the 3D coordinates relative to the center of the earth.
        /// </summary>
        /// <returns></returns>
        public GeneralMatrix To3DPoint()
        {
            // use spherical coordinates: rho, phi, theta
            const double rho = 6378200; // earth radius in metres

            double sinPhi   = Math.Sin(0.5 * Math.PI + latitude / 180.0 * Math.PI);
            double cosPhi   = Math.Cos(0.5 * Math.PI + latitude / 180.0 * Math.PI);
            double sinTheta = Math.Sin(longitude / 180.0 * Math.PI);
            double cosTheta = Math.Cos(longitude / 180.0 * Math.PI);

            GeneralMatrix p = new GeneralMatrix(3, 1);

            p.SetElement(0, 0, rho * sinPhi * cosTheta);
            p.SetElement(1, 0, rho * sinPhi * sinTheta);
            p.SetElement(2, 0, rho * cosPhi);
            return(p);
        }
Ejemplo n.º 19
0
        public void TestSolve()
        {
            GeneralMatrix _ls = new GeneralMatrix(2, 2);

            _ls.SetElement(0, 0, 1);
            _ls.SetElement(0, 1, 2);
            _ls.SetElement(1, 0, 3);
            _ls.SetElement(1, 1, 4);

            GeneralMatrix _rs = new GeneralMatrix(2, 1);

            _rs.SetElement(0, 0, -3);
            _rs.SetElement(1, 0, -5);

            GeneralMatrix _solution = _ls.Solve(_rs);

            Assert.AreEqual(_solution.GetElement(0, 0), 1);
            Assert.AreEqual(_solution.GetElement(1, 0), -2);
        }
            public void Visit(Compiled.Gp elem)
            {
                GeneralMatrix cur = new GeneralMatrix(1, elem.Terms.Length);

                for (int i = 0; i < elem.Terms.Length; ++i)
                {
                    cur.SetElement(0, i, ValueOf(elem.Terms[i]));
                }
                elem.Value = elem.Gpr.Evaluate(cur).GetElement(0, 0);
            }
Ejemplo n.º 21
0
 //
 /// <summary>
 /// Calculates all elements for <code>this.alpha</code>.
 /// </summary>
 protected void UpdateAlpha()
 {
     for (int i = 0; i < parameters.Length; i++)
     {
         for (int j = 0; j < parameters.Length; j++)
         {
             alpha.SetElement(i, j, CalculateAlphaElement(i, j));
         }
     }
 }
            public void Visit(Compiled.Gp elem)
            {
                GeneralMatrix cur = new GeneralMatrix(1, elem.Terms.Length);

                for (int i = 0; i < elem.Terms.Length; ++i)
                {
                    cur.SetElement(0, i, ValueOf(elem.Terms[i]));
                }
                LocalDerivative = elem.Gpr.PartialDerivative(cur, ArgumentIndex);
            }
Ejemplo n.º 23
0
        public void LUDecomposition()
        {
            GeneralMatrix A = new GeneralMatrix(columnwise, 4);
            int           n = A.ColumnDimension;

            A = A.GetMatrix(0, n - 1, 0, n - 1);
            A.SetElement(0, 0, 0.0);
            LUDecomposition LU = A.LUD();

            Assert.IsTrue(GeneralTests.Check(A.GetMatrix(LU.Pivot, 0, n - 1), LU.L.Multiply(LU.U)));
        }
Ejemplo n.º 24
0
        public GeneralMatrix CalculateHessian(double[] x)
        {
            GeneralMatrix hessian = new GeneralMatrix(Dimension, Dimension);

            for (int i = 0; i < Dimension; i++)
            {
                for (int j = 0; j < Dimension; j++)
                {
                    hessian.SetElement(i, j, GetPartialDerivativeVal(i, j, x));
                }
            }
            return(hessian);
        }
Ejemplo n.º 25
0
        /// <summary>
        /// Calculates final model results as a sum of product of choices rated
        /// for each criteria times weighing (preference) of each criteria
        /// </summary>
        private void CalculateFinalResult()
        {
            double sum;

            for (int i = 0; i < _mChoices; i++)
            {
                sum = 0;
                for (int j = 0; j < _nCriteria; j++)
                {
                    sum += _modelResult.GetElement(i, j) * this._orderedCriteria.GetElement(j, 0);
                }
                _calculatedChoices.SetElement(i, 0, sum);
            }
        }
Ejemplo n.º 26
0
        /// <summary>
        /// Average values of the priority matrix over sum of columns.
        /// set values of sum of averaged rows into a new matrix
        /// </summary>
        /// <param name="argMatrix"></param>
        /// <param name="selection"></param>
        public void PCalc(GeneralMatrix argMatrix, GeneralMatrix selection)
        {
            int           n       = argMatrix.ColumnDimension;
            GeneralMatrix sMatrix = new GeneralMatrix(argMatrix.ArrayCopy);

            double c = 0.0;
            int    i, j;

            for (i = 0; i < sMatrix.ColumnDimension; i++)
            {
                c = 0.0;
                for (j = 0; j < sMatrix.RowDimension; j++)
                {
                    c += sMatrix.GetElement(j, i);
                }
                selection.SetElement(i, 0, c);
            }

            for (i = 0; i < sMatrix.ColumnDimension; i++)
            {
                for (j = 0; j < sMatrix.RowDimension; j++)
                {
                    sMatrix.SetElement(j, i, sMatrix.GetElement(j, i) / selection.GetElement(i, 0));
                }
            }

            for (i = 0; i < sMatrix.RowDimension; i++)
            {
                c = 0.0;
                for (j = 0; j < sMatrix.ColumnDimension; j++)
                {
                    c += sMatrix.GetElement(i, j);
                }
                selection.SetElement(i, 0, c / n);
            }
        }
Ejemplo n.º 27
0
        public GeneralMatrix QuantizeScreen(int grid_x, int grid_y)
        {
            // create a general matrix full of 0s
            GeneralMatrix mesh = new GeneralMatrix(grid_x, grid_y, 0.0);

            // Find the bounding box of the points.
            System.Drawing.Rectangle bBox = BoundingBox();

            if (_points.Count == 0)
            {
                return(mesh);
            }

            // find the step we want
            // Note: this is an intentional deviation from the original paper.
            //      We do not maintain the original aspect ratio of the shape.
            double stepX = bBox.Width / (double)(grid_x - 1);
            double stepY = bBox.Height / (double)(grid_y - 1);

            // find the center of the image
            double cx = (double)(bBox.Left + bBox.Right) / 2.0;
            double cy = (double)(bBox.Top + bBox.Bottom) / 2.0;

            foreach (GenericPoint pt in _points)
            {
                // normalize the point's coordinates
                int x_index = (int)Math.Floor(((pt.X - cx) / stepX) + grid_x / 2);
                int y_index = (int)Math.Floor(((pt.Y - cy) / stepY) + grid_y / 2);

                if (x_index < grid_x && y_index < grid_y)
                {
                    mesh.SetElement(y_index, x_index, 1.0);
                }
            }

#if JESSI
            Console.WriteLine("Quantized screen");
            Console.Write(mesh.ToString());
#endif

            return(mesh);
        }
Ejemplo n.º 28
0
        /// <summary>
        /// Calculates and saves the distance transform map for the polar coordinates.
        /// Uses _pMesh and saves to _pDTM.
        /// A distance transform map is a matrix in which every entry represents the
        /// distance from that point to the nearest point with ink.
        /// </summary>
        private void PolarDistanceTransform()
        {
            List <Coord> indices = IndexList(_pMesh);

            for (int i = 0; i < GRID_Y_SIZE; i++)
            {
                for (int j = 0; j < GRID_X_SIZE; j++)
                {
                    Coord cp = new Coord(j, i);

                    double mindist = double.PositiveInfinity;

                    foreach (Coord pt in indices)
                    {
                        // the straight distance between current point and current index
                        double dx           = Math.Abs(cp.X - pt.X);
                        double dy           = Math.Abs(cp.Y - pt.Y);
                        double straightDist = Math.Sqrt(dx * dx + dy * dy);

                        dx = (double)GRID_X_SIZE - dx;
                        double wrapDist = Math.Sqrt(dx * dx + dy * dy);

                        double distan = Math.Min(straightDist, wrapDist);

                        if (distan < mindist)
                        {
                            mindist = distan;
                        }
                    }
                    _pDTM.SetElement(i, j, mindist);
                }
            }

#if FALSE
            Console.WriteLine("Your polar distance transform map:");
            Console.Write(_pDTM.ToString())
#endif
        }
Ejemplo n.º 29
0
        /// <summary>
        ///
        /// </summary>
        /// <param name="p0">First point on route, in projected (metric) coordinates relative to projection origin</param>
        /// <param name="q0">First point on map, in pixels</param>
        /// <param name="p1">Second point on route, in projected (metric) coordinates relative to projection origin</param>
        /// <param name="q1">Second point on map, in pixels</param>
        /// <param name="fallbackMatrix">Matrix to use if calculation fails due to singular matrix</param>
        /// <param name="useRotation">If true, assumes orthogonal map and calculates scale and rotation. If false, calculates different scale in x and y directions and no rotation.</param>
        /// <returns></returns>
        public static GeneralMatrix CalculateTransformationMatrix(PointD p0, PointD q0, PointD p1, PointD q1, GeneralMatrix fallbackMatrix, bool useRotation)
        {
            try
            {
                if (useRotation)
                {
                    // note that we need to mirror y pixel value in x axis
                    double angleDifferece = GetAngleR(p1 - p0, new PointD(q1.X, -q1.Y) - new PointD(q0.X, -q0.Y));
                    double lengthQ        = DistancePointToPoint(q0, q1);
                    double lengthP        = DistancePointToPoint(p0, p1);
                    double scaleFactor    = lengthP == 0 ? 0 : lengthQ / lengthP;
                    double cos            = Math.Cos(angleDifferece);
                    double sin            = Math.Sin(angleDifferece);

                    // translation to origo in metric space
                    var a = new GeneralMatrix(3, 3);
                    a.SetElement(0, 0, 1);
                    a.SetElement(0, 1, 0);
                    a.SetElement(0, 2, -p0.X);
                    a.SetElement(1, 0, 0);
                    a.SetElement(1, 1, 1);
                    a.SetElement(1, 2, -p0.Y);
                    a.SetElement(2, 0, 0);
                    a.SetElement(2, 1, 0);
                    a.SetElement(2, 2, 1);

                    // rotation
                    var b = new GeneralMatrix(3, 3);
                    b.SetElement(0, 0, cos);
                    b.SetElement(0, 1, -sin);
                    b.SetElement(0, 2, 0);
                    b.SetElement(1, 0, sin);
                    b.SetElement(1, 1, cos);
                    b.SetElement(1, 2, 0);
                    b.SetElement(2, 0, 0);
                    b.SetElement(2, 1, 0);
                    b.SetElement(2, 2, 1);

                    // scaling, note that we need to mirror y scale around x axis
                    var c = new GeneralMatrix(3, 3);
                    c.SetElement(0, 0, scaleFactor);
                    c.SetElement(0, 1, 0);
                    c.SetElement(0, 2, 0);
                    c.SetElement(1, 0, 0);
                    c.SetElement(1, 1, -scaleFactor);
                    c.SetElement(1, 2, 0);
                    c.SetElement(2, 0, 0);
                    c.SetElement(2, 1, 0);
                    c.SetElement(2, 2, 1);

                    // translation from origo to pixel space
                    var d = new GeneralMatrix(3, 3);
                    d.SetElement(0, 0, 1);
                    d.SetElement(0, 1, 0);
                    d.SetElement(0, 2, q0.X);
                    d.SetElement(1, 0, 0);
                    d.SetElement(1, 1, 1);
                    d.SetElement(1, 2, q0.Y);
                    d.SetElement(2, 0, 0);
                    d.SetElement(2, 1, 0);
                    d.SetElement(2, 2, 1);

                    return(d * c * b * a);
                }
                else // useRotation == false
                {
                    var m1 = new GeneralMatrix(2, 2);
                    m1.SetElement(0, 0, p0.X);
                    m1.SetElement(0, 1, 1);
                    m1.SetElement(1, 0, p1.X);
                    m1.SetElement(1, 1, 1);

                    var v1 = new GeneralMatrix(2, 1);
                    v1.SetElement(0, 0, q0.X);
                    v1.SetElement(1, 0, q1.X);
                    var t1 = m1.Inverse() * v1;

                    var m2 = new GeneralMatrix(2, 2);
                    m2.SetElement(0, 0, p0.Y);
                    m2.SetElement(0, 1, 1);
                    m2.SetElement(1, 0, p1.Y);
                    m2.SetElement(1, 1, 1);

                    var v2 = new GeneralMatrix(2, 1);
                    v2.SetElement(0, 0, q0.Y);
                    v2.SetElement(1, 0, q1.Y);
                    var t2 = m2.Inverse() * v2;

                    var t = new GeneralMatrix(3, 3);
                    t.SetElement(0, 0, t1.GetElement(0, 0));
                    t.SetElement(0, 1, 0);
                    t.SetElement(0, 2, t1.GetElement(1, 0));
                    t.SetElement(1, 0, 0);
                    t.SetElement(1, 1, t2.GetElement(0, 0));
                    t.SetElement(1, 2, t2.GetElement(1, 0));
                    t.SetElement(2, 0, 0);
                    t.SetElement(2, 1, 0);
                    t.SetElement(2, 2, 1);

                    return(t);
                }
            }
            catch (Exception)
            {
                return((GeneralMatrix)fallbackMatrix.Clone());
            }
        }
Ejemplo n.º 30
0
 public void BadSetValue1()
 {
     B.SetElement(B.RowDimension, B.ColumnDimension - 1, 0.0);
 }