C# Matrix.GetRow方法代码示例

        public void TestMethod1()
        {
            Sun mysun = new Sun();
            Matrix<double> esVec = new Matrix<double>();

            esVec = mysun.getEarSunVec(20.0);

            esVec.GetRow(4);

        }

        /// <summary>
        ///     Performs gaussian elimination on the given matrix.
        /// </summary>
        /// <param name="matrix"></param>
        /// <returns></returns>
        public static Matrix GaussianElimination(Matrix matrix)
        {
            // Derived from ref [1] Theorem 3.5

            // identify the row with the first non zero element
            int nonZeroRowIndex = 0;
            bool run = true;
            int nonZeroColumnIndex = 0;
            while (run && nonZeroColumnIndex < matrix.Columns)
            {
                // Check if columns has non-zero entry
                var col = matrix.GetColumn(nonZeroColumnIndex);
                for (int j = 0; j < col.Length; j++)
                {
                    if (Math.Abs(col[j]) > TOLERANCE)
                    {
                        nonZeroRowIndex = j;
                        run = false;
                        break;
                    }
                }

                if (run)
                {
                    nonZeroColumnIndex++;
                }
            }

            // Move this row into the top position
            if (nonZeroRowIndex != 0)
            {
                matrix = SwapRows(matrix, nonZeroRowIndex, 0);
            }

            // If the first non zero row does not equal 1 then normalize the
            if (Math.Abs(matrix[0, nonZeroColumnIndex] - 1.0) > TOLERANCE)
            {
                matrix = MultiplyRowByScalar(matrix, 0, 1.0 / matrix[0, nonZeroColumnIndex]);
            }

            int iMax = Math.Min(matrix.Columns, matrix.Rows);
            // Zero-wise columns
            for (int i = 0, currentCol = 0; i < iMax && currentCol < matrix.Columns; i++, currentCol++)
            {
                var currentStartElement = matrix[i, currentCol];

                if (Math.Abs(currentStartElement) > TOLERANCE)
                {
                    for (int j = i + 1; j < iMax; j++)
                    {
                        var modifiedRow =
                            matrix.GetRow(i)
                                .Select(m => -1 * m * (matrix[j, currentCol] / currentStartElement))
                                .ToArray();
                        matrix = AddRows(matrix, modifiedRow, j);
                    }
                }
                else
                {
                    i--; // The test element was zero, do not increment the row
                }
            }

            return matrix;
        }

        void nui_DepthFrameReady(object sender, ImageFrameReadyEventArgs e)
        {
            // Depth is in mm
            short[][] depth = e.ImageFrame.ToDepthArray2D();
            if (this._skel != null)
            {
                Matrix<double> data = new Matrix<double>(depthHeight * depthWidth, 3);
                List<MCvPoint3D32f> head_points = new List<MCvPoint3D32f>();
                MCvPoint3D32f head_pos = new MCvPoint3D32f();
                head_pos.x = this._skel.Joints[JointID.Head].Position.X;
                head_pos.y = this._skel.Joints[JointID.Head].Position.Y;
                head_pos.z = this._skel.Joints[JointID.Head].Position.Z;

                int row = 0;
                for (int x = 0; x < depthWidth; x++)
                {
                    for (int y = 0; y < depthHeight; y++)
                    {
                        float depthVal = depth[x][y];
                        depthVal /= 1000.0f; // To convert into meters
                        float x_rw, y_rw;
                        projectiveToRealWorld(depthVal, x, y, out x_rw, out y_rw);

                        MCvPoint3D32f pt = new MCvPoint3D32f(x_rw, y_rw, depthVal);
                        MCvPoint3D32f delta = pt - head_pos;

                        if (delta.Norm < 0.3) // If the point is closer than 10cm
                        {
                            head_points.Add(pt);
                        }

                        data[row, 0] = x_rw;
                        data[row, 1] = y_rw;
                        data[row, 2] = depthVal;
                        row++;
                    }
                }

                // Get a matrix of all of the points in the head
                double[,] head_pts = new double[head_points.Count, 3];
                for (int i=0; i < head_points.Count; i++)
                {
                    MCvPoint3D32f p = head_points[i];
                    head_pts[i, 0] = p.x;
                    head_pts[i, 1] = p.y;
                    head_pts[i, 2] = p.z;
                }
                if (head_points.Count > 0)
                {
                    if (this.icp == null)
                    {
                        this.current_head_points = head_pts;
                        this.icp = new icp_net.ManagedICP(head_pts, head_pts.GetLength(0), 3);
                    }
                    else
                    {
                        double[,] R = new double[3, 3];
                        R[0, 0] = 1.0;
                        R[1, 1] = 1.0;
                        R[2, 2] = 1.0;
                        double[] t = new double[3];
                        t[0] = head_pos.x;
                        t[1] = head_pos.y;
                        t[2] = head_pos.z;
                        icp.fit(head_pts, head_pts.GetLength(0), R, t, -1);

                        Matrix<double> Rot = new Matrix<double>(R);
                        Matrix<double> new_pts_matrix = new Matrix<double>(head_pts);
                        Matrix<double> trans = new Matrix<double>(t);
                        // Move the new head to the origin
                        for (int rowIdx = 0; rowIdx < new_pts_matrix.Rows; rowIdx++)
                        {
                            Matrix<double> ro = new_pts_matrix.GetRow(rowIdx);
                            ro -= trans;
                        }

                        // And rotate it in to place with the other head
                        new_pts_matrix *= Rot;
                        double[,] new_points_array = new double[new_pts_matrix.Rows, 3];
                        for (int i=0; i < new_pts_matrix.Rows; i++)
                        {
                            for (int j=0; j < 3; j++)
                            {
                                new_points_array[i, j] = new_pts_matrix[i, j];
                            }
                        }
                        double[,] refined_head_points = refineHead(current_head_points, new_points_array);
                        // And update our icp model
                        this.icp = new icp_net.ManagedICP(refined_head_points, refined_head_points.GetLength(0), 3);
                        this.current_head_points = refined_head_points;
                    }
                }
                //= _skel.Joints[JointID.Head].Position;
            }
        }