プログラム関連の個人的メモ

ステレオカメラの平行化
http://blog.livedoor.jp/juggleryou/archives/501137...

• 内部パラメータ

cv::calibrateCamera

• 二台のカメラ間の変換行列

cv::stereoCalibrate

• 平行化変換(回転行列3x3)とその後の射影行列3x4を求める

cv::stereoRectify

• 内部パラメータと平行化行列から歪補正と平行化をあわせたマップを求める

cv::initUndistortRectifyMap

• マップから画像を変換する

cv::remap

// 外部パラメータの計算
cv::Mat R, T, E, F;
cv::TermCriteria criteria{ 10000, 10000, 0.0001 };
cv::stereoCalibrate(worldPoints, imagePoints2, imagePoints,
	intrinsicv2, distortionv2, intrinsicv1, distortionv1,
	checkerImgs[0].size(), R, T, E, F, criteria, CV_CALIB_FIX_INTRINSIC);

// 外部パラメータを書き込む
cv::Mat ros;
cv::Rodrigues(R, ros);
FILE *fp;
fopen_s(&fp, "C:\\TempData\\CV_caliPoint\\gaibu\\C1toC2.txt", "w");
for (int jj = 0; jj < 3; jj++)
fprintf_s(fp, "%f,%f,%f,%f\n", R.at<double>(jj, 0),
	R.at<double>(jj, 1), R.at<double>(jj, 2), T.at<double>(jj, 0));
fclose(fp);

cv::Mat R1(3,3,CV_64F), R2(3,3,CV_64F);
cv::Mat P1(3,4,CV_64F), P2(3,4,CV_64F);
cv::Mat Q;
cv::Size s = image.size();

// 平行化後の内部パラメータやら計算
cv::stereoRectify(intrinsicv1, distortionv1, intrinsicv2,
	distortionv2, s, R, T, R1, R2,
	P1, P2, Q, CV_CALIB_ZERO_DISPARITY, 0, s);

cv::Mat mapX1, mapY1;
cv::Mat mapX2, mapY2;

// 平行化のための画素対応を求める
cv::initUndistortRectifyMap(intrinsicv1, distortionv1, R1, P1, s, CV_32FC1, mapX1, mapY1);
cv::initUndistortRectifyMap(intrinsicv2, distortionv2, R2, P2, s, CV_32FC1, mapX2, mapY2);

１では、チェッカーボードを撮影して、内部パラメータと外部パラメータを求めている。
cv::stereoCalibrateが外部パラメータ求める関数。

２では、平行化したときのステレオカメラの内部パラメータと外部パラメータを計算したあと、
その通りに撮影したかのような画像を作るための画素対応マップを作っている。
cv::stereoRectifyはcv::GetOptimalNewCameraMatrixに近いかな。

平行化によって、エピポーラ線がy軸並行となったので、対応がとりやすくなった。
三次元座標を計算するのも楽になっている。

cv::Point3d pos;
double temp = vec2.x - vec1.x;
pos.z = camera_distance * camera_focus / temp;
pos.x = pos.z * (vec1.x - cx) / camera_focus;
pos.y = -pos.z * (-vec1.y + cy) / camera_focus;

cemera_distanceはステレオカメラ間の距離。
camera_focusは焦点距離(内部パラメータのfx,fyの部分)。
cx,cyは中心からのずれ(これも内部パラメータ)。

ちなみに平行化をしなくても、カメラが二つあれば三次元座標を求めることは可能。
計算ぐちゃぐちゃがんばれば式が作れるのでがんばれ。