Exemple #1
0
        //---------------------------------------------------------

        /* Matlab version
         * %% forward substitution
         * %  use to find Z for L*Z = C, given L and C
         * function Z = ForwardSub(L,C)
         * Z = zeros(size(C));
         * for c=1:size(C,2)
         *  Z(1,c) = C(1,c)/L(1,1);
         *  for r=2:size(C,1)
         *     sum = 0;
         *     for j=1:(r-1)
         *         sum = sum + L(r,j)*Z(j,c);
         *     end
         *     Z(r,c) = (C(r,c)-sum)/L(r,r);
         *  end
         * end
         * end
         */
        /// <summary> added by Rahul Rastogi
        /// Forward Substitution function to find Z for L*Z = C, given L and C
        /// </summary>
        /// <param name="L">Lower Triangular Matrix, must be square</param>
        /// <param name="C">Matrix to divide into, must have same number of rows as L</param>
        public static UMatrix ForwardSub(UMatrix L, UMatrix C)
        {
            if (L.numCols != L.numRows || L.numRows != C.numRows)
            {
                throw new ArgumentException("Matrix dimensions are not valid.");
            }
            int rows = L.numCols;
            int cols = C.numCols;

            UMatrix Z = new UMatrix(rows, cols);

            fixed(double *ptrZ = Z.data, ptrL = L.data, ptrC = C.data)
            {
                for (int c = 0; c < C.numCols; c++)
                {
                    (*(ptrZ + c)) = (*(ptrC + c)) / (*(ptrL));
                    for (int r = 1; r < C.numRows; r++)
                    {
                        double sum = 0;
                        for (int j = 0; j <= (r - 1); j++)
                        {
                            sum += (*(ptrL + j + (L.numCols * r))) * (*(ptrZ + c + (Z.numCols * j)));
                        }
                        (*(ptrZ + c + (Z.numCols * r))) = (*(ptrC + c + (C.numCols * r)) - sum) / (*(ptrL + r + (L.numCols * r)));
                    }
                }
                return(Z);
            }
        }
Exemple #2
0
        //-----------------------------------------------------------------

        /* Matlab version
         * %% backward substitution
         * % use to find X for U*X = Z, given U and Z
         * function X = BackSub(U,Z)
         * N = size(U,1);
         * X = zeros(N);
         * for c=1:size(Z,2)
         * X(N,c) = Z(N,c)/U(N,N);
         * for r=(N-1):-1:1
         *      sum = 0;
         *     for j=(r+1):N
         *        sum = sum + U(r,j)*X(j,c);
         *    end
         *    X(r,c) = (Z(r,c)-sum)/U(r,r);
         *  end
         * end
         * end
         */

        /// <summary> added by Rahul Rastogi
        /// Backward Substitution function to find X for U*X = Z, given U and Z
        /// </summary>
        /// <param name="U">Upper Triangular Matrix, must be square</param>
        /// <param name="Z">Matrix to divide into, must have same number of rows as U</param>
        public static UMatrix BackwardSub(UMatrix U, UMatrix Z)
        {
            if (U.numCols != U.numRows || U.numRows != Z.numRows)
            {
                throw new ArgumentException("Matrix dimensions are not valid.");
            }

            int     rows = U.numCols;
            int     cols = Z.numCols;
            int     n    = (U.numRows - 1);
            UMatrix X    = new UMatrix(rows, cols);

            fixed(double *ptrX = X.data, ptrU = U.data, ptrZ = Z.data)
            {
                for (int c = 0; c < Z.numCols; c++)
                {
                    (*(ptrX + c + (X.numCols * n))) = (*(ptrZ + c + (Z.numCols * n))) / (*(ptrU + n + (U.numCols * n)));
                    for (int r = (n - 1); r >= 0; r--)
                    {
                        double sum = 0;
                        for (int j = (r + 1); j <= n; j++)
                        {
                            sum += (*(ptrU + j + (U.numCols * r))) * (*(ptrX + c + (X.numCols * j)));
                        }
                        (*(ptrX + c + (X.numCols * r))) = (*(ptrZ + c + (Z.numCols * r)) - sum) / (*(ptrU + r + (U.numCols * r)));
                    }
                }
                return(X);
            }
        }
Exemple #3
0
        /// <summary>Matrix-matrix multiplication.</summary>
        public static UMatrix Multiply(UMatrix left, UMatrix right)
        {
            if (right.numRows != left.numCols)
            {
                throw new ArgumentException("Matrix dimensions are not valid.");
            }

            int     rows    = left.numRows;
            int     columns = right.numCols;
            int     size    = left.numCols;
            UMatrix X       = new UMatrix(rows, columns);

            fixed(double *ptrX = X.data, ptrL = left.data, ptrR = right.data)
            {
                for (int rl = 0; rl < left.numRows; rl++)
                {
                    for (int cr = 0; cr < right.numCols; cr++)
                    {
                        double sum = 0;

                        for (int rr = 0; rr < right.numRows; rr++)
                        {
                            sum += (*(ptrL + rr + (left.numCols * rl))) * (*(ptrR + cr + (right.numCols * rr)));
                            //sum += left.data[rr + (left.numCols * rl)] * right.data[cr + (right.numCols * rr)];
                        }
                        //X.data[cr + (X.numCols * rl)] = sum;
                        (*(ptrX + cr + (X.numCols * rl))) = sum;
                    }
                }
            }

            //-----------------------------------------------------

            return(X);
        }
Exemple #4
0
        /// <summary>Matrix subtraction.</summary>
        public static UMatrix Subtract(UMatrix left, UMatrix right)
        {
            int rows    = left.numRows;
            int columns = left.numCols;

            if ((rows != right.numRows) || (columns != right.numCols))
            {
                throw new ArgumentException("Matrix dimension do not match.");
            }

            UMatrix X = new UMatrix(rows, columns);

            fixed(double *ptrX = X.data, ptrL = left.data, ptrR = right.data)
            {
                for (int i = 0; i < rows; i++)
                {
                    for (int j = 0; j < columns; j++)
                    {
                        //X.data[j + (columns * i)] = left.data[j + (columns * i)] - right.data[j + (columns * i)];
                        (*(ptrX + j + (columns * i))) = (*(ptrL + j + (columns * i))) - (*(ptrR + j + (columns * i)));
                    }
                }
            }

            return(X);
        }
Exemple #5
0
        /// <summary>Unary minus.</summary>
        public static UMatrix Negate(UMatrix value)
        {
            if (value == null)
            {
                throw new ArgumentNullException("value");
            }

            int     rows    = value.numRows;
            int     columns = value.numCols;
            UMatrix X       = new UMatrix(rows, columns);

            fixed(double *ptrX = X.data, ptrThis = value.data)
            {
                for (int i = 0; i < rows; i++)
                {
                    for (int j = 0; j < columns; j++)
                    {
                        //X.data[j + (columns * i)] = -value.data[j + (columns * i)];
                        (*(ptrX + j + (columns * i))) = -(*(ptrThis + j + (columns * i)));
                    }
                }
            }

            return(X);
        }
Exemple #6
0
        public UMatrix Inverse()
        {
            UMatrix upper, lower, c, z;

            LuDecomposition(out lower, out upper);
            c = new UMatrix(numRows, numCols, 1);
            z = UMatrix.ForwardSub(lower, c);

            return(UMatrix.BackwardSub(upper, z));
        }
Exemple #7
0
 public UMatrix(UMatrix m)
 {
     this.numRows = m.numRows;
     this.numCols = m.numCols;
     this.data    = new double[numRows * numCols];
     for (int i = 0; i < numRows; i++)
     {
         for (int j = 0; j < numCols; j++)
         {
             this[i, j] = m[i, j];
         }
     }
 }
Exemple #8
0
 public UMatrix(UMatrix m)
 {
     this.numRows = m.numRows;
     this.numCols = m.numCols;
     this.data = new double[numRows * numCols];
     for (int i = 0; i < numRows; i++)
     {
         for (int j = 0; j < numCols; j++)
         {
             this[i, j] = m[i, j];
         }
     }
 }
Exemple #9
0
        /// <summary>Returns the transposed matrix.</summary>
        public UMatrix Transpose()
        {
            UMatrix X = new UMatrix(numCols, numRows);

            fixed(double *ptrX = X.data, ptrThis = data)
            {
                for (int i = 0; i < numRows; i++)
                {
                    for (int j = 0; j < numCols; j++)
                    {
                        //X.data[i + (numRows * j)] = data[j + (numCols * i)];
                        (*(ptrX + i + (numRows * j))) = (*(ptrThis + j + (numCols * i)));
                    }
                }
            }

            return(X);
        }
Exemple #10
0
        /// <summary>Matrix-scalar multiplication.</summary>
        public static UMatrix Multiply(UMatrix left, double right)
        {
            int rows    = left.numRows;
            int columns = left.numCols;

            UMatrix X = new UMatrix(rows, columns);

            fixed(double *ptrX = X.data, ptrL = left.data)
            {
                for (int i = 0; i < rows; i++)
                {
                    for (int j = 0; j < columns; j++)
                    {
                        //X.data[j + (columns * i)] = left.data[j + (columns * i)] * right;
                        (*(ptrX + j + (columns * i))) = ((*(ptrL + j + (columns * i))) * right);
                    }
                }
            }

            return(X);
        }
Exemple #11
0
        /// <summary>Returns a sub matrix extracted from the current matrix.</summary>
        /// <param name="startRow">Start row index</param>
        /// <param name="endRow">End row index</param>
        /// <param name="startColumn">Start column index</param>
        /// <param name="endColumn">End column index</param>
        public UMatrix Submatrix(int startRow, int endRow, int startColumn, int endColumn)
        {
            if ((startRow > endRow) || (startColumn > endColumn) || (startRow < 0) || (startRow >= this.numRows) || (endRow < 0) || (endRow >= this.numRows) || (startColumn < 0) || (startColumn >= this.numCols) || (endColumn < 0) || (endColumn >= this.numCols))
            {
                throw new ArgumentException("Argument out of range.");
            }

            UMatrix X = new UMatrix(endRow - startRow + 1, endColumn - startColumn + 1);

            fixed(double *ptrX = X.data, ptrThis = data)
            {
                for (int i = startRow; i <= endRow; i++)
                {
                    for (int j = startColumn; j <= endColumn; j++)
                    {
                        //X.data[j + (X.numCols * i)] = data[j + (numCols * i)];
                        (*(ptrX + j + (X.numCols * i))) = (*(ptrThis + j + (numCols * i)));
                    }
                }
            }

            return(X);
        }
