/// <summary> /// 縦ベクトル用の4x4平行移動行列を作成します /// </summary> /// <param name="translation"></param> /// <returns></returns> public static CvMat GetTranslation(CvPoint3D64f translation) { CvMat ret = CvMat.Identity(4, 4, MatrixType.F64C1); ret[0, 3] = translation.X; ret[1, 3] = translation.Y; ret[2, 3] = translation.Z; return(ret); }
/// <summary> /// マトリックスを拡大します。新しく追加された要素は、対角成分ならば1、さもなくば0になります /// </summary> /// <param name="src">元のマトリックス</param> /// <param name="rows">新しいマトリックスの行数</param> /// <param name="cols">新しいマトリックスの列数</param> /// <returns></returns> public static CvMat GetExtendedTransformMat(CvMat src, int rows, int cols) { CvMat ret = CvMat.Identity(rows, cols, MatrixType.F64C1); for (int i = 0; i < src.Cols && i < cols; i++) { for (int j = 0; j < src.Rows && j < rows; j++) { ret[i, j] = src[i, j]; } } return(ret); }
public CalibrateStereoCamera() { // target filenames string[] pair = { "Data/Image/Calibration/left{0:D2}.jpg", "Data/Image/Calibration/right{0:D2}.jpg" }; string[][] fileNames = new string[ImageNum][]; for (int i = 0; i < ImageNum; i++) { fileNames[i] = new string[2] { string.Format(pair[0], i + 1), string.Format(pair[1], i + 1) }; } // FindChessboardCorners CvPoint2D32f[] imagePointsLeft, imagePointsRight; int[] pointCountLeft, pointCountRight; int[] goodImagelist; FindChessboardCorners(fileNames, out imagePointsLeft, out imagePointsRight, out pointCountLeft, out pointCountRight, out goodImagelist); int nImages = goodImagelist.Length; // StereoCalibrate CvPoint3D32f[, ,] objects = new CvPoint3D32f[ImageNum, BoardSize.Height, BoardSize.Width]; for (int i = 0; i < ImageNum; i++) { for (int j = 0; j < BoardSize.Height; j++) { for (int k = 0; k < BoardSize.Width; k++) { objects[i, j, k] = new CvPoint3D32f(j * SquareSize, k * SquareSize, 0.0f); } } } CvMat objectPoints = new CvMat(AllPoints, 3, MatrixType.F32C1, objects); CvMat imagePoints1 = new CvMat(AllPoints, 1, MatrixType.F32C2, imagePointsLeft); CvMat imagePoints2 = new CvMat(AllPoints, 1, MatrixType.F32C2, imagePointsRight); CvMat pointCount1 = new CvMat(nImages, 1, MatrixType.S32C1, pointCountLeft); CvMat pointCount2 = new CvMat(nImages, 1, MatrixType.S32C1, pointCountRight); CvMat cameraMatrix1 = CvMat.Identity(3, 3, MatrixType.F64C1); CvMat cameraMatrix2 = CvMat.Identity(3, 3, MatrixType.F64C1); CvMat distCoeffs1 = new CvMat(1, 4, MatrixType.F64C1); CvMat distCoeffs2 = new CvMat(1, 4, MatrixType.F64C1); CvMat R = new CvMat(3, 3, MatrixType.F64C1); CvMat T = new CvMat(3, 1, MatrixType.F64C1); Cv.StereoCalibrate(objectPoints, imagePoints1, imagePoints2, pointCount1, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, new CvSize(640, 480), R, T, null, null, new CvTermCriteria(100, 1e-5), CalibrationFlag.FixAspectRatio | CalibrationFlag.ZeroTangentDist | CalibrationFlag.SameFocalLength | CalibrationFlag.RationalModel | CalibrationFlag.FixK3 | CalibrationFlag.FixK4 | CalibrationFlag.FixK5); // Rectify CvMat R1 = new CvMat(3, 3, MatrixType.F64C1); CvMat R2 = new CvMat(3, 3, MatrixType.F64C1); CvMat P1 = new CvMat(3, 4, MatrixType.F64C1); CvMat P2 = new CvMat(3, 4, MatrixType.F64C1); CvMat Q = new CvMat(4, 4, MatrixType.F64C1); Cv.StereoRectify(cameraMatrix1, cameraMatrix2, distCoeffs1, distCoeffs2, new CvSize(640, 480), R, T, R1, R2, P1, P2, Q, StereoRectificationFlag.ZeroDisparity, 1, new CvSize(640, 480)); using (CvMemStorage mem = new CvMemStorage()) using (CvFileStorage fs = new CvFileStorage("extrinsic.yml", mem, OpenCvSharp.FileStorageMode.Write)) { fs.Write("R", R); fs.Write("T", T); fs.Write("R1", R1); fs.Write("R2", R2); fs.Write("P1", P1); fs.Write("P1", P1); fs.Write("Q", Q); } Process.Start("notepad", "extrinsic.yml"); Console.Read(); }
/// <summary> /// Returns centering matrix /// </summary> /// <param name="n">Size of matrix</param> /// <returns></returns> private CvMat CenteringMatrix(int n) { return(CvMat.Identity(n, n, MatrixType.F64C1) - new CvMat(n, n, MatrixType.F64C1, 1.0 / n)); }