/// <summary> /// Расчитывает матрицу перехода /// Взято здесь http://jepsonsblog.blogspot.ru/2012/11/rotation-in-3d-using-opencvs.html /// </summary> private void calculateTransformationMatrix(CoordinatesTransformer transformer) { // Матрица поворота 3x3 CvMat R3x3 = new CvMat(3, 3, MatrixType.F64C1); Cv.Rodrigues2(transformer.Rotation, R3x3); // Произведение матрицы поворота и матрицы переноса CvMat RxT = new CvMat(4, 4, MatrixType.F64C1, new double[, ] { { R3x3[0, 0], R3x3[0, 1], R3x3[0, 2], transformer.Translation[0, 0] }, { R3x3[1, 0], R3x3[1, 1], R3x3[1, 2], transformer.Translation[0, 1] }, { R3x3[2, 0], R3x3[2, 1], R3x3[2, 2], transformer.Translation[0, 2] }, { 0, 0, 0, 1 }, }); CvMat A1 = new CvMat(4, 3, MatrixType.F64C1, new double[, ] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 0 }, { 0, 0, 1 } }); CvMat A2 = new CvMat(3, 4, MatrixType.F64C1, new double[, ] { { transformer.Intrinsic[0, 0], 0, transformer.Intrinsic[0, 2], 0 }, { 0, transformer.Intrinsic[1, 1], transformer.Intrinsic[1, 2], 0 }, { 0, 0, 1, 0 }, }); CvMat InvTransformationMatrix = (A2 * (RxT * A1)); InvTransformationMatrix.Invert(transformer.TransformationMatrix); }
/// <summary> /// 最小二乗法で解を得ます /// </summary> /// <returns></returns> public double[] Solve() { CvMat invLeft = CvEx.InitCvMat(_left); _left.Invert(invLeft, InvertMethod.Cholesky); CvMat ret = invLeft * _right; return(ret.Select(r => r.Val0).ToArray()); }