Exemple #12
0
        /// <summary>Matrix-matrix multiplication.</summary>
        public static UMatrix Multiply(UMatrix left, UMatrix right)
        {
            if (right.numRows != left.numCols)
            {
                throw new ArgumentException("Matrix dimensions are not valid.");
            }

            int rows = left.numRows;
            int columns = right.numCols;
            int size = left.numCols;
            UMatrix X = new UMatrix(rows, columns);

            fixed (double* ptrX = X.data, ptrL = left.data, ptrR = right.data)
            {
                for (int rl = 0; rl < left.numRows; rl++)
                {
                    for (int cr = 0; cr < right.numCols; cr++)
                    {
                        double sum = 0;

                        for (int rr = 0; rr < right.numRows; rr++)
                        {
                            sum += (*(ptrL + rr + (left.numCols * rl))) * (*(ptrR + cr + (right.numCols * rr)));
                            //sum += left.data[rr + (left.numCols * rl)] * right.data[cr + (right.numCols * rr)];
                        }
                        //X.data[cr + (X.numCols * rl)] = sum;
                        (*(ptrX + cr + (X.numCols * rl))) = sum;
                    }
                }
            }

            //-----------------------------------------------------

            return X;
        }
        UMatrix ComputeJacobianQ(double q1, double q2, double q3, double q4)
        {
            UMatrix toReturn = new UMatrix(16, 7);
            toReturn.Zero();

            toReturn[12, 0] = 1;
            toReturn[13, 1] = 1;
            toReturn[14, 2] = 1;

            toReturn[0, 3] = 2 * q1;
            toReturn[1, 3] = 2 * q2;
            toReturn[2, 3] = 2 * q3;
            toReturn[4, 3] = 2 * q2;
            toReturn[5, 3] = -2 * q1;
            toReturn[6, 3] = 2 * q4;
            toReturn[8, 3] = 2 * q3;
            toReturn[9, 3] = -2 * q4;
            toReturn[10, 3] = -2 * q1;

            toReturn[0, 4] = -2 * q2;
            toReturn[1, 4] = 2 * q1;
            toReturn[2, 4] = -2 * q4;
            toReturn[4, 4] = 2 * q1;
            toReturn[5, 4] = 2 * q2;
            toReturn[6, 4] = 2 * q3;
            toReturn[8, 4] = 2 * q4;
            toReturn[9, 4] = 2 * q3;
            toReturn[10, 4] = -2 * q2;

            toReturn[0, 5] = -2 * q3;
            toReturn[1, 5] = 2 * q4;
            toReturn[2, 5] = 2 * q1;
            toReturn[4, 5] = -2 * q4;
            toReturn[5, 5] = -2 * q3;
            toReturn[6, 5] = 2 * q2;
            toReturn[8, 5] = 2 * q1;
            toReturn[9, 5] = 2 * q2;
            toReturn[10, 5] = 2 * q3;

            toReturn[0, 6] = 2 * q4;
            toReturn[1, 6] = 2 * q3;
            toReturn[2, 6] = -2 * q2;
            toReturn[4, 6] = -2 * q3;
            toReturn[5, 6] = 2 * q4;
            toReturn[6, 6] = 2 * q1;
            toReturn[8, 6] = 2 * q2;
            toReturn[9, 6] = -2 * q1;
            toReturn[10, 6] = 2 * q4;

            return toReturn;
        }
Exemple #14
0
        //---------------------------------------------------------
        /* Matlab version
        %% forward substitution
        %  use to find Z for L*Z = C, given L and C
        function Z = ForwardSub(L,C)
        Z = zeros(size(C));
        for c=1:size(C,2)
            Z(1,c) = C(1,c)/L(1,1);
            for r=2:size(C,1)
               sum = 0;
               for j=1:(r-1)
                   sum = sum + L(r,j)*Z(j,c);
               end
               Z(r,c) = (C(r,c)-sum)/L(r,r);
            end
        end
        end
        */
        /// <summary> added by Rahul Rastogi
        /// Forward Substitution function to find Z for L*Z = C, given L and C
        /// </summary>
        /// <param name="L">Lower Triangular Matrix, must be square</param>
        /// <param name="C">Matrix to divide into, must have same number of rows as L</param>
        public static UMatrix ForwardSub(UMatrix L, UMatrix C)
        {
            if (L.numCols != L.numRows || L.numRows != C.numRows)
            {
                throw new ArgumentException("Matrix dimensions are not valid.");
            }
            int rows = L.numCols;
            int cols = C.numCols;

            UMatrix Z = new UMatrix(rows, cols);
            fixed (double* ptrZ = Z.data, ptrL = L.data, ptrC = C.data)
            {
                for (int c = 0; c < C.numCols; c++)
                {
                    (*(ptrZ + c)) = (*(ptrC + c)) / (*(ptrL));
                    for (int r = 1; r < C.numRows; r++)
                    {
                        double sum = 0;
                        for (int j = 0; j <= (r - 1); j++)
                        {
                            sum += (*(ptrL + j + (L.numCols * r))) * (*(ptrZ + c + (Z.numCols * j)));
                        }
                        (*(ptrZ + c + (Z.numCols * r))) = (*(ptrC + c + (C.numCols * r)) - sum) / (*(ptrL + r + (L.numCols * r)));
                    }
                }
                return Z;
            }
        }
Exemple #15
0
        /// <summary>Matrix-scalar multiplication.</summary>
        public static UMatrix Multiply(UMatrix left, double right)
        {
            int rows = left.numRows;
            int columns = left.numCols;

            UMatrix X = new UMatrix(rows, columns);
            fixed (double* ptrX = X.data, ptrL = left.data)
            {
                for (int i = 0; i < rows; i++)
                {
                    for (int j = 0; j < columns; j++)
                    {
                        //X.data[j + (columns * i)] = left.data[j + (columns * i)] * right;
                        (*(ptrX + j + (columns * i))) = ((*(ptrL + j + (columns * i))) * right);
                    }
                }
            }
            return X;
        }
        public RobotTwoWheelCommand GetCommand()
        {
            double g = 3;
            double zeta = .7; //parameters for STC
            double dt = .4; // adjust to change speed
            double deltadist = .10; //incremental distance for looking ahead
            // deltadist/dt = nominal speed along path

            //get trajectory // this step is performed outside of this function by BehavioralDirector
            //discretize trajectory into [xr, yr]

            //generate reference velocities at each trajectory point [vr, wr, thetar] if new path
            if (!pathsame)
            {
                this.GenrefVWHuniformdt(pathCurrentlyTracked, deltadist, dt);
            }
            //use current timestamp to find corresponding index in reference vectors
            //double currenttime = TimeSync.CurrentTime;
            double currenttime = (DateTime.Now.Ticks - 621355968000000000) / 10000000.0;
            int indextotrack = (int) (Math.Ceiling((currenttime - timestamps[0]) / dt));
            if (indextotrack >= timestamps.Length)
                indextotrack = timestamps.Length - 1;
            Console.WriteLine("indextotrack = {0}", indextotrack);
            Console.WriteLine("time = {0}", currenttime);

            //unwrap thetar against thetarold, ignore if null (1337)
            if (!endpointsame) { thetarold = 1337; }
            if (thetarold != 1337)
            {
                while (hr[indextotrack] - thetarold >= Math.PI)
                {
                    hr[indextotrack] -= 2 * Math.PI; ;
                }
                while (hr[indextotrack] - thetarold <= -Math.PI)
                {
                    hr[indextotrack] += 2 * Math.PI;
                }
            }
            //set thetarold to thetar
            thetarold = hr[indextotrack];

            double[] qrefdata = { xr[indextotrack], yr[indextotrack], hr[indextotrack] };
            UMatrix qref = new UMatrix(3, 1, qrefdata);
            //print to qref to screen // use only for debugging
            //string qrefstring = qref.ToString();
            //Console.WriteLine("QREF\n {0}", qrefstring);

            //establish pose vector q = [x, y, theta];
            double x, y, theta;
            x = this.currentPoint.x;
            y = this.currentPoint.y;
            theta = this.currentPoint.yaw; // in radians
            double[] qdata = { x, y, theta };
            UMatrix q = new UMatrix(3, 1, qdata);

            //unwrap pose against qref
            while (q[2, 0] - qref[2, 0] >= Math.PI)
            {
                q[2, 0] -= 2 * Math.PI; ;
            }
            while (q[2, 0] - qref[2, 0] <= -Math.PI)
            {
                q[2, 0] += 2 * Math.PI;
            }

            //establish kinematic state error
            UMatrix delq = new UMatrix(3, 1);
            delq = qref - q;

            //if (!endpointsame) { delqthetarold = 1337; }
            //if (delqthetarold != 1337)
            //{
            //    while (delq[2,0] - delqthetarold >= Math.PI)
            //    {
            //        delq[2,0] -= 2 * Math.PI; ;
            //    }
            //    while (delq[2,0] - delqthetarold <= -Math.PI)
            //    {
            //        delq[2,0] += 2 * Math.PI;
            //    }
            //}
            ////set thetarold to thetar
            //delqthetarold = delq[2,0];

            //print delq to screen // use only for debugging
            string delqstring = delq.ToString();
            //Console.WriteLine("ERROR\n {0}", delqstring);
            //Console.ReadLine();

            //create rotation matrix
            double[] Rotdata = { Math.Cos(theta), Math.Sin(theta), 0, -Math.Sin(theta), Math.Cos(theta), 0, 0, 0, 1 };
            UMatrix Rot = new UMatrix(3, 3, Rotdata);

            //convert to robot frame
            UMatrix e = new UMatrix(3, 1);
            e = Rot * delq; //error in the robot reference frame: first element is along track error, second element is cross track error, last element is angle error
            string estring = e.ToString();
            Console.WriteLine("ERROR\n {0}", estring);

            // calculate Uf = [vf, wf]
            double vf = vr[indextotrack] * Math.Cos(e[2, 0]); // in meters/sec
            double wf = wr[indextotrack] * 180 / Math.PI; // in degrees/sec
            Console.WriteLine("vf = {0}, wf = {1}",vf,wf);

            //calculate wn = sqrt(wr^2 + g*vr^2)
            double wn = Math.Sqrt(Math.Pow(wr[indextotrack], 2) + g * (Math.Pow(vr[indextotrack], 2)));

            //calculte STC gain matrix
            double k1, k2, k3;
            k1 = 2 * zeta * wn;
            k3 = k1;
            k2 = g * vr[indextotrack];
            double[] Kdata = { k1, 0, 0, 0, k2, k3 };
            UMatrix K = new UMatrix(2, 3, Kdata);

            //find control values Ub = K*e
            UMatrix Ub = new UMatrix(2, 1);
            Ub = K * e;
            Console.WriteLine("vb = {0}, wb = {1}", Ub[0, 0], Ub[1, 0]);

            //find total control u = Uf + Ub
            double vcommand = (Ub[0, 0] + vf) * 3.6509; // multiply by 3.6509 to get "segway units"
            double wcommand = (Ub[1, 0] * 180 / Math.PI + wf) * 3.9; // convert to deg/sec and multiply by 3.9 for "segway units" (counts/deg/sec)

            RobotTwoWheelCommand command = new RobotTwoWheelCommand(vcommand * 2.23693629, wcommand);
            if (Math.Abs(wcommand) >= 600)
                Console.WriteLine("W saturation");

            //create text file of information
            if (indextotrack < timestamps.Length && counter < 15)
            {
                if (indextotrack == timestamps.Length - 1) { counter = counter + 1; }
                tw2.WriteLine("{0}  {1}  {2}  {3}  {4}  {5}  {6}  {7}  {8}  {9}  {10}  {11} ", indextotrack, timestamps[indextotrack], x, y, e[0, 0], e[1, 0], e[2, 0], vf, wf, Ub[0, 0], Ub[1, 0]);
            }
            else { tw2.Close(); }

            return command;
        }
