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]); }
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; }
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; }
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; }
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; }
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); }