C# Matrix.SetValue方法代码示例

        /// <summary>
        /// Multiplication of  two matrices
        /// </summary>
        public static Matrix operator *(Matrix first, Matrix second)
        {
            if (first.rows != second.cols
                || first.cols != second.rows)
            {
                throw new Exception("Matrices are different");
            }

            Matrix resultMatrix = new Matrix(first.rows, second.cols);
            for (int i = 0; i < resultMatrix.rows; i++)
            {
                for (int j = 0; j < resultMatrix.cols; j++)
                {
                    int result = 0;
                    for (int k = 0; k < first.cols; k++)
                    {

                        result += first.GetValue(i, k) * second.GetValue(k, j);
                    }

                    resultMatrix.SetValue(i, j, result);
                }
            }

            return resultMatrix;
        }

        //Emgu.CV.Structure.

        public Image<Emgu.CV.Structure.Bgr, Byte> MatchImages(Image<Emgu.CV.Structure.Gray, Byte> modelImage, Image<Emgu.CV.Structure.Gray, byte> observedImage)
        {
            HomographyMatrix homography = null;

            FastDetector fastCPU = new FastDetector(10, true);
            VectorOfKeyPoint modelKeyPoints;
            VectorOfKeyPoint observedKeyPoints;
            Matrix<int> indices;

            BriefDescriptorExtractor descriptor = new BriefDescriptorExtractor();

            Matrix<byte> mask;
            int k = 2;
            double uniquenessThreshold = 0.8;

            //extract features from the object image
            modelKeyPoints = fastCPU.DetectKeyPointsRaw(modelImage, null);
            Matrix<Byte> modelDescriptors = descriptor.ComputeDescriptorsRaw(modelImage, null, modelKeyPoints);

            // extract features from the observed image
            observedKeyPoints = fastCPU.DetectKeyPointsRaw(observedImage, null);
            Matrix<Byte> observedDescriptors = descriptor.ComputeDescriptorsRaw(observedImage, null, observedKeyPoints);
            BruteForceMatcher<Byte> matcher = new BruteForceMatcher<Byte>(DistanceType.L2);
            matcher.Add(modelDescriptors);

            indices = new Matrix<int>(observedDescriptors.Rows, k);
            using (Matrix<float> dist = new Matrix<float>(observedDescriptors.Rows, k))
            {
                matcher.KnnMatch(observedDescriptors, indices, dist, k, null);
                mask = new Matrix<byte>(dist.Rows, 1);
                mask.SetValue(255);
                Features2DToolbox.VoteForUniqueness(dist, uniquenessThreshold, mask);
            }

            int nonZeroCount = CvInvoke.cvCountNonZero(mask);
            if (nonZeroCount >= 4)
            {
                nonZeroCount = Features2DToolbox.VoteForSizeAndOrientation(modelKeyPoints, observedKeyPoints, indices, mask, 1.5, 20);
                if (nonZeroCount >= 4)
                    homography = Features2DToolbox.GetHomographyMatrixFromMatchedFeatures(
                    modelKeyPoints, observedKeyPoints, indices, mask, 2);
            }

            //Draw the matched keypoints
            Image<Emgu.CV.Structure.Bgr, Byte> result = Features2DToolbox.DrawMatches(modelImage, modelKeyPoints, observedImage, observedKeyPoints,
               indices, new Emgu.CV.Structure.Bgr(255, 255, 255), new Emgu.CV.Structure.Bgr(255, 255, 255), mask, Features2DToolbox.KeypointDrawType.DEFAULT);

            #region draw the projected region on the image
            if (homography != null)
            {  //draw a rectangle along the projected model
                Rectangle rect = modelImage.ROI;
                PointF[] pts = new PointF[] { 
         new PointF(rect.Left, rect.Bottom),
         new PointF(rect.Right, rect.Bottom),
         new PointF(rect.Right, rect.Top),
         new PointF(rect.Left, rect.Top)};
                homography.ProjectPoints(pts);

                result.DrawPolyline(Array.ConvertAll<PointF, Point>(pts, Point.Round), true, new Emgu.CV.Structure.Bgr(Color.Red), 5);
            }
            #endregion

            return result;
        }

        private void GetAllKeyPointsAndMatch(Image<Gray, byte> prevFrame, Image<Gray, byte> currFrame)
        {
            int k = 2;
            double uniquenessThreshold = .8;
            VectorOfKeyPoint targetKeyPoints = new VectorOfKeyPoint();
            VectorOfKeyPoint currentFrameKeyPoints = new VectorOfKeyPoint();
            Matrix<float> targetDescriptors = surfDetector.DetectAndCompute(prevFrame, null, targetKeyPoints);
            Matrix<float> currentFrameDescriptors = surfDetector.DetectAndCompute(currFrame, null, currentFrameKeyPoints);
            if (targetDescriptors != null && currentFrameDescriptors != null)
            {
                BruteForceMatcher<float> keyPointMatcher = new BruteForceMatcher<float>(DistanceType.L2);
                keyPointMatcher.Add(targetDescriptors);

                Matrix<int> indices = new Matrix<int>(currentFrameDescriptors.Rows, k);
                using (Matrix<float> dist = new Matrix<float>(currentFrameDescriptors.Rows, k))
                {
                    keyPointMatcher.KnnMatch(currentFrameDescriptors, indices, dist, k, null);
                    Matrix<byte> matchMask = new Matrix<byte>(dist.Rows, 1);
                    matchMask.SetValue(255);
                    Features2DToolbox.VoteForUniqueness(dist, uniquenessThreshold, matchMask);
                    prevMasked = Features2DToolbox.DrawMatches(prevFrame, targetKeyPoints, currFrame, currentFrameKeyPoints, indices, new Bgr(255, 0, 0), new Bgr(0, 255, 0), matchMask, Features2DToolbox.KeypointDrawType.DEFAULT);
                    CalculateAvgDelta(targetDescriptors, currentFrameDescriptors, matchMask);
                }
            }
        }

        private static bool IsModelInObserved( Image<Gray, byte> modelImage, Image<Gray, byte> observedImage, double similarityThreshold = 0.075 )
        {
            var surfCpu = new SURFDetector(500, false);

             Matrix<byte> mask;
             int k = 2;
             double uniquenessThreshold = 0.8;

             //extract features from the object image
             var modelKeyPoints = surfCpu.DetectKeyPointsRaw( modelImage, null );
             Matrix<float> modelDescriptors = surfCpu.ComputeDescriptorsRaw(modelImage, null, modelKeyPoints);

             // extract features from the observed image
             var observedKeyPoints = surfCpu.DetectKeyPointsRaw( observedImage, null );
             Matrix<float> observedDescriptors = surfCpu.ComputeDescriptorsRaw(observedImage, null, observedKeyPoints);
             BruteForceMatcher<float> matcher = new BruteForceMatcher<float>(DistanceType.L2);
             matcher.Add( modelDescriptors );

             var indices = new Matrix<int>( observedDescriptors.Rows, k );
             using ( var dist = new Matrix<float>( observedDescriptors.Rows, k ) )
             {
            matcher.KnnMatch( observedDescriptors, indices, dist, k, null );
            mask = new Matrix<byte>( dist.Rows, 1 );
            mask.SetValue( 255 );
            Features2DToolbox.VoteForUniqueness( dist, uniquenessThreshold, mask );
             }

             int keypointMatchCount = CvInvoke.cvCountNonZero( mask );
             if ( keypointMatchCount >= 4 )
             {
            keypointMatchCount = Features2DToolbox.VoteForSizeAndOrientation( modelKeyPoints, observedKeyPoints, indices, mask, 1.5, 20 );
            if ( keypointMatchCount >= 4 )
            {
               Features2DToolbox.GetHomographyMatrixFromMatchedFeatures( modelKeyPoints, observedKeyPoints, indices, mask, 2 );
            }
             }

             var similarity = (double)keypointMatchCount / observedKeyPoints.Size;
             return similarity > similarityThreshold;
        }

        /// <summary>
        /// Parallel move from focused object to others.
        /// </summary>
        /// <param name="move">Which direction</param>
        /// <param name="status">Status of eye or kinect..etc..</param>
        /// <returns>New focused object</returns>
        private BuildingObjectLib3DS GetObjectByParallelMovement(DrawingEnumTypes.Movement move, DrawingStatus status)
        {
            XmlTextWriter writer = null;
            writer = new XmlTextWriter (Console.Out);

            if (this.Father.Childs.Count <= 1)
                return this;

            int count = Father.Childs.Count - 1;
            BuildingObjectLib3DS rnt = null;

            double shita = Math.Atan((status.eye.x - this.Coordinate.x) / (status.eye.z + this.Coordinate.y));
            Matrix<double> matrix = new Matrix<double>(1, 2);
            matrix.SetValue(0);
            matrix.Data[0, 0] = this.Coordinate.x;
            matrix.Data[0, 1] = -this.Coordinate.y;
            Matrix<double> _matrix = new Matrix<double>(2, 2);
            _matrix.SetValue(0);
            _matrix.Data[0, 0] = Math.Cos(shita);
            _matrix.Data[0, 1] = -Math.Sin(shita);
            _matrix.Data[1, 0] = Math.Sin(shita);
            _matrix.Data[1, 1] = Math.Cos(shita);

            Matrix<double> otherChilds = new Matrix<double>(1, 2);
            otherChilds.SetValue(0);
            double MinIn = 99999.0f;
            double MinOut = 99999.0f;
            double MinLeft = 99999.0f;
            double MinRight = 99999.0f;
            foreach(BuildingObjectLib3DS ThisObj in Father.Childs.Values)
            {
                if(ThisObj.Equals(this))
                    continue;

                otherChilds.Data[0, 0] = ThisObj.Coordinate.x;
                otherChilds.Data[0, 1] = -ThisObj.Coordinate.y;
                otherChilds = (otherChilds - matrix) * _matrix;
                switch (move)
                {
                    case DrawingEnumTypes.Movement.MoveIn:
                        if(otherChilds.Data[0, 1] < 0 &&
                            Math.Abs(otherChilds.Data[0, 1]) < MinIn)
                        {
                            rnt = ThisObj;
                            MinIn = Math.Abs(otherChilds.Data[0, 1]);
                        }
                        break;
                    case DrawingEnumTypes.Movement.MoveOut:
                        if (otherChilds.Data[0, 1] > 0 &&
                            Math.Abs(otherChilds.Data[0, 1]) < MinOut)
                        {
                            rnt = ThisObj;
                            MinOut = Math.Abs(otherChilds.Data[0, 1]);
                        }
                        break;
                    case DrawingEnumTypes.Movement.MoveLeft:
                        if (otherChilds.Data[0, 0] < 0 &&
                            Math.Abs(otherChilds.Data[0, 0]) < MinLeft)
                        {
                            rnt = ThisObj;
                            MinLeft = Math.Abs(otherChilds.Data[0, 0]);
                        }
                        break;
                    case DrawingEnumTypes.Movement.MoveRight:
                        if (otherChilds.Data[0, 0] > 0 &&
                            Math.Abs(otherChilds.Data[0, 0]) < MinRight)
                        {
                            rnt = ThisObj;
                            MinRight = Math.Abs(otherChilds.Data[0, 0]);
                        }
                        break;
                    default:
                        return this;
                }
            }

            // If we find the object
            if (rnt != null)
            {
                return rnt;
            }

            // Do not find....
            return this;
        }

        /// <summary>
        ///  Calculates the Earth-Sun vector in [km] according to the simualtion time.
        /// </summary>
        /// <param name="simTime">the simulation time at which the Earth-Sun vector is required</param>
        /// <returns>a Matrix containing the Earth-Sun vector in ECI.</returns>
        public Matrix<double> getEarSunVec(double simTime){
            if (_isSunVecConstant && esVec.NumCols != 0 && esVec.NumRows != 0) // != Matrix()
                return esVec;

            Matrix<double> RSun = new Matrix<double>(3, 1, 0.0);
            double eclLong, meanLongSun, MASun, obl, rSun, TUt1, TTdb;
            double JDUt1 = (simTime / 86400) + SimParameters.SimStartJD;

            const double aU = 149597870.0;
            const double rad = Math.PI / 180;


            // Computing the number of Julian centuries from the epoch:  
            TUt1 = (JDUt1 - 2451545.0) / 36525;

            // Computing the Mean longitude of the Sun:  
            meanLongSun = 280.460 + 36000.77 * TUt1;

            // Put into range of 0 to 360 degrees
            if (meanLongSun < 0)
            {
                double meanLongSunDiv = Math.Floor(-1 * meanLongSun / 360);
                meanLongSun = meanLongSun + (meanLongSunDiv + 1) * 360;
            }
            else if (meanLongSun > 360)
            {
                double meanLongSunDiv = Math.Floor(meanLongSun / 360);
                meanLongSun = meanLongSun - meanLongSunDiv * 360;
            }
            //end if //

            // Juliamn centuries of Barycentric dynamical time are assumed to be equal
            // to the number of Julian centuries from the epoch:  
            TTdb = TUt1;

            // Computing the Mean Anomaly of the sun: 
            MASun = 357.5277233 + 35999.05034 * TTdb;

            // Put into range of 0 to 360 degrees
            if (MASun < 0)
            {
                double MASunDiv = Math.Floor(-1 * MASun / 360);
                MASun = MASun + (MASunDiv + 1) * 360;
            }
            else if (MASun > 360)
            {
                double MASunDiv = Math.Floor(MASun / 360);
                MASun = MASun - MASunDiv * 360;
            }

            // Computing the ecliptic longitude:  
            eclLong = meanLongSun + 1.914666471 * Math.Sin(MASun * rad) + 0.019994643 * Math.Sin(2 * MASun * rad);

            // Computing the sun-centered position vector from the Sun to Earth:  
            rSun = 1.000140612 - 0.016708617 * Math.Cos(MASun * rad) - 0.000139589 * Math.Cos(2 * MASun * rad);

            // Computing the obliquity of the ecliptic:  
            obl = 23.439291 - 0.0130042 * TTdb;

            // Transforming sun_centered Earth position vector to a geocentric
            // equatorial position vector:
            RSun.SetValue(1, 1, rSun * Math.Cos(eclLong * rad) * aU);
            RSun.SetValue(2, 1, rSun * Math.Cos(obl * rad) * Math.Sin(eclLong * rad) * aU);
            RSun.SetValue(3, 1, rSun * Math.Sin(obl * rad) * Math.Sin(eclLong * rad) * aU);

            //if(isSunVecConstant)
            //	esVec = RSun;

            return (RSun);
        }//End getEarthSunVec method