Exemple #17
0
        //-----------------------------------------------------------------
        /* Matlab version
        %% backward substitution
        % use to find X for U*X = Z, given U and Z
        function X = BackSub(U,Z)
        N = size(U,1);
        X = zeros(N);
        for c=1:size(Z,2)
           X(N,c) = Z(N,c)/U(N,N);
           for r=(N-1):-1:1
                sum = 0;
               for j=(r+1):N
                  sum = sum + U(r,j)*X(j,c);
              end
              X(r,c) = (Z(r,c)-sum)/U(r,r);
            end
        end
        end
        */
        /// <summary> added by Rahul Rastogi
        /// Backward Substitution function to find X for U*X = Z, given U and Z
        /// </summary>
        /// <param name="U">Upper Triangular Matrix, must be square</param>
        /// <param name="Z">Matrix to divide into, must have same number of rows as U</param>
        public static UMatrix BackwardSub(UMatrix U, UMatrix Z)
        {
            if (U.numCols != U.numRows || U.numRows != Z.numRows)
            {
                throw new ArgumentException("Matrix dimensions are not valid.");
            }

            int rows = U.numCols;
            int cols = Z.numCols;
            int n = (U.numRows-1);
            UMatrix X = new UMatrix(rows, cols);
            fixed (double* ptrX = X.data, ptrU = U.data, ptrZ = Z.data)
            {
                for (int c = 0; c < Z.numCols; c++)
                {
                    (*(ptrX + c + (X.numCols * n))) = (*(ptrZ + c + (Z.numCols * n))) / (*(ptrU + n + (U.numCols * n)));
                    for (int r = (n-1); r >= 0; r--)
                    {
                        double sum = 0;
                        for (int j = (r+1); j <= n; j++)
                        {
                            sum += (*(ptrU + j + (U.numCols * r))) * (*(ptrX + c + (X.numCols * j)));
                        }
                        (*(ptrX + c + (X.numCols * r))) = (*(ptrZ + c + (Z.numCols * r)) - sum) / (*(ptrU + r + (U.numCols * r)));
                    }
                }
                return X;
            }
        }
        private UMatrix ComputeJacobianZYX(double yaw, double pitch, double roll)
        {
            UMatrix m = new UMatrix(16, 6);
            m.Zero();

            m[12, 0] = 1;

            m[13, 1] = 1;

            m[14, 2] = 1;

            m[1, 3] = Math.Cos(roll) * Math.Sin(pitch) * Math.Cos(yaw) + Math.Sin(roll) * Math.Sin(yaw);
            m[2, 3] = -Math.Sin(roll) * Math.Sin(pitch) * Math.Cos(yaw) + Math.Cos(roll) * Math.Sin(yaw);
            m[5, 3] = Math.Cos(roll) * Math.Sin(pitch) * Math.Sin(yaw) - Math.Sin(roll) * Math.Cos(yaw);
            m[6, 3] = -Math.Sin(roll) * Math.Sin(pitch) * Math.Sin(yaw) - Math.Cos(roll) * Math.Cos(yaw);
            m[9, 3] = Math.Cos(roll) * Math.Cos(pitch);
            m[10, 3] = -Math.Sin(roll) * Math.Cos(pitch);

            m[0, 4] = -Math.Sin(pitch) * Math.Cos(yaw);
            m[1, 4] = Math.Sin(roll) * Math.Cos(pitch) * Math.Cos(yaw);
            m[2, 4] = Math.Cos(roll) * Math.Cos(pitch) * Math.Cos(yaw);
            m[4, 4] = -Math.Sin(pitch) * Math.Sin(yaw);
            m[5, 4] = Math.Sin(roll) * Math.Cos(pitch) * Math.Sin(yaw);
            m[6, 4] = Math.Cos(roll) * Math.Cos(pitch) * Math.Sin(yaw);
            m[8, 4] = -Math.Cos(pitch);
            m[9, 4] = -Math.Sin(roll) * Math.Sin(pitch);
            m[10, 4] = -Math.Cos(roll) * Math.Sin(pitch);

            m[0, 5] = -Math.Cos(pitch) * Math.Sin(yaw);
            m[1, 5] = -Math.Sin(roll) * Math.Sin(pitch) * Math.Sin(yaw) - Math.Cos(roll) * Math.Cos(yaw);
            m[2, 5] = -Math.Cos(roll) * Math.Sin(pitch) * Math.Sin(yaw) + Math.Sin(roll) * Math.Cos(yaw);
            m[4, 5] = Math.Cos(pitch) * Math.Cos(yaw);
            m[5, 5] = Math.Sin(roll) * Math.Sin(pitch) * Math.Cos(yaw) - Math.Cos(roll) * Math.Sin(yaw);
            m[6, 5] = Math.Cos(roll) * Math.Sin(pitch) * Math.Cos(yaw) + Math.Sin(roll) * Math.Sin(yaw);

            return m;
        }
        private void UpdateCovariance(Matrix robotCov)
        {
            lock (locker)
            {
                UMatrix abc = new UMatrix(6, 6);
                for (int i = 0; i < 6; i++)
                {
                    for (int j = 0; j < 6; j++)
                    {
                        abc[i, j] = robotCov[i, j]; // this is a heck !!!!!
                    }

                    if (abc[i, i] < 0)
                        abc[i, i] = Math.Abs(abc[i, i]);
                }
                if (abc[2, 2] == 0) abc[2, 2] = 0.01;
                if (abc[4, 4] == 0) abc[4, 4] = 0.01;
                if (abc[5, 5] == 0) abc[5, 5] = 0.01;
                covRobotPose = abc;
            }
        }
