3D视觉(三)：双目摄像头的标定与校正
对于双目摄像头而言，除了需要分别标定左目摄像头的内参矩阵K1、畸变系数D1、右目摄像头的内参矩阵K2、畸变系数D2，还需要标定左右目对应的旋转矩阵R和平移向量T。当双目摄像头固定在一个平面上时，旋转矩阵R可近似为一个单位阵，平移向量T的欧式范数即为基线长度b。
我们可以把两个相机都看作针孔相机，它们是水平放置的，意味着两个相机的光圈中心都位于x轴上，两者之间的距离称为双目相机的基线b，它是双目相机的重要参数。通过粗略测量可看出，这里基线b的长度在0.06m-0.07m之间，后面标定得到的估计结果为0.0696m。
一、双目相机模型
双目相机一般由左目和右目两个水平放置的相机组成。当然也可以做成上下两目，但日常见到的主流双目都是做成左右的。两个相机的光圈中心都位于x轴上，它们的距离称为双目相机的基线 Baseline，记作b。
考虑一个空间点P ，它在左目和右目各成一像，记作 PL、PR。由于相机基线的存在，这两个成像位置是不同的。理想情况下，由于左右相机只有在x轴上才有位移，因此P的成像也只有在图像的u轴上才有差异。我们记它在左侧的坐标为uL，在右侧的坐标为uR。
根据三角形P-PL-PR和三角形P-OL-OR的相似关系，可以得到：
稍加整理可以得到：
这里d为左右图的横坐标之差，也称视差disparty。根据视差，在f和b已知的情况下，我们可以估计目标点离相机的深度距离。视差与深度距离成反比，视差越大，距离越近。另外由于视差最小为 一个像素，于是双目的深度存在一个理论上的最大值。可以看到，当基线越长时，双目最大能测到的距离就会变远；反之，小型双目器件则只能测量很近的距离。
虽然由视差计算深度的公式很简洁， 但视差d本身的计算却比较困难，我们需要确切地知道左眼图像某个像素出现在右眼图像的哪一个位置（即对应关系）。此外如果想计算每个像素的深度，其计算量与精度都将成为问题，并且只有在图像纹理变化丰富的地方才能计算视差。
二、双目相机标定流程
第1步：准备一张棋盘格，粘贴于墙面，并用直尺测量黑白方格的真实物理长度。
第2步：调用双目摄像头，分别从不同角度拍摄得到一系列棋盘格图像。
第3步：利用左目图片数据集，进行左目相机标定，得到左目内参矩阵K1、左目畸变系数向量D1。
第4步：利用右目图片数据集，进行右目相机标定，得到右目内参矩阵K2、右目畸变系数向量D2。
第5步：将左右目测量得到的参数K1、K2、D1、D2作为输入，再同时利用左右目一一对应好的棋盘格图片，调用stereoCalibrate函数，输出左右目的旋转矩阵R、平移向量T。
第6步：基于测量得到的K1、K2、D1、D2、R、T，进行双目视觉的图像校正。
备注：需要用直尺测量黑白方格的真实物理长度，因为我们会将真实世界的棋盘格3D坐标，以相同的尺度(米为单位)，存储于object_points容器中，这样求解得到的平移向量T才会有意义，它的尺度才会和真实世界尺度相对应。然后我们会借助OpenCV棋盘格检测函数，将每张图片对应的棋盘格坐标索引，对应存储于left_img_points、 right_img_points容器中。在得到一系列对应好的2D、3D坐标点后，就可通过线性方程求解参数估计。
三、实验过程
准备一张棋盘格，内点size分别是6和4，直尺测量黑白方格的长度为0.0405米。
分别从不同角度拍摄得到200张棋盘格图像，将左目、右目图片对应存储在1和2文件夹中：
先对左目摄像头进行标定，得到内参矩阵K1、畸变系数向量D1：
再对右目摄像头进行标定，得到内参矩阵K2、畸变系数向量D2：
将K1、K2、D1、D2作为输入参数，进行双目视觉标定，得到左、右目摄像头对应的旋转矩阵R、平移向量T：
最后进行双目视觉图像校正，原始双目图像为：
校正后的双目图像为：
可以看出，经过校正后图像不光被去畸变，而且被投影到了与相机平行的角度。
四、源码
双目视觉标定：
#include <opencv2/core/core.hpp>
// core是OpenCV的主要头文件，包括数据结构，矩阵运算，数据变换，内存管理，文本和数学等功能
#include <opencv2/calib3d/calib3d.hpp>
// calib3d模块主要是相机校准和三维重建相关的内容，包括基本的多视角几何算法，单个立体摄像头标定，物体姿态估计，立体相似性算法，3D信息的重建等
#include <opencv2/highgui/highgui.hpp>
// highgui模块，高层GUI图形用户界面，包括媒体的I/O输入输出、视频捕捉、图像和视频的编码解码、图形交互界面的接口等内容
#include <opencv2/imgproc/imgproc.hpp>
// imgproc模块，图像处理模块，包括线性和非线性的图像滤波、图像的几何变换、特征检测等
#include <stdio.h>
#include <iostream>
#include "opencv2/imgcodecs/legacy/constants_c.h"
// OpenCV4.5.3版本，解决未定义标识符"CV_LOAD_IMAGE_COLOR"
#include <opencv2/imgproc/types_c.h>
// OpenCV4.5.3版本，解决未定义标识符"CV_L2"
using namespace std;
using namespace cv;
// obj_points中每个元素都是一个小vector，每个小vector存储的每个元素都是opencv的cv::Point3f数据结构
vector< vector< Point3f > > object_points;
// image_points1、imagePoints2中每个元素都是一个小vector，每个小vector存储的每个元素都是opencv的cv::Point2f数据结构
vector< vector< Point2f > > imagePoints1, imagePoints2;
// corners1是一个小vector，存储左目图片中检测出的所有的棋盘格内点索引
// corners2是一个小vector，存储右目图片中检测出的所有的棋盘格内点索引
vector< Point2f > corners1, corners2;
// left_img_points, right_img_points的作用和image_points1、imagePoints2相同
// 这里重复赋值一次，是为了保证两个容器存储的样本数相同，正好一一对应
vector< vector< Point2f > > left_img_points, right_img_points;
// img表示rgb图像，gray表示对应的灰度图
Mat img1, img2, gray1, gray2;
/*
* board_width： 棋盘格宽方向上的内点数量，此处为9
* board_height： 棋盘格高方向上的内点数量，此处为6
* num_imgs： 图片数量
* square_size： 黑白格真实世界中的边长，单位为米，此处为0.02423米
* leftimg_dir： 左目图片存储文件夹位置，此处为"../calib_imgs/1/"
* rightimg_dir： 右目图片存储文件夹位置，此处为"../calib_imgs/1/"
* leftimg_filename： 左目相机拍摄的图片名称，此处为"left"
* rightimg_filename： 右目相机拍摄的图片名称，此处为"right"
* extension： 图片后缀，此处为"jpg"
*/
void load_image_points(int board_width, int board_height, int num_imgs, float square_size,
char* leftimg_dir, char* rightimg_dir,
char* leftimg_filename, char* rightimg_filename, char* extension) {
Size board_size = Size(board_width, board_height);
int board_n = board_width * board_height;
int all = 0;
for (int k = 1; k <= num_imgs; k++) {
char left_img[1000], right_img[1000];
// 该函数包含在stdio.h的头文件中，函数功能与printf函数的功能基本一样，将结果输出到指定的字符串中
// ../calib_imgs/1/left1.jpg
// ../calib_imgs/1/right1.jpg
sprintf(left_img, "%s%s%d.%s", leftimg_dir, leftimg_filename, k, extension);
sprintf(right_img, "%s%s%d.%s", rightimg_dir, rightimg_filename, k, extension);
cout << left_img << endl;
cout << right_img << endl;
img1 = imread(left_img, CV_LOAD_IMAGE_COLOR);
img2 = imread(right_img, CV_LOAD_IMAGE_COLOR);
if(img1.data==nullptr | img2.data==nullptr)
{
continue;
}
// cv::imshow("b", img1 - img2);
// cv::waitKey(0);
cvtColor(img1, gray1, CV_BGR2GRAY);
cvtColor(img2, gray2, CV_BGR2GRAY);
bool found1 = false, found2 = false;
// 棋盘格检验
found1 = cv::findChessboardCorners(img1, board_size, corners1);
found2 = cv::findChessboardCorners(img2, board_size, corners2);
// 必须要求左目图像、右目图像都检测到棋盘格
if(!found1 || !found2){
cout << "棋盘格未能找到!" << endl;
continue;
}
all = all + 1;
cout << "The total: " << all << endl;
if (found1)
{
// 进一步refine检测到的网格内点的坐标精度
cv::cornerSubPix(gray1, corners1, cv::Size(5, 5), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1));
// 作图，可视化
drawChessboardCorners(img1, board_size, corners1, found1);
cv::imshow("left", img1);
cv::waitKey(10);
}
if (found2)
{
// 进一步refine检测到的网格内点的坐标精度
cv::cornerSubPix(gray2, corners2, cv::Size(5, 5), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1));
// 作图，可视化
cv::drawChessboardCorners(img2, board_size, corners2, found2);
cv::imshow("right", img2);
cv::waitKey(10);
}
// 将棋盘格真实3D坐标点，依次append进入obj中，这里需要将索引乘以棋盘格边长(0.02423米)，以固定真实世界尺度
vector< Point3f > obj;
for (int i = 0; i < board_height; i++)
for (int j = 0; j < board_width; j++)
obj.push_back(Point3f((float)j * square_size, (float)i * square_size, 0));
if (found1 && found2) {
cout << "第" << k << "张图像，左右目均已经检测到棋盘格内点!" << endl;
imagePoints1.push_back(corners1);
imagePoints2.push_back(corners2);
object_points.push_back(obj);
}
}
// 这里重复赋值一次，是为了保证两个容器存储的样本数相同，正好一一对应
for (int i = 0; i < imagePoints1.size(); i++) {
vector< Point2f > v1, v2;
for (int j = 0; j < imagePoints1[i].size(); j++) {
v1.push_back(Point2f((double)imagePoints1[i][j].x, (double)imagePoints1[i][j].y));
v2.push_back(Point2f((double)imagePoints2[i][j].x, (double)imagePoints2[i][j].y));
}
left_img_points.push_back(v1);
right_img_points.push_back(v2);
}
}
int main(int argc, char const *argv[])
{
int board_width = 6;
int board_height = 4;
int num_imgs = 200;
float square_size = 0.0405;
char* leftimg_dir = "../calib_imgs/1/";
char* rightimg_dir = "../calib_imgs/2/";
char* leftimg_filename = "left";
char* rightimg_filename = "right";
char* extension = "jpg";
// 将一系列2D观测坐标和3D观测坐标，对应存储到object_points, image_points中
load_image_points(board_width, board_height, num_imgs, square_size,
leftimg_dir, rightimg_dir, leftimg_filename, rightimg_filename, extension);
// 这是根据calib_left.cpp、calib_right.cpp标定得到的内参矩阵K1、K2，和畸变系数向量D1、D2
double K_left[3][3] = {
662.3562273563088, 0, 312.6263091035918, 0, 662.9296902690498, 258.9996285827844, 0, 0, 1};
Mat K1 = cv::Mat(3, 3, cv::DataType<double>::type, K_left);
double d_left[1][5] = {
0.06966962838870275, 0.02054263655123773, 0.001252584212211826, 0.002089077085379777, -0.4096320330693385};
Mat D1 = cv::Mat(1, 5, cv::DataType<double>::type, d_left);
double K_right[3][3] = {
647.3402626821477, 0, 298.8766921846282, 0, 647.739941990085, 259.9519313557778, 0, 0, 1};
Mat K2 = cv::Mat(3, 3, cv::DataType<double>::type, K_right);
double d_right[1][5] = {
0.02272045036297292, -0.5565235313790773, 0.007380600944678045, -0.007607934580409265, 1.30443069011335};
Mat D2 = cv::Mat(1, 5, cv::DataType<double>::type, d_right);
// 左目摄像头和右目摄像头之间的旋转矩阵R、平移向量T，E是本质矩阵，F是基础矩阵
Mat R, F, E;
Vec3d T;
cout << "相机左目、右目参数：" << endl;
cout << K1 << endl;
cout << D1 << endl;
cout << K2 << endl;
cout << D2 << endl;
// 双目相机标定，object_points, left_img_points, right_img_points, K1, D1, K2, D2, img1.size都是传入参数，R, T, E, F是输出结果
stereoCalibrate(object_points, left_img_points, right_img_points, K1, D1, K2, D2, img1.size(), R, T, E, F);
cout << "双目标定结果：" << endl;
cout << R << endl;
cout << T << endl;
cout << E << endl;
cout << F << endl;
// 还可以进一步进行双目校正
cv::Mat R1, R2, P1, P2, Q;
stereoRectify(K1, D1, K2, D2, img1.size(), R, T, R1, R2, P1, P2, Q);
cout << "进一步双目校正结果：" << endl;
cout << R1 << endl;
cout << P1 << endl;
cout << R2 << endl;
cout << P2 << endl;
return 0;
}
// [0.9997826605620699, 0.004051019540949904, 0.02045044938645102;
// -0.004074743115292153, 0.999991072656389, 0.001118515117933302;
// -0.02044573569166274, -0.001201602348328341, 0.9997902420227071]
// [-0.0676242, -0.0119106, -0.0116169]
双目视觉校正：
#include <opencv2/core/core.hpp>
// core是OpenCV的主要头文件，包括数据结构，矩阵运算，数据变换，内存管理，文本和数学等功能
#include <opencv2/calib3d/calib3d.hpp>
// calib3d模块主要是相机校准和三维重建相关的内容，包括基本的多视角几何算法，单个立体摄像头标定，物体姿态估计，立体相似性算法，3D信息的重建等
#include <opencv2/highgui/highgui.hpp>
// highgui模块，高层GUI图形用户界面，包括媒体的I/O输入输出、视频捕捉、图像和视频的编码解码、图形交互界面的接口等内容
#include <opencv2/imgproc/imgproc.hpp>
// imgproc模块，图像处理模块，包括线性和非线性的图像滤波、图像的几何变换、特征检测等
#include <stdio.h>
#include <iostream>
#include "opencv2/imgcodecs/legacy/constants_c.h"
// OpenCV4.5.3版本，解决未定义标识符"CV_LOAD_IMAGE_COLOR"
using namespace std;
using namespace cv;
int main(int argc, char const *argv[])
{
Mat img1 = imread("../calib_imgs/1/left0.jpg", CV_LOAD_IMAGE_COLOR);
Mat img2 = imread("../calib_imgs/2/right0.jpg", CV_LOAD_IMAGE_COLOR);
cv::imshow("left1.jpg", img1);
cv::imshow("right1.jpg", img2);
cv::waitKey(0);
// 这些是根据calib_left.cpp、calib_right.cpp、calib_stereo.cpp得到的参数
// K1、K2是左目、右目的内参矩阵，D1、D2是左目、右目的畸变系数向量
// R、T是左目摄像头和右目摄像头之间的旋转矩阵、平移向量
double K_left[3][3] = {
662.3562273563088, 0, 312.6263091035918, 0, 662.9296902690498, 258.9996285827844, 0, 0, 1};
Mat K1 = cv::Mat(3, 3, cv::DataType<double>::type, K_left);
double d_left[1][5] = {
0.06966962838870275, 0.02054263655123773, 0.001252584212211826, 0.002089077085379777, -0.4096320330693385};
Mat D1 = cv::Mat(1, 5, cv::DataType<double>::type, d_left);
double K_right[3][3] = {
647.3402626821477, 0, 298.8766921846282, 0, 647.739941990085, 259.9519313557778, 0, 0, 1};
Mat K2 = cv::Mat(3, 3, cv::DataType<double>::type, K_right);
double d_right[1][5] = {
0.02272045036297292, -0.5565235313790773, 0.007380600944678045, -0.007607934580409265, 1.30443069011335};
Mat D2 = cv::Mat(1, 5, cv::DataType<double>::type, d_right);
double R_stereo[3][3] = {
0.9997826605620699, 0.004051019540949904, 0.02045044938645102,-0.004074743115292153, 0.999991072656389, 0.001118515117933302,-0.02044573569166274, -0.001201602348328341, 0.9997902420227071};
Mat R = cv::Mat(3, 3, cv::DataType<double>::type, R_stereo);
Vec3d T = {
-0.0676242, -0.0119106, -0.0116169};
cout << "双目相机参数：" << endl;
cout << K1 << endl;
cout << D1 << endl;
cout << K2 << endl;
cout << D2 << endl;
cout << R << endl;
cout << T << endl;
// 进一步进行双目校正，这里stereoRectify函数必须接受double类型的Mat，否则会报错：
// -205:Formats of input arguments do not match) All the matrices must have the same data type in function 'cvRodrigues2'
cv::Mat R1, R2, P1, P2, Q;
stereoRectify(K1, D1, K2, D2, img1.size(), R, T, R1, R2, P1, P2, Q);
cout << "进一步双目校正结果：" << endl << endl;
cout << R1 << endl;
cout << P1 << endl;
cout << R2 << endl;
cout << P2 << endl;
// 进行图像校正
cv::Mat lmapx, lmapy, rmapx, rmapy;
cv::Mat imgU1, imgU2;
cv::initUndistortRectifyMap(K1, D1, R1, P1, img1.size(), CV_32F, lmapx, lmapy);
cv::initUndistortRectifyMap(K2, D2, R2, P2, img2.size(), CV_32F, rmapx, rmapy);
cv::remap(img1, imgU1, lmapx, lmapy, cv::INTER_LINEAR);
cv::remap(img2, imgU2, rmapx, rmapy, cv::INTER_LINEAR);
cv::imshow("left.jpg", imgU1);
cv::imshow("right.jpg", imgU2);
cv::waitKey(0);
imwrite("left.jpg", imgU1);
imwrite("right.jpg", imgU2);
return 0;
}
五、项目链接
如果代码跑不通，或者想直接使用我自己制作的数据集，可以去下载项目链接：
https://blog.csdn.net/Twilight737