C# Vector.ToColumnMatrix方法代码示例

        public float ComputePlaneError(int index, Vector<float> v)
        {
            Matrix<float> n_sT = PlaneNs[index].ToRowMatrix(); //transposed n_s [1x3]

            Matrix<float> m = n_sT * v.ToColumnMatrix();

            float q0 = n_sT.Multiply(v.ToColumnMatrix())[0, 0];
            float q1 = n_sT.Multiply(GetMassPoint().ToColumnMatrix())[0, 0];

            float q = q0 + q1;
            return q0 + q1; //ideally this is 0
        }

    public static void CalibrationError(
      Emgu.CV.ExtrinsicCameraParameters ecp,
      Emgu.CV.IntrinsicCameraParameters icp,
      System.Drawing.PointF[] image_points,
      Vector[] reference_points,
      out double[] deviations,
      out Vector[] isect_points) 
    {
      // Shoot rays through image points,
      // intersect with plane defined by extrinsic
      // and measure distance to reference points
      Matrix inv_ecp = Matrix.Identity(4, 4);
      inv_ecp.SetMatrix(0, 2, 0, 3, ecp.ExtrinsicMatrix.ToParsley());
      inv_ecp = inv_ecp.Inverse();

      Ray[] rays = Ray.EyeRays(icp, image_points);
      Plane p = new Plane(ecp);
      isect_points = new Vector[rays.Length];

      deviations = new double[rays.Length];
      for (int i = 0; i < rays.Length; ++i) {
        double t;
        Intersection.RayPlane(rays[i], p, out t);
        Vector isect = rays[i].At(t);
        Vector x = new Vector(new double[]{isect[0],isect[1],isect[2],1});
        x = (inv_ecp * x.ToColumnMatrix()).GetColumnVector(0);
        Vector final = new Vector(new double[]{x[0], x[1], x[2]});
        isect_points[i] = final;
        deviations[i] = (final - reference_points[i]).Norm();
      }
    }

    public CloudPoint ApplyTransform(Matrix R, Vector T)
    {
        // I can guarantee that this will work correctly so we index blindly
        var newLocation = (R * location.ToColumnMatrix() + T.ToColumnMatrix()).GetColumnVector(0);

        return new CloudPoint(newLocation, this.color, this.normal);
    }

        public double[] Transform(double[] X)
        {
            if (transformMatrix == null)
            {
                CalculateTransform();
            }

            Vector mean = new Vector(meanVector);
            Vector xVector = new Vector(X);
            xVector -= mean;
            Vector result = (transformMatrix * xVector.ToColumnMatrix()).GetColumnVector(0);
            return result.CopyToArray();
        }

 public KfParameters(int dx, int dy, Vector<double> x, double dt, double p, double q, double r)
 {
     this.Dt = dt;
     this.T = 0;
     this.Dx = dx;
     this.Dy = dy;
     this.X = x.ToColumnMatrix();
     this.Y = Matrix<double>.Build.Dense(dy, 1, 0);
     this.GenerateA();
     this.GenerateC();
     this.GenerateP(p);
     this.GenerateQ(q);
     this.GenerateR(r);
 }

    /// <summary>
    /// Count the number of black/white transitions for a single ellipse
    /// </summary>
    /// <param name="e">Ellipse</param>
    /// <param name="matrix">Affine ellipse frame that transforms the ellipse to a circle located at origin</param>
    /// <param name="gray">Binary image</param>
    /// <returns>The number of black/white transitions found</returns>
    private int CountBinaryTransitions(DetectedEllipse e, Matrix matrix, Emgu.CV.Image<Gray, byte> gray) {
      // Generate points on circle
      double r = e.Ellipse.MCvBox2D.size.Height * 0.5;
      double t_step = (2 * Math.PI) / _number_circle_points;

      System.Drawing.Rectangle rect = new System.Drawing.Rectangle(System.Drawing.Point.Empty, gray.Size);

      int count_transitions = 0;
      double last_intensity = 0;
      for (double t = 0; t <= 2 * Math.PI; t += t_step) {
        Vector v = new Vector(new double[] { r * Math.Cos(t), r * Math.Sin(t), 1.0 });
        Vector x = matrix.Multiply(v.ToColumnMatrix()).GetColumnVector(0);
        System.Drawing.Point p = new System.Drawing.Point((int)Math.Round(x[0]), (int)Math.Round(x[1]));
        if (rect.Contains(p)) {
          if (t == 0) {
            last_intensity = gray[p].Intensity;
          } else {
            double i = gray[p].Intensity;
            if (i != last_intensity) {
              count_transitions += 1;
              last_intensity = i;
            }
          }
        }
      }
      return count_transitions;
    }

 /// <summary>
 /// Fit the model according to the given training data.
 /// </summary>
 /// <param name="regressor">A <see cref="IRegressor"/> instance.</param>
 /// <param name="x">
 /// Matrix with dimensions [nSamples, nFeatures].
 /// Training vectors, where nSamples is the number of samples
 /// and nFeatures is the number of features.</param>
 /// <param name="y">Vector with dimensions [nSamples]. Target values.</param>
 /// <param name="sampleWeight">Individual weights for each sample. Vector with dimensions [nSamples].</param>
 public static void Fit(
     this IRegressor regressor,
     Matrix<double> x,
     Vector<double> y,
     Vector<double> sampleWeight = null)
 {
     regressor.Fit(x, y.ToColumnMatrix(), sampleWeight);
 }

 /// <summary>
 /// <para>
 /// Returns the coefficient of determination R^2 of the prediction.
 /// </para>
 /// <para>
 /// The coefficient R^2 is defined as (1 - u/v), where u is the regression
 /// sum of squares ((y_true - y_pred) ** 2).sum() and v is the residual
 /// sum of squares ((y_true - y_true.mean()) ** 2).sum().
 /// Best possible score is 1.0, lower values are worse.
 /// </para>
 /// </summary>
 /// <param name="regressor">A <see cref="IRegressor"/> instance.</param>
 /// <param name="x">Feature matrix. Matrix with dimensions [nSamples, nFeatures].</param>
 /// <param name="y">True values for <paramref name="x"/>. Vector with dimensions [nSamples].</param>
 /// <returns> R^2 of Predict(X) wrt. y.</returns>
 public static double Score(this IRegressor regressor, Matrix<double> x, Vector<double> y)
 {
     return Metrics.Metrics.R2Score(y.ToColumnMatrix(), regressor.Predict(x));
 }

 /// <summary>
 /// Append observation vector into observation matrix
 /// </summary>
 /// <param name="y">Observation vector</param>
 public override void AppendY(Vector<double> y)
 {
     this.Y = this.Y.Append(y.ToColumnMatrix());
 }

 /// <summary>
 /// Append state vector into state matrix
 /// </summary>
 /// <param name="x">State vector</param>
 public override void AppendX(Vector<double> x)
 {
     this.X = this.X.Append(x.ToColumnMatrix());
 }

    // Update is called once per frame
    void Update()
    {
        // Check if it is time to perform an update
        timeSinceLastUpdateSec += Time.deltaTime;
        if (timeSinceLastUpdateSec >= updatePeriodSec)
        {
            // Save where I am
            Vector ownshipPosUnity = new Vector(6);
            ownshipPosUnity[0] = gameObject.transform.position.x;
            ownshipPosUnity[1] = gameObject.transform.position.y;
            ownshipPosUnity[2] = gameObject.transform.position.z;
            ownshipPosUnity[3] = 0;
            ownshipPosUnity[4] = 0;
            ownshipPosUnity[5] = 0;

            // New scan
            List<GaussianMeasurement> ms = new List<GaussianMeasurement>();

            // Take a noisy measurement of each target
            for (int i = 0; i < targets.Length; i++)
            {
                // Get target position
                Vector targetPosUnity = GetUnityPosition(targets[i]);

                // Generate gaussian noise
                Vector gaussianNoise = new Vector(3);
                gaussianNoise[0] = nd.NextDouble();
                gaussianNoise[1] = nd.NextDouble();
                gaussianNoise[2] = nd.NextDouble();
                gaussianNoise = (noiseCovCholT * gaussianNoise.ToColumnMatrix()).GetColumnVector(0);

                // Generate measurement
                GaussianMeasurement m = new GaussianMeasurement(1, 1, ownshipPosUnity,
                    new GaussianVector(targetPosUnity + gaussianNoise, noiseCovariance),
                    Coordinate.Type.UNITY3,
                    System.DateTime.Now);
                ms.Add(m);
            }

            // Add to fusion engine to be processed
            fusionEngine.AddScanMeasurements(ms);

            // Clear time since last update
            timeSinceLastUpdateSec = 0.0f;
        }
    }

        public void ProcessImage(Emgu.CV.Image<Emgu.CV.Structure.Bgr, byte> image)
        {
            Emgu.CV.Image<Gray, byte> gray = image.Convert<Gray, byte>();
            gray._ThresholdBinary(new Gray(_threshold), new Gray(255.0));
            gray._Not();

            //Emgu.CV.Contour<System.Drawing.Point> c = gray.FindContours(Emgu.CV.CvEnum.CHAIN_APPROX_METHOD.CV_CHAIN_CODE, Emgu.CV.CvEnum.RETR_TYPE.CV_RETR_LIST);
            Emgu.CV.Contour<System.Drawing.Point> c = gray.FindContours();
            List<Ellipse> ellipses = new List<Ellipse>();

            while (c != null)
            {
                if (c.Count() >= _min_contour_count)
                {
                    System.Drawing.PointF[] mypoints = Array.ConvertAll(
                      c.ToArray<System.Drawing.Point>(),
                      value => new System.Drawing.PointF(value.X, value.Y)
                    );

                    Ellipse e = Emgu.CV.PointCollection.EllipseLeastSquareFitting(mypoints);
                    MCvBox2D box = e.MCvBox2D;
                    box.size.Height *= 0.5f;
                    box.size.Width *= 0.5f;
                    Ellipse final_ellipse = new Ellipse(box);

                    Matrix m = Matrix.Identity(3, 3);
                    m[0, 0] = Math.Cos(final_ellipse.MCvBox2D.angle);
                    m[0, 1] = -Math.Sin(final_ellipse.MCvBox2D.angle);
                    m[0, 2] = final_ellipse.MCvBox2D.center.X;
                    m[1, 0] = Math.Sin(final_ellipse.MCvBox2D.angle);
                    m[1, 1] = Math.Cos(final_ellipse.MCvBox2D.angle);
                    m[1, 2] = final_ellipse.MCvBox2D.center.Y;

                    Matrix inv = m.Inverse();
                    double rating = 0.0;
                    double a = final_ellipse.MCvBox2D.size.Width;
                    double b = final_ellipse.MCvBox2D.size.Height;

                    if (a < b)
                    {
                        double tmp = a;
                        a = b;
                        a = tmp;
                    }
                    foreach (System.Drawing.PointF p in mypoints)
                    {
                        Vector x = new Vector(new double[] { p.X, p.Y, 1 });
                        Matrix r = inv.Multiply(x.ToColumnMatrix());

                        rating += Math.Abs((Math.Pow(r[0, 0] / a, 2) + Math.Pow(r[1, 0] / b, 2)) - 1);
                    }
                    Console.WriteLine(rating);
                    if (rating < 50)
                    {
                        ellipses.Add(final_ellipse);
                    }

                }
                c = c.HNext;
            }

            ellipses.Sort(
              (a, b) =>
              {
                  double dista = a.MCvBox2D.center.X * a.MCvBox2D.center.X + a.MCvBox2D.center.Y * a.MCvBox2D.center.Y;
                  double distb = b.MCvBox2D.center.X * b.MCvBox2D.center.X + b.MCvBox2D.center.Y * b.MCvBox2D.center.Y;
                  return dista.CompareTo(distb);
              }
            );

            Bgr bgr = new Bgr(0, 255, 0);
            MCvFont f = new MCvFont(Emgu.CV.CvEnum.FONT.CV_FONT_HERSHEY_PLAIN, 0.8, 0.8);
            int count = 1;
            foreach (Ellipse e in ellipses)
            {
                image.Draw(e, bgr, 2);
                image.Draw(count.ToString(), ref f, new System.Drawing.Point((int)e.MCvBox2D.center.X, (int)e.MCvBox2D.center.Y), bgr);
                count++;
            }
        }

        static void NIPALS(Matrix X, int PCs, Matrix PCmatrix, Vector EigenValues)
        {

            //TODO:
            //Check that PCs<min(X.rows,X.columns)
            //Check that PCMatrix size is (X.columns, PCs) , i.e. (features, lower number of dimensions)
            //Change name of eigenvalues to something more appropriate
            //Check that eigenvalues vector passed in is of size PCs.


            //X is the zero-mean data matrix
            //E is the residual error after i itereations
            int i;
            //PCmatrix = new Matrix(PCs, X.ColumnCount, 0);
            //EigenValues = new Vector(PCs);
            Matrix E = X.Clone();
            Vector u = new Vector(E.RowCount);
            Vector v = new Vector(E.ColumnCount);
            Matrix E_transposed;
            int initialVector = 0;
            //convergence threshold
            const double threshold = 0.0000001;

            //from http://www.vias.org/tmdatanaleng/dd_nipals_algo.html
            for (i = 0; i < PCs; i++)
            {
                //printMatrix(E, "E");
                //1.    u := x(i) 	Select a column vector xi of the matrix X and copy it to the vector u
                //The vector must be such that the self-inner product is not zero 

                double ut_u = 0;
                Boolean valid = false;
                while (!valid && initialVector < E.ColumnCount)
                {
                    u = E.GetColumnVector(initialVector);
                    initialVector++;
                    if (u.ScalarMultiply(u) != 0)
                        valid = true;
                }
                if (!valid)
                    throw new Exception("Could not find " + PCs + " principal components");

                E_transposed = E.Clone();
                E_transposed.Transpose();
                //printMatrix(E_transposed, "E transposed");

                //int step = 1;
                double error = 1;
                while (error > threshold)
                {
                    //  Console.Out.WriteLine("PC " + (i+1) + " Step : " + step++);

                    //2. 	v := (X'u)/(u'u) 	Project the matrix X onto u in order to find the corresponding loading vs
                    //Console.Out.WriteLine("u: " + u.ToColumnMatrix().ToString());
                    ut_u = u.ScalarMultiply(u);
                    if (ut_u == 0)
                        throw new Exception("Principal component results in complex answer");

                    //Console.Out.WriteLine("u'u: " + ut_u);

                    Matrix v_prime = E_transposed.Multiply(u.ToColumnMatrix());
                    //printMatrix(v_prime, "v prime");
                    v = v_prime.GetColumnVector(0) / ut_u;
                    //Console.Out.WriteLine("v: " + v.ToString());

                    //3.    v := v/|v| 	Normalize the loading vector v to length 1
                    v = v.Normalize();
                    //v = v / v.Norm();
                    //Console.Out.WriteLine("v after normalization: " + v.ToString());


                    //4.1 	u_old := u  Store the score vector u into uold 
                    Vector u_old = u.Clone();
                    //Console.Out.WriteLine("u old: " + u_old.ToString());

                    //4.2      u := (Xp)/(v'v) 	and project the matrix X onto v in order to find corresponding score vector u
                    Matrix u_prime = E.Multiply(v.ToColumnMatrix());
                    //Console.Out.WriteLine("u_prime: ");
                    //printMatrix(u_prime);

                    Vector u_primeColumn = u_prime.GetColumnVector(0);
                    //Console.Out.WriteLine("u_primeColumn: " + u_primeColumn.ToString());

                    double v_v = v.ScalarMultiply(v);

                    //Console.Out.WriteLine("v_v: " + v_v);

                    u = u_primeColumn / v_v;
                    //Console.Out.WriteLine("new u: " + u.ToString());

                    //5. 	d := uold-u 	In order to check for the convergence of the process 
                    //calculate the difference vector d as the difference between the previous scores
                    //and the current scores. If the difference |d| is larger than a pre-defined threshold,
                    //then return to step 2.
                    Vector d = u_old.Subtract(u);
                    //Console.Out.WriteLine("d: " + d.ToString());
                    error = d.Norm();
                    //Console.Out.WriteLine("Error: " + error.ToString());

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

    PointF Backproject(Vector p, Emgu.CV.ExtrinsicCameraParameters ecp) {
      Matrix m = Matrix.Identity(4, 4);
      m.SetMatrix(0, 2, 0, 3, ecp.ExtrinsicMatrix.ToParsley());
      Matrix i = m.Inverse();
      Vector x = new Vector(new double[] { p[0], p[1], p[2], 1 });
      Matrix r = i * x.ToColumnMatrix();
      PointF[] coords = Emgu.CV.CameraCalibration.ProjectPoints(
        new MCvPoint3D32f[] { 
          new MCvPoint3D32f((float)r[0,0], (float)r[1,0], (float)r[2,0])
        },
        ecp,
        Context.Setup.World.Camera.Intrinsics);
      return coords[0];

    }