Exemple #20
0
        //----------------------------------------------------------------------------

        public UMatrix LuDecomposition(out UMatrix l, out UMatrix u)
        {
            double[] lu = new double[data.Length];
            Array.Copy(data, lu, data.Length);

            //double[] luRowI;
            double *luRowIPtr;

            double[] luColJ = new double[numRows];
            double[] lData  = new double[numRows * numRows];
            double[] uData  = new double[numRows * numRows];

            fixed(double *luPtr = lu, luColJPtr = luColJ, lDataPtr = lData, uDataPtr = uData)
            {
                int pivotSign = 1;

                int[] pivotVector = new int[numRows];
                for (int i = 0; i < numRows; i++)
                {
                    pivotVector[i] = i;
                }

                // Outer loop.
                for (int j = 0; j < numCols; j++)
                {
                    // Make a copy of the j-th column to localize references.
                    for (int i = 0; i < numRows; i++)
                    {
                        *(luColJPtr + i) = *(luPtr + j + i * numCols);
                    }

                    // Apply previous transformations.
                    for (int i = 0; i < numRows; i++)
                    {
                        luRowIPtr = luPtr + i * numCols;

                        // Most of the time is spent in the following dot product.
                        //int kmax = (i > j) ? j : i;
                        int    kmax = Math.Min(i, j);
                        double s    = 0.0;
                        for (int k = 0; k < kmax; k++)
                        {
                            s += *(luRowIPtr + k) * *(luColJPtr + k);
                        }
                        *(luRowIPtr + j) = *(luColJPtr + i) -= s;
                    }

                    // Find pivot and exchange if necessary.
                    int p = j;
                    for (int i = j + 1; i < numRows; i++)
                    {
                        if (Math.Abs(*(luColJPtr + i)) > Math.Abs(*(luColJPtr + p)))
                        {
                            p = i;
                        }
                    }

                    if (p != j)
                    {
                        for (int k = 0; k < numCols; k++)
                        {
                            double t = *(luPtr + k + p * numCols);
                            *(luPtr + k + p * numCols) = *(luPtr + k + j * numCols);
                            *(luPtr + k + j * numCols) = t;
                        }

                        int v = pivotVector[p];
                        pivotVector[p] = pivotVector[j];
                        pivotVector[j] = v;

                        pivotSign = -pivotSign;
                    }

                    // Compute multipliers.
                    if (j < numRows & *(luPtr + j + j * numCols) != 0.0)
                    {
                        for (int i = j + 1; i < numRows; i++)
                        {
                            *(luPtr + j + i * numCols) /= *(luPtr + j + j * numCols);
                        }
                    }
                }



                for (int i = 0; i < numRows; i++)
                {
                    for (int j = 0; j < numCols; j++)
                    {
                        *(lDataPtr + j + i * numCols) = (j == i) ? 1 : (i < j) ? 0 : *(luPtr + j + i * numCols);
                        *(uDataPtr + j + i * numCols) = (i > j) ? 0 : *(luPtr + j + i * numCols);
                    }
                }

                l = new UMatrix(numRows, numCols, lData);
                u = new UMatrix(numRows, numCols, uData);
            }

            return(new UMatrix(numRows, numCols, lu));
        }
        /// <summary>
        /// Update OccupancyGrid based on lidarScan and robotPose received
        /// </summary>
        /// <param name="lidarScan"></param>
        /// <param name="currentRobotPose"></param>
        public void UpdateOccupancyGrid(ILidarScan<ILidar2DPoint> lidarScan, int robotID, RobotPose currentRobotPose, SensorPose lidarPose, List<Polygon> dynamicObstacles)
        {
            if (lidarPose == null)
            {
                lidarPose = new SensorPose(0, 0, 0.5, 0, 0 * Math.PI / 180.0, 0, 0);
            }
            if (laser2RobotTransMatrixDictionary.ContainsKey(robotID))
            {
                JTpl = jacobianLaserPoseDictionary[robotID];
                laserToRobotDCM = laser2RobotTransMatrixDictionary[robotID];
            }
            else
            {
                Matrix4 laser2RobotDCM = Matrix4.FromPose(lidarPose);
                for (int i = 0; i < 4; i++)
                {
                    for (int j = 0; j < 4; j++)
                    {
                        laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
                    }
                }
                laser2RobotTransMatrixDictionary.Add(robotID, laserToRobotDCM);
                jacobianLaserPoseDictionary.Add(robotID, ComputeJacobian(lidarPose.yaw, lidarPose.pitch, lidarPose.roll));
                JTpl = jacobianLaserPoseDictionary[robotID];
            }

            // calculate robot2global transformation matrix
            Matrix4 robot2GlocalDCM = Matrix4.FromPose(currentRobotPose);
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    robotToGlocalDCM[i, j] = robot2GlocalDCM[i, j];
                }
            }

            if (lidarScan == null) return;
            Stopwatch sw1 = new Stopwatch();
            Stopwatch sw2 = new Stopwatch();
            Stopwatch sw3 = new Stopwatch();
            Stopwatch sw4 = new Stopwatch();
            Stopwatch sw5 = new Stopwatch();
            Stopwatch sw6 = new Stopwatch();

            UMatrix JTpr = ComputeJacobian(currentRobotPose.yaw, currentRobotPose.pitch, currentRobotPose.roll);
            List<UMatrix> JfPrCubixLaserToRobotDCM = new List<UMatrix>(6);
            List<UMatrix> RobotToGlocalDCMJfPlCubix = new List<UMatrix>(6);
            for (int i = 0; i < 6; i++)
            {
                //derivative of the robot transform matrtix w.r.t. some element of the robot psoe
                UMatrix j = new UMatrix(4, 4);
                j[0, 0] = JTpr[0, i]; j[1, 0] = JTpr[1, i]; j[2, 0] = JTpr[2, i]; j[3, 0] = JTpr[3, i];
                j[0, 1] = JTpr[4, i]; j[1, 1] = JTpr[5, i]; j[2, 1] = JTpr[6, i]; j[3, 1] = JTpr[7, i];
                j[0, 2] = JTpr[8, i]; j[1, 2] = JTpr[9, i]; j[2, 2] = JTpr[10, i]; j[3, 2] = JTpr[11, i];
                j[0, 3] = JTpr[12, i]; j[1, 3] = JTpr[13, i]; j[2, 3] = JTpr[14, i]; j[3, 3] = JTpr[15, i];
                JfPrCubixLaserToRobotDCM.Add(j * laserToRobotDCM);

                UMatrix tempJacobianPl = new UMatrix(4, 4);
                tempJacobianPl[0, 0] = JTpl[0, i]; tempJacobianPl[1, 0] = JTpl[1, i]; tempJacobianPl[2, 0] = JTpl[2, i]; tempJacobianPl[3, 0] = JTpl[3, i];
                tempJacobianPl[0, 1] = JTpl[4, i]; tempJacobianPl[1, 1] = JTpl[5, i]; tempJacobianPl[2, 1] = JTpl[6, i]; tempJacobianPl[3, 1] = JTpl[7, i];
                tempJacobianPl[0, 2] = JTpl[8, i]; tempJacobianPl[1, 2] = JTpl[9, i]; tempJacobianPl[2, 2] = JTpl[10, i]; tempJacobianPl[3, 2] = JTpl[11, i];
                tempJacobianPl[0, 3] = JTpl[12, i]; tempJacobianPl[1, 3] = JTpl[13, i]; tempJacobianPl[2, 3] = JTpl[14, i]; tempJacobianPl[3, 3] = JTpl[15, i];
                RobotToGlocalDCMJfPlCubix.Add(robotToGlocalDCM * tempJacobianPl);
            }
            UMatrix laserToENU = robotToGlocalDCM * laserToRobotDCM;
            UMatrix pijCell = new UMatrix(rangeToApply * 2 + 1, rangeToApply * 2 + 1);
            // update covariance
            UpdateCovariance(currentRobotPose.covariance);

            //SickPoint p = new SickPoint(new RThetaCoordinate(1.0f, 0.0f));
            for (int laserIndex = 0; laserIndex < lidarScan.Points.Count; laserIndex++)
            {
                lock (locker)
                {

                    ILidar2DPoint p = lidarScan.Points[laserIndex];
                    if (p.RThetaPoint.R >= MAXRANGE) continue;

                    bool hitDynamicObstacles = false;
                    // figure out if this lidar point is hitting other robot

                    // find laser points in 3D space
                    // first define 2D point on the laser plane
                    UMatrix laserPoint = new UMatrix(4, 1);

                    double deg = (p.RThetaPoint.theta * 180.0 / Math.PI);
                    int thetaDegIndex = (int)Math.Round((deg + 90.0) * 2.0) % 360;
                    double cosTheta = cosLookup[thetaDegIndex];
                    double sinTheta = sinLookup[thetaDegIndex];

                    laserPoint[0, 0] = p.RThetaPoint.R * cosTheta;
                    laserPoint[1, 0] = p.RThetaPoint.R * sinTheta;
                    laserPoint[2, 0] = 0;
                    laserPoint[3, 0] = 1;

                    //calculate r_hat_ENU
                    UMatrix r_hat_ENU = laserToENU * laserPoint;

                    foreach (Polygon dp in dynamicObstacles)
                    {
                        if (dp.IsInside(new Vector2(r_hat_ENU[0, 0], r_hat_ENU[1, 0])))
                        {
                            hitDynamicObstacles = true;
                            break;
                        }
                    }
                    if (hitDynamicObstacles) continue;

                    //-------------------------------//
                    // COVARIANCE UMatrix CALCULATION //
                    //-------------------------------//
                    UMatrix JRr = new UMatrix(4, 2);
                    JRr.Zero();
                    JRr[0, 0] = cosTheta;
                    JRr[0, 1] = -p.RThetaPoint.R * sinTheta;
                    JRr[1, 0] = sinTheta;
                    JRr[1, 1] = p.RThetaPoint.R * cosTheta;

                    UMatrix Jfr = new UMatrix(3, 2); // 3x2
                    Jfr = (laserToENU * JRr).Submatrix(0, 2, 0, 1);	 // 4x4 * 4x4 * 4x2

                    UMatrix Jfpr = new UMatrix(3, 6);
                    UMatrix Jfpl = new UMatrix(3, 6);
                    sw1.Reset();
                    sw1.Start();

                    for (int i = 0; i < 6; i++) //for each state var (i.e. x,y,z,y,p,r)
                    {
                        UMatrix JfprTemp = (JfPrCubixLaserToRobotDCM[i]) * laserPoint; // 4 by 1 UMatrix
                        Jfpr[0, i] = JfprTemp[0, 0];
                        Jfpr[1, i] = JfprTemp[1, 0];
                        Jfpr[2, i] = JfprTemp[2, 0];

                        //UMatrix JfplTemp = (RobotToGlocalDCMJfPlCubix[i]) * laserPoint; // 4 by 1 UMatrix
                        //Jfpl[0, i] = JfplTemp[0, 0];
                        //Jfpl[1, i] = JfplTemp[1, 0];
                        //Jfpl[2, i] = JfplTemp[2, 0];
                    }
                    sw1.Stop();
                    sw2.Reset();
                    sw2.Start();
                    UMatrix JfprQprJfprT = new UMatrix(3, 3);
                    UMatrix JfplQplJfplT = new UMatrix(3, 3);
                    UMatrix JfrQrJfrT = new UMatrix(3, 3);
                    JfprQprJfprT = (Jfpr * covRobotPose) * Jfpr.Transpose();
                    //JfplQplJfplT = (Jfpl * covLaserPose) * Jfpl.Transpose();
                    JfrQrJfrT = (Jfr * covLaserScan) * Jfr.Transpose();

                    // add above variables together and get the covariance
                    UMatrix covRHatENU = JfprQprJfprT + /*JfplQplJfplT +*/ JfrQrJfrT; // 3x3 UMatrix
                    sw2.Stop();
                    sw3.Reset();
                    sw3.Start();
                    //-----------------------------//
                    // FIND WHICH CELLS TO COMPUTE //
                    //-----------------------------//

                    // find out cells around this laser point
                    int laserPointIndexX = 0;
                    int laserPointIndexY = 0;
                    //this is used just to do the transformation from reaal to grid and visa versa
                    psig_u_hat_square.GetIndicies(r_hat_ENU[0, 0], r_hat_ENU[1, 0], out laserPointIndexX, out laserPointIndexY); // get cell (r_hat_ENU_X, r_hat_ENU_y)
                    sw3.Stop();
                    sw4.Reset();
                    sw4.Start();
                    //-----------------------------------------//
                    // COMPUTE THE DISTRIBUTION OF UNCERTAINTY //
                    //-----------------------------------------//

                    double sigX = Math.Sqrt(covRHatENU[0, 0]);
                    double sigY = Math.Sqrt(covRHatENU[1, 1]);
                    double rho = covRHatENU[1, 0] / (sigX * sigY);
                    double logTerm = Math.Log(2 * Math.PI * sigX * sigY * Math.Sqrt(1 - (rho * rho)));
                    double xTermDenom = (1 - (rho * rho));
                    for (int i = -rangeToApply; i <= rangeToApply; i++) // column
                    {
                        for (int j = -rangeToApply; j <= rangeToApply; j++) // row
                        {
                            // estimate using Bivariate Normal Distribution
                            double posX = 0; double posY = 0;
                            psig_u_hat_square.GetReals(laserPointIndexX + i, laserPointIndexY + j, out posX, out posY);
                            posX += psig_u_hat_square.ResolutionX / 2;
                            posY += psig_u_hat_square.ResolutionY / 2;
                            double x = posX - r_hat_ENU[0, 0];
                            double y = posY - r_hat_ENU[1, 0];
                            double z = (x * x) / (sigX * sigX) -
                                                (2 * rho * x * y / (sigX * sigY)) +
                                                 (y * y) / (sigY * sigY);
                            double xTerm = -0.5 * z / xTermDenom;
                            // chi2 test
                            //if ((2 * (1 - (rho * rho))) * ((x * x) / (sigX * sigX) + (y * y) / (sigY * sigY) - (2 * rho * x * y) / (sigX * sigY)) > 15.2)
                            //  continue;
                            pijCell[j + rangeToApply, i + rangeToApply] = Math.Exp(xTerm - logTerm) * psig_u_hat_square.ResolutionX * psig_u_hat_square.ResolutionY;
                            laserHit.SetCellByIdx(laserPointIndexX + i, laserPointIndexY + j, laserHit.GetCellByIdx(laserPointIndexX + i, laserPointIndexY + j) + 1);
                        }
                    }
                    sw4.Stop();
                    sw5.Reset();
                    sw5.Start();

                    //---------------------------//
                    // COMPUTE HEIGHT ESTIMATION //
                    //---------------------------//
                    //				Matrix2 PEN = new Matrix2(covRHatENU[0, 0], covRHatENU[0, 1], covRHatENU[1, 0], covRHatENU[1, 1]);

                    UMatrix PEN = covRHatENU.Submatrix(0, 1, 0, 1);

                    UMatrix PENInv = PEN.Inverse2x2;

                    UMatrix PuEN = new UMatrix(1, 2);
                    UMatrix PENu = new UMatrix(2, 1);
                    UMatrix PuENPENInv = PuEN * PENInv;
                    UMatrix uHatMatrix = new UMatrix(rangeToApply * 2 + 1, rangeToApply * 2 + 1);
                    UMatrix sigUHatMatrix = new UMatrix(rangeToApply * 2 + 1, rangeToApply * 2 + 1);

                    PuEN[0, 0] = covRHatENU[2, 0];
                    PuEN[0, 1] = covRHatENU[2, 1];

                    PENu[0, 0] = covRHatENU[0, 2];
                    PENu[1, 0] = covRHatENU[1, 2];

                    double sig_u_hat_product = (PuENPENInv * PENu)[0, 0]; // output = 1x1 UMatrix

                    for (int i = -rangeToApply; i <= rangeToApply; i++) // column
                    {
                        for (int j = -rangeToApply; j <= rangeToApply; j++) // row
                        {
                            UMatrix ENmr_EN = new UMatrix(2, 1);
                            double posX = 0; double posY = 0;
                            psig_u_hat_square.GetReals(laserPointIndexX + i, laserPointIndexY + j, out posX, out posY);
                            ENmr_EN[0, 0] = posX - r_hat_ENU[0, 0];
                            ENmr_EN[1, 0] = posY - r_hat_ENU[1, 0];
                            double u_hat_product = (PuENPENInv * (ENmr_EN))[0, 0]; // output = 1x1 UMatrix
                            uHatMatrix[j + rangeToApply, i + rangeToApply] = r_hat_ENU[2, 0] + u_hat_product;
                            sigUHatMatrix[j + rangeToApply, i + rangeToApply] = covRHatENU[2, 2] - sig_u_hat_product;
                        }
                    }
                    sw5.Stop();
                    sw6.Reset();
                    sw6.Start();

                    //-------------------------------------------//
                    // ASSIGN FINAL VALUES TO THE OCCUPANCY GRID //
                    //-------------------------------------------//
                    for (int i = -rangeToApply; i <= rangeToApply; i++)
                    {
                        for (int j = -rangeToApply; j <= rangeToApply; j++)
                        {
                            int indexXToUpdate = laserPointIndexX + i;
                            int indexYToUpdate = laserPointIndexY + j;
                            // if the cell to update is out of range, continue
                            if (!psig_u_hat_square.CheckValidIdx(indexXToUpdate, indexYToUpdate))
                            {
                                //Console.WriteLine("Laser points out of the occupancy grid map");
                                continue;
                            }

                            pij_sum.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                            pijCell[j + rangeToApply, i + rangeToApply] + pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate));
                            pu_hat.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                            pijCell[j + rangeToApply, i + rangeToApply] * uHatMatrix[j + rangeToApply, i + rangeToApply] + pu_hat.GetCellByIdx(indexXToUpdate, indexYToUpdate));
                            pu_hat_square.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                            pijCell[j + rangeToApply, i + rangeToApply] * uHatMatrix[j + rangeToApply, i + rangeToApply] * uHatMatrix[j + rangeToApply, i + rangeToApply] + pu_hat_square.GetCellByIdx(indexXToUpdate, indexYToUpdate));
                            psig_u_hat_square.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                            pijCell[j + rangeToApply, i + rangeToApply] * sigUHatMatrix[j + rangeToApply, i + rangeToApply] + psig_u_hat_square.GetCellByIdx(indexXToUpdate, indexYToUpdate));
                            uhatGM.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                                            (pu_hat.GetCellByIdx(indexXToUpdate, indexYToUpdate) / pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate)));
                            normalisedPijSum.SetCellByIdx(indexXToUpdate, indexYToUpdate, pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate) / laserHit.GetCellByIdx(indexXToUpdate, indexYToUpdate));
                            double largeU = (pu_hat.GetCellByIdx(indexXToUpdate, indexYToUpdate) / pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate));
                            double largeSig = (psig_u_hat_square.GetCellByIdx(indexXToUpdate, indexYToUpdate) + pu_hat_square.GetCellByIdx(indexXToUpdate, indexYToUpdate)) / pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate) - largeU * largeU;

                            uhatGM.SetCellByIdx(indexXToUpdate, indexYToUpdate, largeU);
                            sigSqrGM.SetCellByIdx(indexXToUpdate, indexYToUpdate, largeSig);

                            Index index = new Index(indexXToUpdate, indexYToUpdate);
                            if (!indicesDictionary.ContainsKey(index))
                                indicesDictionary.Add(index, indicesDictionary.Count);
                            /*
                            if (indicesDictionary.ContainsKey(index))
                            {
                                int indexOfIndices = indicesDictionary[index];
                                heightList[indexOfIndices] = (float)largeU;
                                covList[indexOfIndices] = (float)largeSig;
                                pijSumList[indexOfIndices] = (float)pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate);
                            }
                            else
                            {
                                indicesDictionary.Add(index, indicesDictionary.Count);
                                indicesList.Add(new Index(indexXToUpdate, indexYToUpdate));
                                heightList.Add((float)largeU);
                                covList.Add((float)largeSig);
                                pijSumList.Add((float)pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate));
                            }
                             */
                        }
                    }
                    sw6.Stop();

                } // end foreach

                //Console.WriteLine("1: " + sw1.ElapsedMilliseconds +
                //                                    " 2: " + sw2.ElapsedMilliseconds +
                //                                    " 3: " + sw3.ElapsedMilliseconds +
                //                                    " 4: " + sw4.ElapsedMilliseconds +
                //                                    " 5: " + sw5.ElapsedMilliseconds +
                //                                    " 6: " + sw6.ElapsedMilliseconds +
                //                                    " TOTAL: " + (sw1.ElapsedMilliseconds + sw2.ElapsedMilliseconds + sw3.ElapsedMilliseconds + sw4.ElapsedMilliseconds + sw5.ElapsedMilliseconds + sw6.ElapsedMilliseconds).ToString());
            } // end function
        }
