Exemplo n.º 1
0
        public void TestArithmatic()
        {
            Matrix<byte> m = new Matrix<byte>(10, 8);
             m.SetRandNormal(new MCvScalar(), new MCvScalar(30));
             Matrix<byte> mMultiplied = m.Mul(2.0);

             for (int i = 0; i < m.Rows; i++)
            for (int j = 0; j < m.Cols; j++)
               Assert.AreEqual(m[i, j] * 2, mMultiplied[i, j]);
        }
Exemplo n.º 2
0
        protected double GetSVDRotation(Matrix<double>[] p1, Matrix<double>[] p2, out Matrix<double> rotation)
        {
            var centr1 = GetCentroid(p1);
            var centr2 = GetCentroid(p2);

            var q1 = new List<Matrix<double>>();
            var q2 = new List<Matrix<double>>();
            var H = new Matrix<double>(3, 3);

            for (int i = 0; i < Math.Min(p1.Count(), p2.Count()); ++i)
            {
                var q1d = new Matrix<double>(new double[,]
                {
                    {p1[i][0, 0] - centr1[0, 0]},
                    {p1[i][1, 0] - centr1[1, 0]},
                    {p1[i][2, 0] - centr1[2, 0]}
                });

                q1.Add(q1d);

                var q2d = new Matrix<double>(new double[,]
                {
                    {p2[i][0, 0] - centr2[0, 0]},
                    {p2[i][1, 0] - centr2[1, 0]},
                    {p2[i][2, 0] - centr2[2, 0]}
                });

                q2.Add(q2d);

                H = H.Add(q1d.Mul(q2d.Transpose()));
            }

            var U = new Matrix<double>(3, 3);
            var W = new Matrix<double>(3, 3);
            var V = new Matrix<double>(3, 3);

            CvInvoke.cvSVD(H, W, U, V, SVD_TYPE.CV_SVD_DEFAULT);

            var X = V.Mul(U.Transpose());

            var detX = CvInvoke.cvDet(X);

            rotation = X;
            return detX;
        }
Exemplo n.º 3
0
        public Matrix<double>[] ReprojectTo3d(Image<Gray, byte> leftImg, double[] disps, PointF[] points, Matrix<double> Q)
        {
            var res = new Matrix<double>[points.Count()];
            for (int i = 0; i < points.Count(); ++i)
            {
                if (points[i].X >= leftImg.Width || points[i].X < 0 || points[i].Y >= leftImg.Height || points[i].Y < 0)
                {
                    res[i] = null;
                    continue;
                }

                //
                if (disps[i] <= 0)
                {
                    res[i] = null;
                    continue;
                }

                var p = new Matrix<double>(new double[,]
                {
                    {points[i].X},
                    {points[i].Y},
                    {disps[i]},
                    {1}
                });

                var ph3d = Q.Mul(p);
                if (ph3d[3, 0] == 0)
                {
                    res[i] = null;
                    continue;
                }

                res[i] = new Matrix<double>(new double[,]
                    {
                        {ph3d[0, 0] / ph3d[3, 0]},
                        {ph3d[1, 0] / ph3d[3, 0]},
                        {ph3d[2, 0] / ph3d[3, 0]}
                    });
            }
            return res;
        }
Exemplo n.º 4
0
        public Matrix<double> GetGyroRotationMatrix(ReadingsVector3f gyroVect, double gyroSpeedMul)
        {
            Matrix<double> res = new Matrix<double>(3, 3);
            if (this.OldGyroReadings == null)
            {
                res.SetIdentity();
            }
            else
            {
                long diffMS = gyroVect.TimeStampI - this.OldGyroReadings.TimeStampI;

                double xr = -((double)gyroVect.Values[0] * diffMS) / 1000 * gyroSpeedMul;
                double yr = ((double)gyroVect.Values[1] * diffMS) / 1000 * gyroSpeedMul;
                double zr = -((double)gyroVect.Values[2] * diffMS) / 1000 * gyroSpeedMul;

                double sinxr = Math.Sin(xr);
                double cosxr = Math.Cos(xr);

                double sinyr = Math.Sin(yr);
                double cosyr = Math.Cos(yr);

                double sinzr = Math.Sin(zr);
                double coszr = Math.Cos(zr);

                Matrix<double> xM = new Matrix<double>(new double[,]
                    {
                        {1,   0,      0    },
                        {0,   cosxr,  sinxr},
                        {0,   -sinxr, cosxr}
                    });

                Matrix<double> yM = new Matrix<double>(new double[,]
                    {
                        {cosyr,  0,   sinyr},
                        {0,      1,   0    },
                        {-sinyr, 0,   cosyr}
                    });

                Matrix<double> zM = new Matrix<double>(new double[,]
                    {
                        {coszr,  sinzr, 0},
                        {-sinzr, coszr, 0},
                        {0,      0,     1}
                    });

                res = xM.Mul(yM).Mul(zM);
            }

            return res;
        }
Exemplo n.º 5
0
        public static Matrix<double> SolveLeastSquares(Matrix<double> A, Matrix<double> b)
        {
            int M = A.Rows;
            int N = A.Cols;

            var x = new Matrix<double>(N, 1);

            var U = new Matrix<double>(M, M);
            var F = new Matrix<double>(M, N);
            var V = new Matrix<double>(N, N);

            var c = new Matrix<double>(M, 1);
            var y = new Matrix<double>(N, 1);

            CvInvoke.cvSVD(A.Ptr, F.Ptr, U.Ptr, V.Ptr, SVD_TYPE.CV_SVD_DEFAULT);

            c = U.Transpose().Mul(b);

            for (int i = 0; i < N; i++)
            {
                y[i, 0] = c[i, 0]/F[i, i];
            }

            x = V.Mul(y);

            return x;
        }
Exemplo n.º 6
0
        public double[][] GetRotationMatrixBetweenTwoStates(double[][] state1, double[][] state2, Matrix<double> calibMatrix)
        {
            var size = new Size(state1.Length, state1.Length);
            var state1M = calibMatrix.Mul(Utils.CvHelper.ArrayToMatrix(state1, size));
            var state2M = calibMatrix.Mul(Utils.CvHelper.ArrayToMatrix(state2, size));

            var state1MInv = new Matrix<double>(state1M.Size);
            CvInvoke.cvInvert(state1M, state1MInv, SOLVE_METHOD.CV_LU);

            //var diff = state1MInv.Mul(state2M);

            //var res = state1M.Mul(diff).Mul(state1MInv);

            var res = state2M.Mul(state1MInv);

            return Utils.CvHelper.MatrixToArray(res);
        }