Esempio n. 1
0
        public void testCvSolve1()
        {
            Vector3D[] planeVecs = new Vector3D[3];
            planeVecs[0] = new Vector3D(.0, .0, .0);
            planeVecs[1] = new Vector3D(.0, 1.0, .0);
            planeVecs[2] = new Vector3D(1.0, .0, .0);

            Matrix<float> A = new Matrix<float>(4, 3);
            A.SetZero();
            Matrix<float> x = new Matrix<float>(3, 1);
            x.SetZero();
            Matrix<float> b = new Matrix<float>(4, 1);
            //b.SetValue(1.0);
            b.SetZero();

            for (int i = 0; i < planeVecs.Length; ++i)
            {
                Vector3D v = planeVecs[i];
                A[0, 0] += (float)(v.X * v.X);
                A[0, 1] += (float)(v.X * v.Y);
                A[0, 2] += (float)(v.X * v.Z);
                A[1, 1] += (float)(v.Y * v.Y);
                A[1, 2] += (float)(v.Y * v.Z);
                A[2, 2] += (float)(v.Z * v.Z);

                b[0, 0] += (float)v.X;
                b[1, 0] += (float)v.Y;
                b[2, 0] += (float)v.Z;
                b[3, 0] += (float)1.0;
            }
            A[1, 0] = A[0, 1];
            A[2, 0] = A[0, 2];
            A[2, 1] = A[1, 2];

            A[3, 0] = b[0, 0];
            A[3, 1] = b[1, 0];
            A[3, 2] = b[2, 0];

            Emgu.CV.CvInvoke.cvSolve(A, b, x, Emgu.CV.CvEnum.SOLVE_METHOD.CV_SVD);
            Vector3D bestNormal = new Vector3D();
            bestNormal.X = x[0, 0];
            bestNormal.Y = x[1, 0];
            bestNormal.Z = x[2, 0];
        }
Esempio n. 2
0
        private bool isInitialized; // true if any data has been fed

        public KalmanFilter(int variables)
        {
            variablesCount = variables;

            int measurementVariables = variables;
            int dynamicVariables = variables * 2;

            float[] state = new float[dynamicVariables];
            for (int i = 0; i < dynamicVariables; ++i)
                state[i] = 0.0f;

            Matrix<float> transitionMatrix = new Matrix<float>(dynamicVariables, dynamicVariables);
            transitionMatrix.SetZero();
            for (int i = 0; i < dynamicVariables; ++i)
            {
                transitionMatrix[i, i] = 1.0f;
                if (i >= measurementVariables)
                    transitionMatrix[i - measurementVariables, i] = 1;
            }

            Matrix<float> measurementMatrix = new Matrix<float>(measurementVariables, dynamicVariables);
            measurementMatrix.SetZero();
            for (int i = 0; i < measurementVariables; ++i)
                measurementMatrix[i, i] = 1.0f;

            Matrix<float> processNoise = new Matrix<float>(dynamicVariables, dynamicVariables);
            processNoise.SetIdentity(new MCvScalar(1));//1.0e-4));

            Matrix<float> measurementNoise = new Matrix<float>(measurementVariables, measurementVariables);
            measurementNoise.SetIdentity(new MCvScalar(4));//1.0e-1));

            Matrix<float> errorCovariancePost = new Matrix<float>(dynamicVariables, dynamicVariables);
            errorCovariancePost.SetIdentity();

            kalman = new Kalman(dynamicVariables, measurementVariables, 0);
            kalman.CorrectedState = new Matrix<float>(state);
            kalman.TransitionMatrix = transitionMatrix;
            kalman.MeasurementNoiseCovariance = measurementNoise;
            kalman.ProcessNoiseCovariance = processNoise;
            kalman.ErrorCovariancePost = errorCovariancePost;
            kalman.MeasurementMatrix = measurementMatrix;
        }
        // this is from the blog entry
        public void Recompute()
        {
            if (source == null || dest == null || source.Count != dest.Count)
                throw new Exception("Input data null or not equal in length");

            // compute covariance matrix
            Matrix<double> H = new Matrix<double>(2, 2);

            H.SetZero();
            for (int i = 0; i < source.Count; i++) {
                var a = source[i].ToMatrix() - SourceCentroid;
                var b = dest[i].ToMatrix() - DestCentroid;
                H += a * b.Transpose();
            }

            /* perform svd  where A =  U W VT
             *  A  IntPtr  Source MxN matrix
             *  W  IntPtr  Resulting singular value matrix (MxN or NxN) or vector (Nx1).
             *  U  IntPtr  Optional left orthogonal matrix (MxM or MxN). If CV_SVD_U_T is specified, the number of rows and columns in the sentence above should be swapped
             *  V  IntPtr  Optional right orthogonal matrix (NxN)
             */

            Matrix<double> U = new Matrix<double>(2, 2);
            Matrix<double> W = new Matrix<double>(2, 2);
            Matrix<double> V = new Matrix<double>(2, 2);
            CvInvoke.cvSVD(H.Ptr, W.Ptr, U.Ptr, V.Ptr, SVD_TYPE.CV_SVD_DEFAULT);

            // compute rotational matrix R=V*UT
            Rotation = V * U.Transpose();

            // find translation
            //Offset = DestCentroid - ( Rotation * SourceCentroid);
            Offset = DestCentroid - SourceCentroid;

            if (Angle > 5) {
                Global.Instance.mainForm.ShowSimpleMessageBox("Excessive Angle Detected - Problem detecting rotation\nOffset = " + new PartLocation(Offset-DestCentroid) + "\nAngle=" + Angle);
            }
        }