Exemple #22
0
        /// <summary>Returns a sub matrix extracted from the current matrix.</summary>
        /// <param name="startRow">Start row index</param>
        /// <param name="endRow">End row index</param>
        /// <param name="startColumn">Start column index</param>
        /// <param name="endColumn">End column index</param>
        public UMatrix Submatrix(int startRow, int endRow, int startColumn, int endColumn)
        {
            if ((startRow > endRow) || (startColumn > endColumn) || (startRow < 0) || (startRow >= this.numRows) || (endRow < 0) || (endRow >= this.numRows) || (startColumn < 0) || (startColumn >= this.numCols) || (endColumn < 0) || (endColumn >= this.numCols))
            {
                throw new ArgumentException("Argument out of range.");
            }

            UMatrix X = new UMatrix(endRow - startRow + 1, endColumn - startColumn + 1);
            fixed (double* ptrX = X.data, ptrThis = data)
            {
                for (int i = startRow; i <= endRow; i++)
                {
                    for (int j = startColumn; j <= endColumn; j++)
                    {
                        //X.data[j + (X.numCols * i)] = data[j + (numCols * i)];
                        (*(ptrX + j + (X.numCols * i))) = (*(ptrThis + j + (numCols * i)));
                    }
                }
            }

            return X;
        }
Exemple #23
0
        public UMatrix Inverse()
        {
            UMatrix upper, lower, c, z;

            LuDecomposition(out lower, out upper);
            c = new UMatrix(numRows, numCols, 1);
            z = UMatrix.ForwardSub(lower, c);

            return UMatrix.BackwardSub(upper, z);
        }
