C# DenseMatrix.Multiply方法代码示例

        //def SSF.tf2ssd(num,denum,k,ts)# public static SSFilter TF2SSd(double[] Num, double[] Den, double k, double Ts)
        //    n,num,denum = SSF::align_num_denum(num,denum)
        //    a = []
        //    b = []
        //    c = []
        //    (0).upto(n-3) {
        //        |i|
        //        ta = NArray.float(n-1)
        //        ta[i+1] = 1.0
        //        a << ta.to_a
        //        b << [0.0]
        //    }
        //    ta = []
        //    (n-1).downto(1) {
        //        |i|
        //        ta << -denum[i]/denum[0]
        //    }
        //    a << ta
        //    b << [1.0/denum[0]]
        //    d = num[0]/denum[0]
        //    c = NMatrix.float(n-1,1)
        //    (n-1).downto(1) {
        //        |i|
        //        c[i-1,0] = d*(-denum[i]) + num[i]
        //    }
        //    am = NMatrix.rows(a)    #Matrix.rows(a)
        //    bm = NMatrix.rows(b)    #Matrix.rows(b)
        //    e =  NMatrix.float(n-1,n-1).unit       #Matrix.xdiag(n-1,1.0)
        //    f = e + am*ts + am**2*ts**2/2 + am**3*ts**3/3
        //    r = (e*ts + am*ts**2/2 + am**2*ts**3/3)*bm #e*ts*bm
        //    ssf = SSF.new(f,r,c,d,ts,k*denum.last/num.last)
        //    ssf
        //end
        public static SSF C2DSS(Vector<double> num, Vector<double> denom, double gain, double timeSample)
        {
            int n = AlignNumDenom(ref num, ref denom);
            var a = new DenseMatrix(n - 1);
            var b = new DenseMatrix(n - 1, 1, 0.0);
            for (int i = 0; i < n-2; i++)
            {
                a[i, i + 1] = 1.0;
            }
            b[n - 2, 0] = 1/denom[0];
            var d = num[0]/denom[0];
            for (int i = 0; i < n-1; i++)
            {
                a[n - 2, i] = -denom[n - 1 - i]/denom[0];
            }
            var c = new DenseMatrix(1, n - 1);
            for (int i = 0; i < n-1; i++)
            {
                c[0, i] = num[n - 1 - i] - denom[n - 1 - i] * d;
            }

            var e = new DenseMatrix(a.RowCount);
            for (int i = 0; i < a.RowCount; i++)
            {
                e[i,i] = 1.0;
            }

            var f = e + a.Multiply(timeSample) +
                        a.Multiply(a).Multiply(timeSample * timeSample * 0.5);// +
                        //a.Multiply(a).Multiply(a).Multiply(timeSample*timeSample*timeSample/3.0);
            var r = (e * timeSample + a.Multiply(timeSample * timeSample * 0.5)). // + a.Multiply(a).Multiply(Math.Pow(timeSample, 3)/3.0) ).
                        Multiply(b);
            return new SSF((DenseMatrix)f, (DenseMatrix)r, (DenseMatrix)c, d, timeSample, gain * denom[n - 1] / num[n - 1]);
        }

        /// <summary>
        /// Run example
        /// </summary>
        /// <seealso cref="http://en.wikipedia.org/wiki/Matrix_multiplication#Scalar_multiplication">Multiply matrix by scalar</seealso>
        /// <seealso cref="http://reference.wolfram.com/mathematica/tutorial/MultiplyingVectorsAndMatrices.html">Multiply matrix by vector</seealso>
        /// <seealso cref="http://en.wikipedia.org/wiki/Matrix_multiplication#Matrix_product">Multiply matrix by matrix</seealso>
        /// <seealso cref="http://en.wikipedia.org/wiki/Matrix_multiplication#Hadamard_product">Pointwise multiplies matrix with another matrix</seealso>
        /// <seealso cref="http://en.wikipedia.org/wiki/Matrix_%28mathematics%29#Basic_operations">Addition and subtraction</seealso>
        public void Run()
        {
            // Initialize IFormatProvider to print matrix/vector data
            var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
            formatProvider.TextInfo.ListSeparator = " ";

            // Create matrix "A"
            var matrixA = new DenseMatrix(new[,] { { 1.0, 2.0, 3.0 }, { 4.0, 5.0, 6.0 }, { 7.0, 8.0, 9.0 } });
            Console.WriteLine(@"Matrix A");
            Console.WriteLine(matrixA.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // Create matrix "B"
            var matrixB = new DenseMatrix(new[,] { { 1.0, 3.0, 5.0 }, { 2.0, 4.0, 6.0 }, { 3.0, 5.0, 7.0 } });
            Console.WriteLine(@"Matrix B");
            Console.WriteLine(matrixB.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // Multiply matrix by scalar
            // 1. Using operator "*"
            var resultM = 3.0 * matrixA;
            Console.WriteLine(@"Multiply matrix by scalar using operator *. (result = 3.0 * A)");
            Console.WriteLine(resultM.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 2. Using Multiply method and getting result into different matrix instance
            resultM = (DenseMatrix)matrixA.Multiply(3.0);
            Console.WriteLine(@"Multiply matrix by scalar using method Multiply. (result = A.Multiply(3.0))");
            Console.WriteLine(resultM.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 3. Using Multiply method and updating matrix itself
            matrixA.Multiply(3.0, matrixA);
            Console.WriteLine(@"Multiply matrix by scalar using method Multiply. (A.Multiply(3.0, A))");
            Console.WriteLine(matrixA.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // Multiply matrix by vector (right-multiply)
            var vector = new DenseVector(new[] { 1.0, 2.0, 3.0 });
            Console.WriteLine(@"Vector");
            Console.WriteLine(vector.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 1. Using operator "*"
            var resultV = matrixA * vector;
            Console.WriteLine(@"Multiply matrix by vector using operator *. (result = A * vec)");
            Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 2. Using Multiply method and getting result into different vector instance
            resultV = (DenseVector)matrixA.Multiply(vector);
            Console.WriteLine(@"Multiply matrix by vector using method Multiply. (result = A.Multiply(vec))");
            Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 3. Using Multiply method and updating vector itself
            matrixA.Multiply(vector, vector);
            Console.WriteLine(@"Multiply matrix by vector using method Multiply. (A.Multiply(vec, vec))");
            Console.WriteLine(vector.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // Multiply vector by matrix (left-multiply)
            // 1. Using operator "*"
            resultV = vector * matrixA;
            Console.WriteLine(@"Multiply vector by matrix using operator *. (result = vec * A)");
            Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 2. Using LeftMultiply method and getting result into different vector instance
            resultV = (DenseVector)matrixA.LeftMultiply(vector);
            Console.WriteLine(@"Multiply vector by matrix using method LeftMultiply. (result = A.LeftMultiply(vec))");
            Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 3. Using LeftMultiply method and updating vector itself
            matrixA.LeftMultiply(vector, vector);
            Console.WriteLine(@"Multiply vector by matrix using method LeftMultiply. (A.LeftMultiply(vec, vec))");
            Console.WriteLine(vector.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // Multiply matrix by matrix
            // 1. Using operator "*"
            resultM = matrixA * matrixB;
            Console.WriteLine(@"Multiply matrix by matrix using operator *. (result = A * B)");
            Console.WriteLine(resultM.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 2. Using Multiply method and getting result into different matrix instance
            resultM = (DenseMatrix)matrixA.Multiply(matrixB);
            Console.WriteLine(@"Multiply matrix by matrix using method Multiply. (result = A.Multiply(B))");
            Console.WriteLine(resultM.ToString("#0.00\t", formatProvider));
            Console.WriteLine();
//.........这里部分代码省略.........

            /// <summary>
            /// SkeletonFrameReady gets fired every skeleton frame update, and refreshes the LocatedSensor's
            ///  global and relative skeleton maps
            /// </summary>
            /// <param name="sender"></param>
            /// <param name="e"></param>
            private void refreshSkeletonPositions(object sender, SkeletonFrameReadyEventArgs e)
            {
                using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame()) {
                    if (skeletonFrame != null) {
                        // First, get the relative skeletons - easy peasy
                        Skeleton[] skeletonsR = new Skeleton[skeletonFrame.SkeletonArrayLength];
                        skeletonFrame.CopySkeletonDataTo(skeletonsR);
                        this.relativeSkeletons = skeletonsR.ToList<Skeleton>();

                        // Now global skeletons...
                        // First, clear our global skeletons list.
                        //  We'll be building this back up from scratch here
                        this.globalSkeletons.Clear();
                        // Next, iterate through all the skeletons, applying a rotation and translation
                        //  to get us into global coordinates
                        foreach (Skeleton skel in this.relativeSkeletons) {
                            // Add a temporary skeleton object to store transformed
                            //  data into
                            Skeleton tempSkel = skel;

                            foreach (Joint j in skel.Joints){
                                // Make a new joint, then put it into our temporary joint
                                //  collection
                                JointType type = j.JointType;
                                Joint tempJoint = tempSkel.Joints[type];
                                // Copy the current joint state
                                JointTrackingState tracking = j.TrackingState;
                                tempJoint.TrackingState = tracking;

                                // However, we transform the position of the joint at least
                                SkeletonPoint shiftedPoint = new SkeletonPoint();
                                // Rotate the points
                                DenseMatrix point = new DenseMatrix(1, 3);
                                point[0, 0] = j.Position.X;
                                point[0, 1] = j.Position.Y;
                                point[0, 2] = j.Position.Z;
                                var rotatedPoint = point.Multiply(this.rotationMatrix);

                                // Then shift them by the global coordinates.
                                shiftedPoint.X = (float)rotatedPoint[0, 0] + this.xOffset;
                                shiftedPoint.Y = (float)rotatedPoint[0, 1] + this.yOffset;
                                shiftedPoint.Z = (float)rotatedPoint[0, 2] + this.zOffset;
                                tempJoint.Position = shiftedPoint;

                                tempSkel.Joints[type] = tempJoint;
                            }
                            // Next, alter the higher-level parameters of our skeleton
                            SkeletonPoint shiftedPosition = new SkeletonPoint();
                            // Rotate
                            DenseMatrix p = new DenseMatrix(1, 3);
                            p[0, 0] = tempSkel.Position.X;
                            p[0, 1] = tempSkel.Position.Y;
                            p[0, 2] = tempSkel.Position.Z;
                            var rPoint = p.Multiply(this.rotationMatrix);

                            // Then shift them by the global coordinates.
                            shiftedPosition.X = (float)rPoint[0, 0] + this.xOffset;
                            shiftedPosition.Y = (float)rPoint[0, 1] + this.yOffset;
                            shiftedPosition.Z = (float)rPoint[0, 2] + this.zOffset;

                            tempSkel.Position = shiftedPosition;

                            // Now add that skeleton to our global skeleton list
                            this.globalSkeletons.Add(tempSkel);
                        }

                    }
                }
            }

//.........这里部分代码省略.........

            //First column of U
            U = new DenseMatrix(Rjk.RowCount, 1);
            U.SetColumn(0, Rjk.Column(0));

            // Norm of (approximate) Z, to test error
            double d1 = U.L2Norm();
            double d2 = V.L2Norm();
            double normZ = d1 * d1 * d2 * d2;

            //Find 2nd row index I(2)
            int row = 0;
            max = -1.0;

            foreach (int ind in i)
            {
                if (Math.Abs(Rjk[ind, 0]) > max)
                {
                    max = Math.Abs(Rjk[ind, 0]);
                    row = ind;
                }
            }

            //I[1] = i[row];
            I[1] = row;
            i.Remove(I[1]);

            //Iteration
            for (int k = 1; k < Math.Min(M, N); k++)
            {
                //Update (Ik)th row of the approximate error matrix:
                List<int> t1 = new List<int>();
                t1.Add(m[I[k]]);
                Rik = (Matrix)(UserImpedance(t1, n) - U.SubMatrix(I[k], 1, 0, U.ColumnCount).Multiply(V));
                //CHECKED OK all code before works fine
                //Find kth column index Jk
                max = -1.0;
                col = 0;

                foreach (int ind in j)
                {
                    if (Math.Abs(Rik[0, ind]) > max)
                    {
                        max = Math.Abs(Rik[0, ind]);
                        col = ind;
                    }
                }

                J[k] = col;
                j.Remove(J[k]);

                //Terminate if R(I(k),J(k)) == 0
                if (Rik[0, J[k]] == 0)
                {
                    break;
                }

                //Set k-th row of V equal to normalized error
                Matrix Vk = (Matrix)Rik.Divide(Rik[0, J[k]]);

                //Update (Jk)th column of the approximate error matrix
                List<int> n1 = new List<int>();
                n1.Add(n[J[k]]);
                Rjk = (Matrix)(UserImpedance(m, n1) - U.Multiply(V.SubMatrix(0, V.RowCount, J[k], 1)));

                // Set k-th column of U equal to updated error

        public void StructureRecurse(DenseMatrix X, DenseMatrix Psi, DenseVector d, int n, ref DenseMatrix Q,
            ref DenseMatrix O, ref DenseMatrix pT_n)
        {
            //O = O(t-1) O_enxt = O(t)
            //o should be a column vector ( in matrix form)
            var x = new DenseVector(X.RowCount);
            var psi = new DenseVector(Psi.ColumnCount);

            X.Column(n, x);
            Psi.Row(n, psi);

            DenseMatrix p_n = CalculateGreatPsi((DenseMatrix) x.ToColumnMatrix(), (DenseMatrix) psi.ToRowMatrix());

            pT_n = (DenseMatrix) p_n.Transpose();

            double ee = Math.Abs(d[n] - (pT_n.Multiply(O))[0, 0]);
            double temp = 1 + (pT_n.Multiply(Q)).Multiply(p_n)[0, 0];
            double ae = Math.Abs(ee/temp);

            if (ee >= ae)
            {
                var L = (DenseMatrix) Q.Multiply(p_n).Multiply(1/temp);
                Q = (DenseMatrix) ((DenseMatrix.Identity(Q.RowCount).Subtract(L.Multiply(pT_n))).Multiply(Q));
                O = (DenseMatrix) O.Add(L*ee);
            }
            else
            {
                Q = (DenseMatrix) DenseMatrix.Identity(Q.RowCount).Multiply(Q);
            }
        }

        public void Train(DenseMatrix X, DenseVector d, DenseVector Kd)
        {
            int R = X.RowCount;
            int N = X.ColumnCount;
            int U = 0; //the number of neurons in the structure


            var c = new DenseMatrix(R, 1);
            var sigma = new DenseMatrix(R, 1);

            var Q = new DenseMatrix((R + 1), (R + 1));
            var O = new DenseMatrix(1, (R + 1));
            var pT_n = new DenseMatrix((R + 1), 1);

            double maxPhi = 0;
            int maxIndex;

            var Psi = new DenseMatrix(N, 1);

            Console.WriteLine("Running...");
            //for each observation n in X
            for (int i = 0; i < N; i++)
            {
                Console.WriteLine(100*(i/(double) N) + "%");

                var x = new DenseVector(R);
                X.Column(i, x);

                //if there are neurons in structure,
                //update structure recursively.
                if (U == 0)
                {
                    c = (DenseMatrix) x.ToColumnMatrix();
                    sigma = new DenseMatrix(R, 1, SigmaZero);
                    U = 1;
                    Psi = CalculatePsi(X, c, sigma);
                    UpdateStructure(X, Psi, d, ref Q, ref O);
                    pT_n =
                        (DenseMatrix)
                            (CalculateGreatPsi((DenseMatrix) x.ToColumnMatrix(), (DenseMatrix) Psi.Row(i).ToRowMatrix()))
                                .Transpose();
                }
                else
                {
                    StructureRecurse(X, Psi, d, i, ref Q, ref O, ref pT_n);
                }


                bool KeepSpinning = true;
                while (KeepSpinning)
                {
                    //Calculate the error and if-part criteria
                    double ee = pT_n.Multiply(O)[0, 0];

                    double approximationError = Math.Abs(d[i] - ee);

                    DenseVector Phi;
                    double SumPhi;
                    CalculatePhi(x, c, sigma, out Phi, out SumPhi);

                    maxPhi = Phi.Maximum();
                    maxIndex = Phi.MaximumIndex();

                    if (approximationError > delta)
                    {
                        if (maxPhi < threshold)
                        {
                            var tempSigma = new DenseVector(R);
                            sigma.Column(maxIndex, tempSigma);

                            double minSigma = tempSigma.Minimum();
                            int minIndex = tempSigma.MinimumIndex();
                            sigma[minIndex, maxIndex] = k_sigma*minSigma;
                            Psi = CalculatePsi(X, c, sigma);
                            UpdateStructure(X, Psi, d, ref Q, ref O);
                            var psi = new DenseVector(Psi.ColumnCount);
                            Psi.Row(i, psi);

                            pT_n =
                                (DenseMatrix)
                                    CalculateGreatPsi((DenseMatrix) x.ToColumnMatrix(), (DenseMatrix) psi.ToRowMatrix())
                                        .Transpose();
                        }
                        else
                        {
                            //add a new neuron and update strucutre

                            double distance = 0;
                            var cTemp = new DenseVector(R);
                            var sigmaTemp = new DenseVector(R);

                            //foreach input variable
                            for (int j = 0; j < R; j++)
                            {
                                distance = Math.Abs(x[j] - c[j, 0]);
                                int distanceIndex = 0;

                                //foreach neuron past 1
                                for (int k = 1; k < U; k++)
                                {
//.........这里部分代码省略.........

        private void ComputeEssentialFundamental()
        {
            if (!IsCamLeftCalibrated || !IsCamRightCalibrated)
                return;

            // E = [T]xR -> translation/rotation from L to R frames
            // Al = [Rl|Tl], Al^-1 = [Rl^T | -Rl^T * Tl] (https://pl.wikipedia.org/wiki/Elementarne_macierze_transformacji)
            // Al->r = [R|T] = Ar * Al^-1
            // [R|T] = [Rr*Rl^T | Rr * (-Rl^T * Tl) + Tr]

            Matrix<double> rotLR = RotationRight.Multiply(RotationLeft.Transpose());
            Matrix<double> transLR = (RotationRight.Multiply(
                -RotationLeft.Transpose().Multiply(TranslationLeft)))
                .Add(TranslationRight);

            Matrix<double> skewTransMat = new DenseMatrix(3, 3);
            skewTransMat[0, 0] = 0;
            skewTransMat[0, 1] = -transLR[2, 0];
            skewTransMat[0, 2] = transLR[1, 0];
            skewTransMat[1, 0] = transLR[2, 0];
            skewTransMat[1, 1] = 0;
            skewTransMat[1, 2] = -transLR[0, 0];
            skewTransMat[2, 0] = -transLR[1, 0];
            skewTransMat[2, 1] = transLR[0, 0];
            skewTransMat[2, 2] = 0;

            Essential = skewTransMat.Multiply(rotLR);
            // F = Kr^-T * E * Kl^-1
            Fundamental = CalibrationRight.Inverse().Transpose().Multiply(Essential).Multiply(CalibrationLeft.Inverse());
        }

        /// <summary>
        ///     Train.  Single iteration.
        /// </summary>
        public void Iteration()
        {
            int rowCount = _trainingData.Count;
            int inputColCount = _trainingData[0].Input.Length;

            Matrix<double> xMatrix = new DenseMatrix(rowCount, inputColCount + 1);
            Matrix<double> yMatrix = new DenseMatrix(rowCount, 1);

            for (int row = 0; row < _trainingData.Count; row++)
            {
                BasicData dataRow = _trainingData[row];
                int colSize = dataRow.Input.Count();

                xMatrix[row, 0] = 1;
                for (int col = 0; col < colSize; col++)
                {
                    xMatrix[row, col + 1] = dataRow.Input[col];
                }
                yMatrix[row, 0] = dataRow.Ideal[0];
            }

            // Calculate the least squares solution
            QR qr = xMatrix.QR();
            Matrix<double> beta = qr.Solve(yMatrix);

            double sum = 0.0;
            for (int i = 0; i < inputColCount; i++)
                sum += yMatrix[i, 0];
            double mean = sum/inputColCount;

            for (int i = 0; i < inputColCount; i++)
            {
                double dev = yMatrix[i, 0] - mean;
                _sst += dev*dev;
            }

            Matrix<double> residuals = xMatrix.Multiply(beta).Subtract(yMatrix);
            _sse = residuals.L2Norm()*residuals.L2Norm();

            for (int i = 0; i < _algorithm.LongTermMemory.Length; i++)
            {
                _algorithm.LongTermMemory[i] = beta[i, 0];
            }

            // calculate error
            _errorCalculation.Clear();
            foreach (BasicData dataRow in _trainingData)
            {
                double[] output = _algorithm.ComputeRegression(dataRow.Input);
                _errorCalculation.UpdateError(output, dataRow.Ideal, 1.0);
            }
            _error = _errorCalculation.Calculate();
        }

 private double[] Polyfit(double[] x, double[] y, int degree)
 {
     // Vandermonde matrix
     var v = new DenseMatrix(x.Length, degree + 1);
     for (int i = 0; i < v.RowCount; i++)
         for (int j = 0; j <= degree; j++) v[i, j] = Math.Pow(x[i], j);
     var yv = new DenseVector(y).ToColumnMatrix();
     QR<double> qr = v.QR();
     // Math.Net doesn't have an "economy" QR, so:
     // cut R short to square upper triangle, then recompute Q
     var r = qr.R.SubMatrix(0, degree + 1, 0, degree + 1);
     var q = v.Multiply(r.Inverse());
     var p = r.Inverse().Multiply(q.TransposeThisAndMultiply(yv));
     return p.Column(0).ToArray();
 }

        public static Matrix<double> WorldToImagePoints(this Matrix<double> worldPoints, DenseMatrix cameraCalibration, DenseVector posePosition, DenseMatrix poseRotation)
        {
            return
            cameraCalibration.Multiply(
                        poseRotation.Inverse()
                            .Multiply(worldPoints)
                            .Translate(-posePosition)
                            .ProjectCamCenteredWorldToHomogImagePoints());

            /*This version is consistent with my DPixelToWorld but different from openCV ProjectPoints
             * cameraCalibration.Multiply(
                poseRotation.Inverse()
                .Multiply(worldPoints.Translate(-posePosition))
                .ProjectCamCenteredWorldToHomogImagePoints());*/
        }

 public static DenseMatrix DPixelToWorld(DenseMatrix inverseCalibration, DenseMatrix homogeneousPixels, DenseMatrix depths)
 {
     return (DenseMatrix)
             depths.Replicate(3,1)
             .PointwiseMultiply(inverseCalibration.Multiply(homogeneousPixels));
 }