opencv实现双目标定+立体匹配
双目标定
总体来说，opencv的标定精度不如GML Calibration Toolbox。但是GML不能标两相机外参啊。
相机内参标定
vector<vector<Point2f>> img_points_all;
vector<vector<Point3f>> obj_points_all;
Size img_size;
// 处理多幅图片，得到棋盘格点
for (int index = 0; index < file_num; index++)
{
// 读取图片
Mat img = imread(filename_pre + to_string(index) + file_extension);
img_size = img.size();
// 转为灰度图片，在cornerSub的时候要用
Mat gray;
cvtColor(img, gray, COLOR_BGR2GRAY);
// 找棋盘格点， pattern size是格点数，例如8x5的棋盘格，pattern size = (7,4)
vector<Point2f> corners;
findChessboardCorners(img, pattern_size, corners);
if (corners.size() != pattern_size.height * pattern_size.width)
{
cout << "in index " << index << ", corners found: " << corners.size() << "." << endl;
continue;
}
// 亚像素精度提取格点
TermCriteria criteria = TermCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 40, 0.01);
cornerSubPix(gray, corners, Size(5, 5), Size(-1, -1), criteria);
// 也可以用 find4QuadCornerSubpix(gray, corners, Size(5, 5));
// 但是感觉精度没有cornerSubPix高，不知道有什么区别
// 构建棋盘格点在真是世界的3d坐标，以第一个格点为坐标原点，棋盘格平面为xy平面
img_points_all.push_back(corners);
vector<Point3f> obj_points;
for (int y = 0; y < pattern_size.height; y++)
{
for (int x = 0; x < pattern_size.width; x++)
{
Point3f p;
p.x = x * square_w;
p.y = y * square_h;
p.z = 0;
obj_points.push_back(p);
}
}
obj_points_all.push_back(obj_points);
}
// 求相机内参
TermCriteria criteria = TermCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 30, DBL_EPSILON);
calibrateCamera(obj_points_all, img_points_all, img_size, camera_matrix, dist_coeffs, r_mats, t_mats, CALIB_FIX_K3 + CALIB_FIX_K4 + CALIB_FIX_K5, criteria);
相机外参标定
Mat R, T, E, F;
stereoCalibrate(obj_points_all, // input - 棋盘格点在世界坐标系下的坐标
img_points_left, // input - 左相机棋盘格点坐标
img_points_right, // input - 右相机棋盘格点坐标
camera_matrix_left, // in/out - 左相机内参矩阵
dist_coeffs_left, // in/out - 左相机畸变参数
camera_matrix_right, // in/out - 右相机内参矩阵
dist_coeffs_right, // in/out - 右相机畸变参数
img_size, // input - 图像尺寸
R, // output - 两相机旋转矩阵
T, // output - 两相机位移矩阵
E, // output - 本质矩阵
F, // output - 基础矩阵
CALIB_FIX_K3 + CALIB_FIX_K4 + CALIB_FIX_K5); // input - 标志位
// 如果左右相机分辨率不一样，可以设置标志位为CALIC_FIX_INTRINSIC, 这时候函数会
// 忽略img_size, 直接使用输入的相机内参
相机矫正
// 计算极线对齐参数
Mat R1, R2, P1, P2, Q;
Rect validRoi[2]; // validRoi中存放着矫正后的图像有效区域
stereoRectify(camera_matrix_left, dist_coeffs_left,
camera_matrix_right, dist_coeffs_right,
img_size, R, T, R1, R2, P1, P2, Q,
CALIB_ZERO_DISPARITY, 1, img_size, &validRoi[0], &validRoi[1]);
// 计算矫正投影矩阵
Mat rmap[2][2];
initUndistortRectifyMap(camera_matrix_left, dist_coeffs_left, R1, P1, img_size, CV_16SC2, rmap[0][0], rmap[0][1]);
initUndistortRectifyMap(camera_matrix_right, dist_coeffs_right, R2, P2, img_size, CV_16SC2, rmap[1][0], rmap[1][1]);
Mat img1[2];
imread("left.png", img[0]);
imread("right.png", img[1]);
// remap
Mat imgl, imgr;
remap(img[0], imgl, rmap[0][0], rmap[0][1], INTER_LINEAR);
remap(img[1], imgr, rmap[1][0], rmap[1][1], INTER_LINEAR);
立体匹配
Mat imgl, imgr;
int x = validRoi[0].x > validRoi[1].x ? validRoi[0].x : validRoi[1].x;
int y = validRoi[0].y > validRoi[1].y ? validRoi[0].y : validRoi[1].y;
int width = 800;
int height = 400;
imgl = imgl(Rect(x, y, width, height));
imgr = imgr(Rect(x, y, width, height));
// SGBM 立体匹配
int numberOfDisparities = ((img_size.width / 8) + 15) & -16;
cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(0, 16, 3);
sgbm->setPreFilterCap(63);
int SADWindowSize = 9;
int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3;
sgbm->setBlockSize(sgbmWinSize);
int cn = img1[0].channels();
sgbm->setP1(8 * cn*sgbmWinSize*sgbmWinSize);
sgbm->setP2(32 * cn*sgbmWinSize*sgbmWinSize);
sgbm->setMinDisparity(0);
sgbm->setNumDisparities(numberOfDisparities);
sgbm->setUniquenessRatio(10);
sgbm->setSpeckleWindowSize(100);
sgbm->setSpeckleRange(32);
sgbm->setDisp12MaxDiff(1);
Mat disp;
if (alg == 1)
sgbm->setMode(cv::StereoSGBM::MODE_HH);
else if (alg == 2)
sgbm->setMode(cv::StereoSGBM::MODE_SGBM);
else if (alg == 3)
sgbm->setMode(cv::StereoSGBM::MODE_SGBM_3WAY);
sgbm->compute(imgl, imgr, disp);