Exemple #24
0
        /// <summary>Unary minus.</summary>
        public static UMatrix Negate(UMatrix value)
        {
            if (value == null)
            {
                throw new ArgumentNullException("value");
            }

            int rows = value.numRows;
            int columns = value.numCols;
            UMatrix X = new UMatrix(rows, columns);
            fixed (double* ptrX = X.data, ptrThis = value.data)
            {
                for (int i = 0; i < rows; i++)
                {
                    for (int j = 0; j < columns; j++)
                    {
                        //X.data[j + (columns * i)] = -value.data[j + (columns * i)];
                        (*(ptrX + j + (columns * i))) = -(*(ptrThis + j + (columns * i)));
                    }
                }
            }

            return X;
        }
        public GaussianMixMappingQ(IOccupancyGrid2D ocToUpdate, SensorPose laserRelativePositionToRover, int cellToUpdate)
        {
            //currentLaserPoseToRobot = laserRelativePositionToRover;
            this.rangeToApply = cellToUpdate;
            Matrix4 laser2RobotDCM = Matrix4.FromPose(laserRelativePositionToRover);
            laserToRobotDCM = new UMatrix(4, 4);
            robotToGlocalDCM = new UMatrix(4, 4);
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
                }
            }
            covRobotPose = new UMatrix(6, 6);
            covLaserPose = new UMatrix(6, 6);
            covLaserScan = new UMatrix(2, 2);

            pij_sum = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            pu_hat = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            pu_hat_square = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            psig_u_hat_square = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            uhatGM = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            sigSqrGM = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            laserHit = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            thresholdedHeightMap = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            resetCountMap = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            indexMap = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            this.numCellX = ocToUpdate.NumCellX;
            this.numCellY = ocToUpdate.NumCellY;
            #region Initial setup

            double[] Qp_robot = new double[36]{0.1, 0, 0, 0, 0, 0,
                                               0, 0.1, 0, 0, 0, 0,
                                               0, 0, 0.1, 0, 0, 0,
                                               0, 0, 0, 0.01, 0, 0,
                                               0, 0, 0, 0, 0.01, 0,
                                               0, 0, 0, 0, 0, 0.01};

            double[] Qp_laser = new double[36]{0.001, 0, 0, 0, 0, 0,
                                               0, 0.001, 0, 0, 0, 0,
                                               0, 0, 0.001, 0, 0, 0,
                                               0, 0, 0, 0.0001, 0, 0,
                                               0, 0, 0, 0, 0.0001, 0,
                                               0, 0, 0, 0, 0, 0.0001};

            double[] Qr = new double[4] { .01 * .01, 0, 0, (0.1 * Math.PI / 180.0) * (0.1 * Math.PI / 180.0) };

            // assign to our UMatrix
            for (int i = 0; i < 6; i++)
            {
                for (int j = 0; j < 6; j++)
                {
                    covRobotPose[j, i] = Qp_robot[j * 6 + i];// *1e-6;
                    covLaserPose[j, i] = Qp_laser[j * 6 + i];// *1e-6;
                }
            }
            covLaserScan[0, 0] = Qr[0];
            covLaserScan[0, 1] = 0;
            covLaserScan[1, 0] = 0;
            covLaserScan[1, 1] = Qr[3];
            #endregion

            int k = 0;

            sinLookupHokuyo = new double[682];
            cosLookupHokuyo = new double[682];
            for (double i = -120; i <= 120; i += 0.35190615835777126099706744868035)
            {
                sinLookupHokuyo[k] = (Math.Sin(i * Math.PI / 180.0));
                cosLookupHokuyo[k] = (Math.Cos(i * Math.PI / 180.0));
                k++;
            }
            laserHalfAngleHokuyo = 120;
            MAXRANGEHokuyo = 5.0;
            MINRANGEHokuyo = 0.5;

            k = 0;
            sinLookupSick = new double[361];
            cosLookupSick = new double[361];
            for (double i = -90; i <= 90; i += .5)
            {
                sinLookupSick[k] = (Math.Sin(i * Math.PI / 180.0));
                cosLookupSick[k] = (Math.Cos(i * Math.PI / 180.0));
                k++;
            }
            laserHalfAngleSick = 90;
            MAXRANGESick = 30.0;
            MINRANGESick = 0.5;

            hokuyoStartIdx = 100;
            hokuyoEndIdx = 500;

            indicesList = new List<Index>();
            heightList = new List<float>();
            covList = new List<float>();
            pijSumList = new List<float>();

            laser2RobotTransMatrixDictionary = new Dictionary<int, Dictionary<int, UMatrix>>();
            jacobianLaserPoseDictionary = new Dictionary<int, Dictionary<int, UMatrix>>();

            indicesDictionary = new Dictionary<Index, int>();
        }
        /// <summary>
        /// Update OccupancyGrid based on lidarScan and robotPose received
        /// </summary>
        /// <param name="lidarScan"></param>
        /// <param name="currentRobotPose"></param>
        public void UpdateOccupancyGrid(ILidarScan<ILidar2DPoint> lidarScan, int robotID, int scannerID, PoseFilterState currentRobotPose, SensorPose lidarPose, List<Polygon> dynamicObstacles)
        {
            if (robotID == 1)
            {
                MAXRANGESick = 7.0;
            }
            else if (robotID == 3)
            {
                MAXRANGESick = 30.0;
            }

            if (lidarPose == null)
            {
                lidarPose = new SensorPose(0, 0, 0.5, 0, 0 * Math.PI / 180.0, 0, 0);
            }
            if (laser2RobotTransMatrixDictionary.ContainsKey(robotID))
            {
                if (laser2RobotTransMatrixDictionary[robotID].ContainsKey(scannerID))
                {
                    JTpl = jacobianLaserPoseDictionary[robotID][scannerID];
                    laserToRobotDCM = laser2RobotTransMatrixDictionary[robotID][scannerID];
                }
                else
                {
                    Matrix4 laser2RobotDCM = Matrix4.FromPose(lidarPose);
                    for (int i = 0; i < 4; i++)
                    {
                        for (int j = 0; j < 4; j++)
                        {
                            laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
                        }
                    }
                    laser2RobotTransMatrixDictionary[robotID].Add(scannerID, laserToRobotDCM);
                    jacobianLaserPoseDictionary[robotID].Add(scannerID, ComputeJacobian(lidarPose.yaw, lidarPose.pitch, lidarPose.roll));
                    JTpl = jacobianLaserPoseDictionary[robotID][scannerID];
                }
            }
            else
            {
                laser2RobotTransMatrixDictionary.Add(robotID, new Dictionary<int, UMatrix>());
                jacobianLaserPoseDictionary.Add(robotID, new Dictionary<int, UMatrix>());
                Matrix4 laser2RobotDCM = Matrix4.FromPose(lidarPose);
                for (int i = 0; i < 4; i++)
                {
                    for (int j = 0; j < 4; j++)
                    {
                        laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
                    }
                }
                laser2RobotTransMatrixDictionary[robotID].Add(scannerID, new UMatrix(laserToRobotDCM));
                jacobianLaserPoseDictionary[robotID].Add(scannerID, ComputeJacobian(lidarPose.yaw, lidarPose.pitch, lidarPose.roll));
                JTpl = jacobianLaserPoseDictionary[robotID][scannerID];
            }

            // calculate robot2global transformation matrix
            if (currentRobotPose == null) return;
            Matrix4 robot2GlocalDCM = Matrix4.FromPose(currentRobotPose);
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    robotToGlocalDCM[i, j] = robot2GlocalDCM[i, j];
                }
            }

            if (lidarScan == null) return;

            UMatrix JTpr = ComputeJacobianQ(currentRobotPose.q1, currentRobotPose.q2, currentRobotPose.q3, currentRobotPose.q4);
            List<UMatrix> JfPrCubixLaserToRobotDCM = new List<UMatrix>(6);
            List<UMatrix> RobotToGlocalDCMJfPlCubix = new List<UMatrix>(7);
            for (int i = 0; i < 7; i++)
            {
                //derivative of the robot transform matrtix w.r.t. some element of the robot psoe
                UMatrix j = new UMatrix(4, 4);
                j[0, 0] = JTpr[0, i]; j[1, 0] = JTpr[1, i]; j[2, 0] = JTpr[2, i]; j[3, 0] = JTpr[3, i];
                j[0, 1] = JTpr[4, i]; j[1, 1] = JTpr[5, i]; j[2, 1] = JTpr[6, i]; j[3, 1] = JTpr[7, i];
                j[0, 2] = JTpr[8, i]; j[1, 2] = JTpr[9, i]; j[2, 2] = JTpr[10, i]; j[3, 2] = JTpr[11, i];
                j[0, 3] = JTpr[12, i]; j[1, 3] = JTpr[13, i]; j[2, 3] = JTpr[14, i]; j[3, 3] = JTpr[15, i];
                JfPrCubixLaserToRobotDCM.Add(j * laserToRobotDCM);

                if (i == 7) continue; // same as break
                UMatrix tempJacobianPl = new UMatrix(4, 4);
                tempJacobianPl[0, 0] = JTpl[0, i]; tempJacobianPl[1, 0] = JTpl[1, i]; tempJacobianPl[2, 0] = JTpl[2, i]; tempJacobianPl[3, 0] = JTpl[3, i];
                tempJacobianPl[0, 1] = JTpl[4, i]; tempJacobianPl[1, 1] = JTpl[5, i]; tempJacobianPl[2, 1] = JTpl[6, i]; tempJacobianPl[3, 1] = JTpl[7, i];
                tempJacobianPl[0, 2] = JTpl[8, i]; tempJacobianPl[1, 2] = JTpl[9, i]; tempJacobianPl[2, 2] = JTpl[10, i]; tempJacobianPl[3, 2] = JTpl[11, i];
                tempJacobianPl[0, 3] = JTpl[12, i]; tempJacobianPl[1, 3] = JTpl[13, i]; tempJacobianPl[2, 3] = JTpl[14, i]; tempJacobianPl[3, 3] = JTpl[15, i];
                RobotToGlocalDCMJfPlCubix.Add(robotToGlocalDCM * tempJacobianPl);
            }
            UMatrix laserToENU = robotToGlocalDCM * laserToRobotDCM;
            //UMatrix pijCell = new UMatrix(rangeToApply * 2 + 1, rangeToApply * 2 + 1);
            // update covariance
            UpdateCovarianceQ(currentRobotPose.Covariance);

            //SickPoint p = new SickPoint(new RThetaCoordinate(1.0f, 0.0f));
            for (int laserIndex = 0; laserIndex < lidarScan.Points.Count; laserIndex++)
            {
                lock (locker)
                {
                    ILidar2DPoint p = lidarScan.Points[laserIndex];
                    if (scannerID == 1)
                    {
                        if (p.RThetaPoint.R >= MAXRANGESick || p.RThetaPoint.R <= MINRANGESick)
                            continue;
                    }
                    else if (scannerID == 2)
                    {
                        if (p.RThetaPoint.R >= MAXRANGEHokuyo || p.RThetaPoint.R <= MINRANGEHokuyo || laserIndex < hokuyoStartIdx || laserIndex > hokuyoEndIdx)
                            continue;
                    }
                    bool hitDynamicObstacles = false;
                    // figure out if this lidar point is hitting other robot

                    // find laser points in 3D space
                    // first define 2D point on the laser plane
                    UMatrix laserPoint = new UMatrix(4, 1);

                    double deg = (p.RThetaPoint.theta * 180.0 / Math.PI);
                    int thetaDegIndex = 0;
                    if (scannerID == 2) // hokuyo
                        thetaDegIndex = (int)Math.Round((deg + laserHalfAngleHokuyo) * 2.84);// % 360;
                    else if (scannerID == 1) // sick
                        thetaDegIndex = (int)Math.Round((deg + laserHalfAngleSick) * 2) % 360;

                    double cosTheta = 0, sinTheta = 0;

                    if (scannerID == 1)
                    {
                        cosTheta = cosLookupSick[thetaDegIndex];
                        sinTheta = sinLookupSick[thetaDegIndex];
                    }
                    else if (scannerID == 2)
                    {
                        cosTheta = cosLookupHokuyo[thetaDegIndex];
                        sinTheta = sinLookupHokuyo[thetaDegIndex];
                    }

                    // initial laser points
                    laserPoint[0, 0] = p.RThetaPoint.R * cosTheta;
                    laserPoint[1, 0] = p.RThetaPoint.R * sinTheta;
                    laserPoint[2, 0] = 0;
                    laserPoint[3, 0] = 1;

                    //calculate r_hat_ENU
                    UMatrix r_hat_ENU = laserToENU * laserPoint;

                    foreach (Polygon dp in dynamicObstacles)
                    {
                        if (dp.IsInside(new Vector2(r_hat_ENU[0, 0], r_hat_ENU[1, 0])))
                        {
                            hitDynamicObstacles = true;
                            break;
                        }
                    }
                    if (hitDynamicObstacles) continue;

                    //-------------------------------//
                    // COVARIANCE UMatrix CALCULATION //
                    //-------------------------------//
                    UMatrix JRr = new UMatrix(4, 2);
                    JRr.Zero();
                    JRr[0, 0] = cosTheta;
                    JRr[0, 1] = -p.RThetaPoint.R * sinTheta;
                    JRr[1, 0] = sinTheta;
                    JRr[1, 1] = p.RThetaPoint.R * cosTheta;

                    UMatrix Jfr = new UMatrix(3, 2); // 3x2
                    Jfr = (laserToENU * JRr).Submatrix(0, 2, 0, 1);	 // 4x4 * 4x4 * 4x2

                    UMatrix Jfpr = new UMatrix(3, 7);
                    UMatrix Jfpl = new UMatrix(3, 6);

                    for (int i = 0; i < 7; i++) //for each state var (i.e. x,y,z,y,p,r)
                    {
                        UMatrix JfprTemp = (JfPrCubixLaserToRobotDCM[i]) * laserPoint; // 4 by 1 UMatrix
                        Jfpr[0, i] = JfprTemp[0, 0];
                        Jfpr[1, i] = JfprTemp[1, 0];
                        Jfpr[2, i] = JfprTemp[2, 0];

                        //UMatrix JfplTemp = (RobotToGlocalDCMJfPlCubix[i]) * laserPoint; // 4 by 1 UMatrix
                        //Jfpl[0, i] = JfplTemp[0, 0];
                        //Jfpl[1, i] = JfplTemp[1, 0];
                        //Jfpl[2, i] = JfplTemp[2, 0];
                    }
                    UMatrix JfprQprJfprT = new UMatrix(3, 3);
                    UMatrix JfplQplJfplT = new UMatrix(3, 3);
                    UMatrix JfrQrJfrT = new UMatrix(3, 3);
                    JfprQprJfprT = (Jfpr * covRobotPoseQ) * Jfpr.Transpose();
                    //JfplQplJfplT = (Jfpl * covLaserPose) * Jfpl.Transpose(); // not doing because covariance of laser point is so small
                    JfrQrJfrT = (Jfr * covLaserScan) * Jfr.Transpose();

                    // add above variables together and get the covariance
                    UMatrix covRHatENU = JfprQprJfprT + /*JfplQplJfplT +*/ JfrQrJfrT; // 3x3 UMatrix
                    //-----------------------------//
                    // FIND WHICH CELLS TO COMPUTE //
                    //-----------------------------//

                    // find out cells around this laser point
                    int laserPointIndexX = 0;
                    int laserPointIndexY = 0;
                    //this is used just to do the transformation from reaal to grid and visa versa
                    psig_u_hat_square.GetIndicies(r_hat_ENU[0, 0], r_hat_ENU[1, 0], out laserPointIndexX, out laserPointIndexY); // get cell (r_hat_ENU_X, r_hat_ENU_y)
                    if ((laserPointIndexX < 0 || laserPointIndexX >= numCellX) || (laserPointIndexY < 0 || laserPointIndexY >= numCellY))
                        continue;

                    int rangeToApplyX = (int)Math.Round(Math.Sqrt(covRHatENU[0, 0]) / (pij_sum.ResolutionX * 2)) * 2;
                    int rangeToApplyY = (int)Math.Round(Math.Sqrt(covRHatENU[1, 1]) / (pij_sum.ResolutionY * 2)) * 2;
                    //-----------------------------------------//
                    // COMPUTE THE DISTRIBUTION OF UNCERTAINTY //
                    //-----------------------------------------//
                    UMatrix pijCell = new UMatrix(rangeToApplyY * 2 + 1, rangeToApplyX * 2 + 1);

                    double sigX = Math.Sqrt(covRHatENU[0, 0]);
                    double sigY = Math.Sqrt(covRHatENU[1, 1]);
                    double rho = covRHatENU[1, 0] / (sigX * sigY);
                    double logTerm = Math.Log(2 * Math.PI * sigX * sigY * Math.Sqrt(1 - (rho * rho)));
                    double xTermDenom = (1 - (rho * rho));
                    //for (int i = -rangeToApply; i <= rangeToApply; i++) // row
                    for (int i = -rangeToApplyX; i <= rangeToApplyX; i++) // row
                    {
                        //for (int j = -rangeToApplyX; j <= rangeToApplyX; j++) // column
                        for (int j = -rangeToApplyY; j <= rangeToApplyY; j++) // column
                        {
                            if (laserPointIndexX + i < 0 || laserPointIndexX + i >= numCellX || laserPointIndexY + j < 0 || laserPointIndexY + j >= numCellY) continue;
                            // estimate using Bivariate Normal Distribution
                            double posX = 0; double posY = 0;
                            psig_u_hat_square.GetReals(laserPointIndexX + i, laserPointIndexY + j, out posX, out posY);
                            posX += psig_u_hat_square.ResolutionX / 2;
                            posY += psig_u_hat_square.ResolutionY / 2;
                            double x = posX - r_hat_ENU[0, 0];
                            double y = posY - r_hat_ENU[1, 0];
                            double z = (x * x) / (sigX * sigX) -
                                                (2 * rho * x * y / (sigX * sigY)) +
                                                 (y * y) / (sigY * sigY);
                            double xTerm = -0.5 * z / xTermDenom;
                            // chi2 test
                            //if ((2 * (1 - (rho * rho))) * ((x * x) / (sigX * sigX) + (y * y) / (sigY * sigY) - (2 * rho * x * y) / (sigX * sigY)) > 15.2)
                            //  continue;
                            pijCell[j + rangeToApplyY, i + rangeToApplyX] = Math.Exp(xTerm - logTerm) * psig_u_hat_square.ResolutionX * psig_u_hat_square.ResolutionY;
                            laserHit.SetCellByIdx(laserPointIndexX + i, laserPointIndexY + j, laserHit.GetCellByIdxUnsafe(laserPointIndexX + i, laserPointIndexY + j) + 1);
                        }
                    }

                    //---------------------------//
                    // COMPUTE HEIGHT ESTIMATION //
                    //---------------------------//
                    UMatrix PEN = covRHatENU.Submatrix(0, 1, 0, 1);

                    UMatrix PENInv = PEN.Inverse2x2;

                    UMatrix PuEN = new UMatrix(1, 2);
                    UMatrix PENu = new UMatrix(2, 1);
                    UMatrix PuENPENInv = PuEN * PENInv;
                    UMatrix uHatMatrix = new UMatrix(rangeToApplyY * 2 + 1, rangeToApplyX * 2 + 1);
                    UMatrix sigUHatMatrix = new UMatrix(rangeToApplyY * 2 + 1, rangeToApplyX * 2 + 1);

                    PuEN[0, 0] = covRHatENU[2, 0];
                    PuEN[0, 1] = covRHatENU[2, 1];

                    PENu[0, 0] = covRHatENU[0, 2];
                    PENu[1, 0] = covRHatENU[1, 2];

                    double sig_u_hat_product = (PuENPENInv * PENu)[0, 0]; // output = 1x1 UMatrix

                    for (int i = -rangeToApplyX; i <= rangeToApplyX; i++) // row
                    {
                        for (int j = -rangeToApplyY; j <= rangeToApplyY; j++) // column
                        {
                            UMatrix ENmr_EN = new UMatrix(2, 1);
                            double posX = 0; double posY = 0;
                            psig_u_hat_square.GetReals(laserPointIndexX + i, laserPointIndexY + j, out posX, out posY);
                            ENmr_EN[0, 0] = posX - r_hat_ENU[0, 0];
                            ENmr_EN[1, 0] = posY - r_hat_ENU[1, 0];
                            double u_hat_product = (PuENPENInv * (ENmr_EN))[0, 0]; // output = 1x1 UMatrix
                            uHatMatrix[j + rangeToApplyY, i + rangeToApplyX] = r_hat_ENU[2, 0] + u_hat_product;
                            sigUHatMatrix[j + rangeToApplyY, i + rangeToApplyX] = covRHatENU[2, 2] - sig_u_hat_product;
                        }
                    }

                    //-------------------------------------------//
                    // ASSIGN FINAL VALUES TO THE OCCUPANCY GRID //
                    //-------------------------------------------//
                    for (int i = -rangeToApplyX; i <= rangeToApplyX; i++)
                    {
                        for (int j = -rangeToApplyY; j <= rangeToApplyY; j++)
                        {
                            int indexXToUpdate = laserPointIndexX + i;
                            int indexYToUpdate = laserPointIndexY + j;
                            // if the cell to update is out of range, continue
                            if (!psig_u_hat_square.CheckValidIdx(indexXToUpdate, indexYToUpdate))
                                continue;

                            pij_sum.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                            pijCell[j + rangeToApplyY, i + rangeToApplyX] + pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
                            pu_hat.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                            pijCell[j + rangeToApplyY, i + rangeToApplyX] * uHatMatrix[j + rangeToApplyY, i + rangeToApplyX] + pu_hat.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
                            pu_hat_square.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                            pijCell[j + rangeToApplyY, i + rangeToApplyX] * uHatMatrix[j + rangeToApplyY, i + rangeToApplyX] * uHatMatrix[j + rangeToApply, i + rangeToApply] + pu_hat_square.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
                            psig_u_hat_square.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                            pijCell[j + rangeToApplyY, i + rangeToApplyX] * sigUHatMatrix[j + rangeToApplyY, i + rangeToApplyX] + psig_u_hat_square.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
                            uhatGM.SetCellByIdx(indexXToUpdate, indexYToUpdate,
                                                            (pu_hat.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) / pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate)));

                            double largeU = (pu_hat.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) / pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
                            double largeSig = (psig_u_hat_square.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) + pu_hat_square.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate)) / pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) - largeU * largeU;
                            if (pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) > 1)
                                thresholdedHeightMap.SetCellByIdx(indexXToUpdate, indexYToUpdate, largeU);//pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) / laserHit.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));

                            uhatGM.SetCellByIdx(indexXToUpdate, indexYToUpdate, largeU);
                            //sigSqrGM.SetCellByIdx(indexXToUpdate, indexYToUpdate, largeU + 2 * Math.Sqrt(largeSig));

                            if (indexMap.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) != 1.0)
                            {
                                Index index = new Index(indexXToUpdate, indexYToUpdate);
                                indicesDictionary.Add(index, indicesDictionary.Count);
                                indexMap.SetCellByIdx(indexXToUpdate, indexYToUpdate, 1.0);
                            }
                        }
                    }

                } // end foreach

                //Console.WriteLine("1: " + sw1.ElapsedMilliseconds +
                //                                    " 2: " + sw2.ElapsedMilliseconds +
                //                                    " 3: " + sw3.ElapsedMilliseconds +
                //                                    " 4: " + sw4.ElapsedMilliseconds +
                //                                    " 5: " + sw5.ElapsedMilliseconds +
                //                                    " 6: " + sw6.ElapsedMilliseconds +
                //                                    " TOTAL: " + (sw1.ElapsedMilliseconds + sw2.ElapsedMilliseconds + sw3.ElapsedMilliseconds + sw4.ElapsedMilliseconds + sw5.ElapsedMilliseconds + sw6.ElapsedMilliseconds).ToString());
            } // end function
        }