Esempio n. 4
0
        private void button7_Click(object sender, EventArgs e)
        {
            Matrix<double> K = new Matrix<double>(3, 3);
            K.SetZero();
            Matrix<double> D = new Matrix<double>(8, 1);
            D.SetZero();

            if (calibrator != null)
            {
                int i, j;
                for (i = 0; i < 3; i++)
                    for (j = 0; j < 3; j++)
                        K.Data[i, j] = calibrator.Intrinsics.IntrinsicMatrix.Data[i, j];
                for (i = 0; i < 4; i++)
                    D.Data[i, 0] = calibrator.Intrinsics.DistortionCoeffs.Data[i, 0];

                calibrator.stopCalibration();

                // saving calibration parameters
                StreamWriter sw = new StreamWriter("CalibParams.txt");

                string intrinsicsStr = Convert.ToString(K.Data[0, 0]) + ",";
                       intrinsicsStr += Convert.ToString(K.Data[0, 1]) + ",";
                       intrinsicsStr += Convert.ToString(K.Data[0, 2]) + ",";
                       intrinsicsStr += Convert.ToString(K.Data[1, 0]) + ",";
                       intrinsicsStr += Convert.ToString(K.Data[1, 1]) + ",";
                       intrinsicsStr += Convert.ToString(K.Data[1, 2]) + ",";
                       intrinsicsStr += Convert.ToString(K.Data[2, 0]) + ",";
                       intrinsicsStr += Convert.ToString(K.Data[2, 1]) + ",";
                       intrinsicsStr += Convert.ToString(K.Data[2, 2]);

                sw.WriteLine(intrinsicsStr);

                string distcoeffsStr = Convert.ToString(D.Data[0, 0]) + "," +
                                        Convert.ToString(D.Data[1, 0]) + "," +
                                        Convert.ToString(D.Data[2, 0]) + "," +
                                        Convert.ToString(D.Data[3, 0]);
                sw.WriteLine(distcoeffsStr);

                sw.Close();

                LatestCamIntrinsics.IntrinsicMatrix.Data[0, 0] = K.Data[0, 0];
                LatestCamIntrinsics.IntrinsicMatrix.Data[0, 2] = K.Data[0, 2];
                LatestCamIntrinsics.IntrinsicMatrix.Data[1, 1] = K.Data[1, 1];
                LatestCamIntrinsics.IntrinsicMatrix.Data[1, 2] = K.Data[1, 2];
                LatestCamIntrinsics.IntrinsicMatrix.Data[2, 2] = 1;

                LatestCamIntrinsics.DistortionCoeffs.Data[0, 0] = D.Data[0, 0];
                LatestCamIntrinsics.DistortionCoeffs.Data[1, 0] = D.Data[1, 0];
                LatestCamIntrinsics.DistortionCoeffs.Data[2, 0] = D.Data[2, 0];
                LatestCamIntrinsics.DistortionCoeffs.Data[3, 0] = D.Data[3, 0];
            }
        }
