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