Exemple #27
0
        /// <summary>Matrix subtraction.</summary>
        public static UMatrix Subtract(UMatrix left, UMatrix right)
        {
            int rows = left.numRows;
            int columns = left.numCols;

            if ((rows != right.numRows) || (columns != right.numCols))
            {
                throw new ArgumentException("Matrix dimension do not match.");
            }

            UMatrix X = new UMatrix(rows, columns);
            fixed (double* ptrX = X.data, ptrL = left.data, ptrR = right.data)
            {
                for (int i = 0; i < rows; i++)
                {
                    for (int j = 0; j < columns; j++)
                    {
                        //X.data[j + (columns * i)] = left.data[j + (columns * i)] - right.data[j + (columns * i)];
                        (*(ptrX + j + (columns * i))) = (*(ptrL + j + (columns * i))) - (*(ptrR + j + (columns * i)));
                    }
                }
            }
            return X;
        }
 private void UpdateCovarianceQ(Matrix state)
 {
     UMatrix abc = new UMatrix(7, 7);
     for (int i = 0; i < 7; i++)
     {
         for (int j = 0; j < 7; j++)
             abc[i, j] = state[i, j];
     }
     covRobotPoseQ = abc;
 }
Exemple #29
0
        //----------------------------------------------------------------------------
        public UMatrix LuDecomposition(out UMatrix l, out UMatrix u)
        {
            double[] lu = new double[data.Length];
            Array.Copy(data, lu, data.Length);

            //double[] luRowI;
            double* luRowIPtr;
            double[] luColJ = new double[numRows];
            double[] lData = new double[numRows * numRows];
            double[] uData = new double[numRows * numRows];

            fixed (double* luPtr = lu, luColJPtr = luColJ, lDataPtr = lData, uDataPtr = uData)
            {
                int pivotSign = 1;
                int[] pivotVector = new int[numRows];
                for (int i = 0; i < numRows; i++)
                {
                    pivotVector[i] = i;
                }

                // Outer loop.
                for (int j = 0; j < numCols; j++)
                {
                    // Make a copy of the j-th column to localize references.
                    for (int i = 0; i < numRows; i++)
                    {
                        *(luColJPtr + i) = *(luPtr + j + i * numCols);
                    }

                    // Apply previous transformations.
                    for (int i = 0; i < numRows; i++)
                    {
                        luRowIPtr = luPtr + i * numCols;

                        // Most of the time is spent in the following dot product.
                        //int kmax = (i > j) ? j : i;
                        int kmax = Math.Min(i, j);
                        double s = 0.0;
                        for (int k = 0; k < kmax; k++)
                        {
                            s += *(luRowIPtr + k) * *(luColJPtr + k);
                        }
                        *(luRowIPtr + j) = *(luColJPtr + i) -= s;
                    }

                    // Find pivot and exchange if necessary.
                    int p = j;
                    for (int i = j + 1; i < numRows; i++)
                    {
                        if (Math.Abs(*(luColJPtr + i)) > Math.Abs(*(luColJPtr + p)))
                        {
                            p = i;
                        }
                    }

                    if (p != j)
                    {
                        for (int k = 0; k < numCols; k++)
                        {
                            double t = *(luPtr + k + p * numCols);
                            *(luPtr + k + p * numCols) = *(luPtr + k + j * numCols);
                            *(luPtr + k + j * numCols) = t;
                        }

                        int v = pivotVector[p];
                        pivotVector[p] = pivotVector[j];
                        pivotVector[j] = v;

                        pivotSign = -pivotSign;
                    }

                    // Compute multipliers.
                    if (j < numRows & *(luPtr + j + j * numCols) != 0.0)
                    {
                        for (int i = j + 1; i < numRows; i++)
                        {
                            *(luPtr + j + i * numCols) /= *(luPtr + j + j * numCols);
                        }
                    }
                }

                for (int i = 0; i < numRows; i++)
                {
                    for (int j = 0; j < numCols; j++)
                    {
                        *(lDataPtr + j + i * numCols) = (j == i) ? 1 : (i < j) ? 0 : *(luPtr + j + i * numCols);
                        *(uDataPtr + j + i * numCols) = (i > j) ? 0 : *(luPtr + j + i * numCols);
                    }
                }

                l = new UMatrix(numRows, numCols, lData);
                u = new UMatrix(numRows, numCols, uData);
            }

            return new UMatrix(numRows, numCols, lu);
        }
        public GaussianMixMapping(IOccupancyGrid2D ocToUpdate, SensorPose laserRelativePositionToRover)
        {
            //currentLaserPoseToRobot = laserRelativePositionToRover;
            Matrix4 laser2RobotDCM = Matrix4.FromPose(laserRelativePositionToRover);
            laserToRobotDCM = new UMatrix(4, 4);
            robotToGlocalDCM = new UMatrix(4, 4);
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
                }
            }
            covRobotPose = new UMatrix(6, 6);
            covLaserPose = new UMatrix(6, 6);
            covLaserScan = new UMatrix(2, 2);

            pij_sum = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            pu_hat = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            pu_hat_square = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            psig_u_hat_square = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            uhatGM = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            sigSqrGM = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            laserHit = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            normalisedPijSum = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
            #region Initial setup
            // initial setup
            // these matrices were obtained from Jon using Vicon with Pioneer and HOKUYO laser
            // these matrices need to be redefined
            // these matrices are in row-wise

            //double[] Qp_robot = new double[36]{0.0026,    0.0011,   -0.0008,   -0.0019,    0.0125,   -0.0047,
            //                      0.0011,    0.0082,   -0.0054,    0.0098,    0.0116,   -0.0330,
            //                     -0.0008,   -0.0054,    0.0132,   -0.0173,   -0.0154,    0.0115,
            //                     -0.0019,    0.0098,   -0.0173,    0.0542,    0.0076,   -0.0319,
            //                      0.0125,    0.0116,   -0.0154,    0.0076,    0.0812,   -0.0397,
            //                     -0.0047,   -0.0330,    0.0115,   -0.0319,   -0.0397,    0.1875};

            //double[] Qp_laser = new double[36]{0.0077, -0.0009, -0.0016, -0.0127, -0.0415, -0.0011,
            //                       -0.0009, 0.0051, -0.0013, -0.0035, 0.0045, 0.0197,
            //                       -0.0016, -0.0013, 0.0101, -0.0167, 0.0189, 0.0322,
            //                       -0.0127, -0.0035, -0.0167, 0.2669, -0.0548, -0.2940,
            //                       -0.0415, 0.0045, 0.0189, -0.0548, 0.8129, 0.3627,
            //                       -0.0011, 0.0197, 0.0322, -0.2940, 0.3627, 0.7474};

            double[] Qp_robot = new double[36]{0.1, 0, 0, 0, 0, 0,
                                                                                 0, 0.1, 0, 0, 0, 0,
                                                                                 0, 0, 0.1, 0, 0, 0,
                                                                                 0, 0, 0, 0.01, 0, 0,
                                                                                 0, 0, 0, 0, 0.01, 0,
                                                                                 0, 0, 0, 0, 0, 0.01};

            double[] Qp_laser = new double[36]{0.001, 0, 0, 0, 0, 0,
                                                                                 0, 0.001, 0, 0, 0, 0,
                                                                                 0, 0, 0.001, 0, 0, 0,
                                                                                 0, 0, 0, 0.0001, 0, 0,
                                                                                 0, 0, 0, 0, 0.0001, 0,
                                                                                 0, 0, 0, 0, 0, 0.0001};

            double[] Qr = new double[4] { .01 * .01, 0, 0, (0.1 * Math.PI / 180.0) * (0.1 * Math.PI / 180.0) };

            // assign to our UMatrix
            for (int i = 0; i < 6; i++)
            {
                for (int j = 0; j < 6; j++)
                {
                    covRobotPose[j, i] = Qp_robot[j * 6 + i];// *1e-6;
                    covLaserPose[j, i] = Qp_laser[j * 6 + i];// *1e-6;
                }
            }
            covLaserScan[0, 0] = Qr[0];
            covLaserScan[0, 1] = 0;
            covLaserScan[1, 0] = 0;
            covLaserScan[1, 1] = Qr[3];
            #endregion

            //JTpl = ComputeJacobian(currentLaserPoseToRobot.yaw, currentLaserPoseToRobot.pitch, currentLaserPoseToRobot.roll);

            //	int thetaDegIndex = (int)((p.RThetaPoint.theta * 180.0/Math.PI) * 2) + 180;
            int k = 0;
            for (double i = -90; i <= 90; i += .5)
            {
                sinLookup[k] = (Math.Sin(i * Math.PI / 180.0));
                cosLookup[k] = (Math.Cos(i * Math.PI / 180.0));
                k++;
            }

            // initialize arrays
            //pijToSend = new float[361 * rangeLong * rangeLong];
            //largeUToSend = new float[361 * rangeLong * rangeLong];
            //largeSigToSend = new float[361 * rangeLong * rangeLong];
            //colIdxChange = new int[361 * rangeLong * rangeLong];
            //rowIdxChange = new int[361 * rangeLong * rangeLong];

            indicesList = new List<Index>();
            heightList = new List<float>();
            covList = new List<float>();
            pijSumList = new List<float>();

            laser2RobotTransMatrixDictionary = new Dictionary<int, UMatrix>();
            jacobianLaserPoseDictionary = new Dictionary<int, UMatrix>();

            indicesDictionary = new Dictionary<Index, int>();
        }