Esempio n. 5
0
        public void testSVD()
        {
            Vector3D[] planeVecs = new Vector3D[3];
            planeVecs[0] = new Vector3D(.0, .0, 2.0);
            planeVecs[1] = new Vector3D(.0, 2.0, .0);
            planeVecs[2] = new Vector3D(2.0, .0, .0);

            Matrix<float> A = new Matrix<float>(4, 4);
            A.SetZero();
            Matrix<float> x = new Matrix<float>(4, 1);
            x.SetZero();
            Matrix<float> U = new Matrix<float>(4, 4);
            //b.SetValue(1.0);
            U.SetZero();
            Matrix<float> V = new Matrix<float>(4, 4);
            //b.SetValue(1.0);
            U.SetZero();

            for (int i = 0; i < planeVecs.Length; ++i)
            {
                Vector3D v = planeVecs[i];
                A[0, 0] += (float)(v.X * v.X);
                A[0, 1] += (float)(v.X * v.Y);
                A[0, 2] += (float)(v.X * v.Z);
                A[1, 1] += (float)(v.Y * v.Y);
                A[1, 2] += (float)(v.Y * v.Z);
                A[2, 2] += (float)(v.Z * v.Z);

                A[0, 3] -= (float)(v.X);
                A[1, 3] -= (float)(v.Y);
                A[2, 3] -= (float)(v.Z);
                A[3, 3] -= (float)1.0;

            }
            A[1, 0] = A[0, 1];
            A[2, 0] = A[0, 2];
            A[2, 1] = A[1, 2];

            A[3, 0] = -A[0, 3];
            A[3, 1] = -A[1, 3];
            A[3, 2] = -A[2, 3];

            CvInvoke.cvSVD(A, x, U, V, Emgu.CV.CvEnum.SVD_TYPE.CV_SVD_DEFAULT );
        }
Esempio n. 6
0
        public void testEigen()
        {
            Vector3D[] planeVecs = new Vector3D[3];
            planeVecs[0] = new Vector3D(.0, .0, 1.0);
            planeVecs[1] = new Vector3D(.0, 1.0, .0);
            planeVecs[2] = new Vector3D(.0, .0, .0);

            Matrix<float> A = new Matrix<float>(4, 4);
            A.SetZero();
            Matrix<float> x = new Matrix<float>(4, 1);
            x.SetZero();
            Matrix<float> ev = new Matrix<float>(4, 4);
            //b.SetValue(1.0);
            ev.SetZero();

            for (int i = 0; i < planeVecs.Length; ++i)
            {
                Vector3D v = planeVecs[i];
                A[0, 0] += (float)(v.X * v.X);
                A[0, 1] += (float)(v.X * v.Y);
                A[0, 2] += (float)(v.X * v.Z);
                A[1, 1] += (float)(v.Y * v.Y);
                A[1, 2] += (float)(v.Y * v.Z);
                A[2, 2] += (float)(v.Z * v.Z);

                A[0, 3] -= (float)(v.X);
                A[1, 3] -= (float)(v.Y);
                A[2, 3] -= (float)(v.Z);
                A[3, 3] -= (float)1.0;

            }
            A[1, 0] = A[0, 1];
            A[2, 0] = A[0, 2];
            A[2, 1] = A[1, 2];

            A[3, 0] = - A[0, 3];
            A[3, 1] = - A[1, 3];
            A[3, 2] = - A[2, 3];

            CvInvoke.cvEigenVV(A, ev, x, 1e-4, -1, -1);
        }