Exemple #31
0
        /// <summary>Returns the transposed matrix.</summary>
        public UMatrix Transpose()
        {
            UMatrix X = new UMatrix(numCols, numRows);
            fixed (double* ptrX = X.data, ptrThis = data)
            {
                for (int i = 0; i < numRows; i++)
                {
                    for (int j = 0; j < numCols; j++)
                    {
                        //X.data[i + (numRows * j)] = data[j + (numCols * i)];
                        (*(ptrX + i + (numRows * j))) = (*(ptrThis + j + (numCols * i)));
                    }
                }
            }

            return X;
        }
        public UMatrix ComputeJacobian(double yaw, double pitch, double roll)
        {
            UMatrix m = new UMatrix(16, 6);
            m.Zero();

            double cp = Math.Cos(pitch);
            double sp = Math.Sin(pitch);
            double cy = Math.Cos(yaw);
            double sy = Math.Sin(yaw);
            double cr = Math.Cos(roll);
            double sr = Math.Sin(roll);

            m[0, 3] = -cp * sy;
            m[0, 4] = -sp * cy;
            m[1, 3] = -sp * sr * sy - cy * cr;
            m[1, 4] = cp * sr * cy;
            m[1, 5] = sp * cr * cy + sy * sr;

            m[2, 3] = -sp * cr * sy + cy * sr;
            m[2, 4] = cp * cr * cy;
            m[2, 5] = -sp * sr * cy + sy * cr;

            m[4, 3] = cp * cy;
            m[4, 4] = -sp * sy;
            m[5, 3] = sp * sr * cy - sy * cr;
            m[5, 4] = cp * sr * sy;
            m[5, 5] = sp * cr * sy - cy * sr;
            m[6, 3] = sp * cr * cy + sy * sr;
            m[6, 4] = cp * cr * sy;
            m[6, 5] = -sp * sr * sy - cy * cr;

            m[8, 4] = -cp;
            m[9, 4] = -sp * sr;
            m[9, 5] = cp * cr;
            m[10, 4] = -sp * cr;
            m[10, 5] = -cp * sr;

            m[12, 0] = 1;
            m[13, 1] = 1;
            m[14, 2] = 1;

            return m;
        }