写在开头
最近在学习三维重建的相关知识，打算将三维重建SFM的整个过程用代码的形式梳理一下，本章节主要实现相机标定代码。
这里我们假定你有一定的三维重建相关的基本知识，作者在这里推荐高翔博士的《视觉SLAM十四讲：从理论到实践》，在B站上有高翔博士的讲解视频。
程序流程
- 使用相机拍摄一系列的棋盘格的图片。
- 对每一个图片提取角点信息，对于角点信息不够精确，进一步提取亚像素角点信息；
- 在图片中提取出角点。
- 相机标定。
- 对标定结果评价，计算误差。
- 使用标定结果对原图片进行矫正。
步骤一：拍摄图片
标定图片一般来说需要我们在不同位置、不同角度、不同姿态下拍摄。在之前我们的数学推到中我们得知，一般来说我们需要至少3张图片才可以确定为一解。但是，通常我们使用10~20张图片来标定。
步骤二：提取角点信息和提取亚像素角点信息
OPENCV中提取棋盘格中内角点的函数：findChessboardCorners()
函数原型
CV_EXPORTS_W bool findChessboardCorners( InputArray image, Size patternSize, OutputArray corners,
int flags = CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE );
函数说明：
这个函数的功能是确定输入图像中是否有棋盘格图案，并检测棋盘格的内角点。如果所有的内角点都找到了，那么函数返回一个非0值；如果没有找到所有的内角点，就会返回0。
参数说明：
- image：输入的棋盘格图像，必须是8位的灰度或彩色图像。
- patternSize：每一个图片中，每行和每列角点的个数。注意，每行和每列的角点个数不能相同，因为这样不易辨别方向。
- corners：输入角点的坐标。通常用 cv:Point2f 向量来保存。
-
flags：默认为0。可选参数如下：
- CALIB_CB_ADAPTIVE_THRESH 使用自适应阈值（通过平均图像亮度计算得到）将图像转换为黑白图，而不是一个固定的阈值。
- CALIB_CB_NORMALIZE_IMAGE 在使用固定阈值或者自适应阈值进行二值化之前，先使用equalizeHist()来均衡化图像亮度。
- CALIB_CB_FILTER_QUADS 使用其他的准则（如轮廓面积，周长，方形形状）来去除在轮廓检测阶段检测到的错误方块。
有两个函数可以实现提取亚像素角点信息：cornerSubPix、find4QuadCornerSubpix。在提取棋盘格角点时两者的效果差不多，随便使用哪一个都行。
函数原型
void cornerSubPix(InputArray image, InputOutputArray corners, Size winSize, Size zeroZone, TermCriteria criteria)
参数说明
- image：输入图像。
- corners：初始的角点坐标，同时也会作为亚像素角点坐标的输出；通常用cv::Point2f/Point2d 向量来保存，vector<cv::Point2f/Point2d> points。
- winSize：大小为搜索窗口的一半。
- zeroZone：死区的一般尺寸，死区为不对搜索区的中央位置做求和运算的区域。
- criteria：迭代的终止条件。
函数原型
CV_EXPORTS bool find4QuadCornerSubpix( InputArray img, InputOutputArray corners, Size region_size )
参数说明
- img：输入图像，最好是8位灰度图像，检测效率更高；
- corners：初始的角点坐标，同时也会作为亚像素角点坐标的输出；通常用cv::Point2f/Point2d 向量来保存，vector<cv::Point2f/Point2d> points。
- region_size：角点搜索窗口的大小。
步骤三：画出角点
函数原型
CV_EXPORTS_W void drawChessboardCorners( InputOutputArray image, Size patternSize, InputArray corners, bool patternWasFound )
函数说明
画出检测的角点。
参数说明
- image：图像，8位灰度或彩色图像。
- patternSize：每一幅棋盘格图片中，每行和每列角点的个数。
- corners：初始的角点坐标，同时也会作为亚像素角点坐标的输出；通常用cv::Point2f/Point2d 向量来保存，vector<cv::Point2f/Point2d> points。
- patternWasFound：标志位，用来只是是否检测倒所有的棋盘内角点。true表示完整地检测到了所有内角点，函数会用直线将角点依次连接起来；false表示没有完整检测到所有内角点，函数会用红色圆圈标出检测到的内角点。
步骤四：相机标定
函数原型
CV_EXPORTS_W double calibrateCamera( InputArrayOfArrays objectPoints,
InputArrayOfArrays imagePoints, Size imageSize,
InputOutputArray cameraMatrix, InputOutputArray distCoeffs,
OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
int flags = 0, TermCriteria criteria = TermCriteria(
TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );
参数说明
- objectPoints：一系列点的三维坐标，即若干张图片中对应的若干个点的三维坐标。在使用时应该建立一个二维的vector，即vector<vector<Point3f>> objectPoints。我们需要根据棋盘格每个黑白格子的长宽，计算出各个内角点的三维坐标。通常我们会取z=0，而只计算x和y 坐标。
- imagePoints：若干张图片对应的若干的内角点的坐标，通常采用vector<vector<Point2f>> image_points表示。
- imageSize：图像的像素尺寸大小。
-
cameraMatrix：相机的内参矩阵，对应推导时的内参矩阵
，大小为
。
-
distCoeffs：相机的畸变参数矩阵，有5个畸变参数：
，矩阵大小为
。
- rvecs：旋转向量，罗德里格旋转向量，是相机外参；因为有若干张图片，所以通常使用Mat类型的vector 表示，vector<Mat> rvecs。
- tvecs：位移向量，与旋转向量一样，也是相机外参，通常使用Mat类型的vector 表示，vector<Mat> tvecs。
- flags：表示标定时采用的算法。默认为0，其他有：
- CV_CALIB_USE_INTRINSIC_GUESS：使用该参数时，在cameraMatrix矩阵中应该有fx,fy,u0,v0的估计值。否则的话，将初始化(u0,v0）图像的中心点，使用最小二乘估算出fx，fy。
- CV_CALIB_FIX_PRINCIPAL_POINT：在进行优化时会固定光轴点。当CV_CALIB_USE_INTRINSIC_GUESS参数被设置，光轴点将保持在中心或者某个输入的值。
- CV_CALIB_FIX_ASPECT_RATIO：固定fx/fy的比值，只将fy作为可变量，进行优化计算。当CV_CALIB_USE_INTRINSIC_GUESS没有被设置，fx和fy将会被忽略。只有fx/fy的比值在计算中会被用到。
- CV_CALIB_ZERO_TANGENT_DIST：设定切向畸变参数（p1,p2）为零。
- CV_CALIB_FIX_K1,…,CV_CALIB_FIX_K6：对应的径向畸变在优化中保持不变。
- CV_CALIB_RATIONAL_MODEL：计算k4，k5，k6三个畸变参数。如果没有设置，则只计算其它5个畸变参数。
-
criteria：迭代的终止条件。
这个函数解决的就是我们以前推导的极大似然优化问题：
其中：表示的是点
在第
个图像上的投影。
步骤五：评价标定结果
我们在进行相机标定时，本身要解决的是一个优化问题，而优化的对象就是角点与三维点投影到图像点坐标之间的差值，通过不断迭代，尽可能地最小化这个差值。我们对标定结果评价时，就是计算投影点与检测到的亚像素角点坐标的差值。由于是二维的，所以分别对x和y坐标求差值，再求平方根，即求L2范数。
函数原型
CV_EXPORTS_W void projectPoints( InputArray objectPoints,
InputArray rvec, InputArray tvec,
InputArray cameraMatrix, InputArray distCoeffs,
OutputArray imagePoints,
OutputArray jacobian = noArray(),
double aspectRatio = 0 );
参数说明
- objectPoints：一系列点的三维坐标，即若干张图片中对应的若干个点的三维坐标。在使用时应该建立一个二维的vector，即vector<vector<Point3f>> objectPoints。我们需要根据棋盘格每个黑白格子的长宽，计算出各个内角点的三维坐标。通常我们会取z=0，而只计算x和y 坐标。
- rvecs：旋转向量，罗德里格旋转向量，是相机外参；因为有若干张图片，所以通常使用Mat类型的vector 表示，vector<Mat> rvecs。
- tvecs：位移向量，与旋转向量一样，也是相机外参，通常使用Mat类型的vector 表示，vector<Mat> tvecs。
-
cameraMatrix：相机的内参矩阵，对应推导时的内参矩阵A，大小为
-
distCoeffs：相机的畸变参数矩阵，有5个畸变参数：
，矩阵大小为 - imagePoints：若干张图片对应的若干的内角点的坐标，通常采用vector<vector<Point2f>> image_points表示。
步骤六：矫正图像
使用前面求得的内参和外参以及畸变参数数据，可以对图像进行畸变矫正。使用initUndistortRectifyMap和remap两个函数来实现。initUndistortRectifyMap用来计算畸变映射，remap把求得的映射应用到图像上。
函数原型
CV_EXPORTS_W void initUndistortRectifyMap( InputArray cameraMatrix, InputArray distCoeffs,
InputArray R, InputArray newCameraMatrix,
Size size, int m1type, OutputArray map1, OutputArray map2 );
参数说明
- cameraMatrix：相机的内参矩阵；
- distCoeffs：相机的畸变参数构成的矩阵；
- R：可选的输入，是第一和第二相机坐标之间的旋转矩阵；
- newCameraMatrix：校正后的内参矩阵；
- size：摄像机采集的无失真的图像尺寸；
- m1type：定义map1的数据类型，可以是CV_32FC1或者CV_16SC2；
- map1和map2：分别对应X和Y坐标的重映射参数。
函数原型
CV_EXPORTS_W void remap( InputArray src, OutputArray dst,
InputArray map1, InputArray map2,
int interpolation, int borderMode = BORDER_CONSTANT,
const Scalar& borderValue = Scalar());
参数说明
- src：输入图像，原始有畸变的图像；
- dst：输出图像，校正后的图像；
- map1：X坐标的映射；
- map2：Y坐标的映射；
- interpolation：图像的插值方式；
- borderMode：边界填充方式；
Code
#include <iostream>
#include <vector>
#include <fstream>
#include <string>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
using namespace std;
using namespace cv;
int main()
{
ifstream inImgPath("calibdata.txt"); //标定所用图像文件的路径
vector<string> imgList;
vector<string>::iterator p;
string temp;
if (!inImgPath.is_open())
{
cout << "没有找到文件" << endl;
}
//读取文件中保存的图片文件路径，并存放在数组中
while (getline(inImgPath, temp))
{
imgList.push_back(temp);
}
ofstream fout("caliberation_result.txt"); //保存标定结果的文件
cout << "开始提取角点......" << endl;
cv::Size image_size;//保存图片大小
cv::Size pattern_size = cv::Size(4, 6);//标定板上每行、每列的角点数；测试图片中的标定板上内角点数为4*6
vector<cv::Point2f> corner_points_buf;//建一个数组缓存检测到的角点，通常采用Point2f形式
vector<cv::Point2f>::iterator corner_points_buf_ptr;
vector<vector<cv::Point2f>> corner_points_of_all_imgs;
int image_num = 0;
string filename;
while(image_num < imgList.size())
{
filename = imgList[image_num++];
cout << "image_num = " << image_num << endl;
cout << filename.c_str() << endl;
cv::Mat imageInput = cv::imread(filename.c_str());
if (image_num == 1)
{
image_size.width = imageInput.cols;
image_size.height = imageInput.rows;
cout << "image_size.width = " << image_size.width << endl;
cout << "image_size.height = " << image_size.height << endl;
}
if (findChessboardCorners(imageInput, pattern_size, corner_points_buf) == 0)
{
cout << "can not find chessboard corners!\n"; //找不到角点
exit(1);
}
else
{
cv::Mat gray;
cv::cvtColor(imageInput, gray, CV_RGB2GRAY);
cv::find4QuadCornerSubpix(gray, corner_points_buf, cv::Size(5, 5));
corner_points_of_all_imgs.push_back(corner_points_buf);
cv::drawChessboardCorners(gray, pattern_size, corner_points_buf, true);
cv::imshow("camera calibration", gray);
cv::waitKey(100);
}
}
int total = corner_points_of_all_imgs.size();
cout << "total=" << total << endl;
int cornerNum = pattern_size.width * pattern_size.height;//每张图片上的总的角点数
for (int i = 0; i < total;i++)
{
cout << "--> 第" << i + 1 << "幅图片的数据 -->:" << endl;
for (int j = 0;j < cornerNum;j++)
{
cout << "-->" << corner_points_of_all_imgs[i][j].x;
cout << "-->" << corner_points_of_all_imgs[i][j].y;
if ((j + 1) % 3 == 0)
{
cout << endl;
}
else
{
cout.width(10);
}
}
cout << endl;
}
cout << endl << "角点提取完成" << endl;
//摄像机标定
cout << "开始标定………………" << endl;
cv::Mat cameraMatrix = cv::Mat(3, 3, CV_32FC1, cv::Scalar::all(0));//内外参矩阵，H——单应性矩阵
cv::Mat distCoefficients = cv::Mat(1, 5, CV_32FC1, cv::Scalar::all(0));//摄像机的5个畸变系数：k1,k2,p1,p2,k3
vector<cv::Mat> tvecsMat;//每幅图像的平移向量，t
vector<cv::Mat> rvecsMat;//每幅图像的旋转向量（罗德里格旋转向量）
vector<vector<cv::Point3f>> objectPoints;//保存所有图片的角点的三维坐标
//初始化每一张图片中标定板上角点的三维坐标
int i, j, k;
for (k = 0;k < image_num;k++)//遍历每一张图片
{
vector<cv::Point3f> tempCornerPoints;//每一幅图片对应的角点数组
//遍历所有的角点
for (i = 0;i < pattern_size.height;i++)
{
for (j = 0;j < pattern_size.width;j++)
{
cv::Point3f singleRealPoint;//一个角点的坐标
singleRealPoint.x = i * 10;
singleRealPoint.y = j * 10;
singleRealPoint.z = 0;//假设z=0
tempCornerPoints.push_back(singleRealPoint);
}
}
objectPoints.push_back(tempCornerPoints);
}
cv::calibrateCamera(objectPoints, corner_points_of_all_imgs, image_size, cameraMatrix, distCoefficients, rvecsMat, tvecsMat, 0);
cout << "标定完成" << endl;
//开始保存标定结果
cout << "开始保存标定结果" << endl;
cout << endl << "相机相关参数：" << endl;
fout << "相机相关参数：" << endl;
cout << "1.内外参数矩阵:" << endl;
fout << "1.内外参数矩阵:" << endl;
cout << "大小：" << cameraMatrix.size() << endl;
fout << "大小：" << cameraMatrix.size() << endl;
cout << cameraMatrix << endl;
fout << cameraMatrix << endl;
cout << "2.畸变系数：" << endl;
fout << "2.畸变系数：" << endl;
cout << "大小：" << distCoefficients.size() << endl;
fout << "大小：" << distCoefficients.size() << endl;
cout << distCoefficients << endl;
fout << distCoefficients << endl;
cout << endl << "图像相关参数：" << endl;
fout << endl << "图像相关参数：" << endl;
cv::Mat rotation_Matrix = cv::Mat(3, 3, CV_32FC1, cv::Scalar::all(0));//旋转矩阵
for (i = 0;i < image_num;i++)
{
cout << "第" << i + 1 << "幅图像的旋转向量：" << endl;
fout << "第" << i + 1 << "幅图像的旋转向量：" << endl;
cout << rvecsMat[i] << endl;
fout << rvecsMat[i] << endl;
cout << "第" << i + 1 << "幅图像的旋转矩阵：" << endl;
fout << "第" << i + 1 << "幅图像的旋转矩阵：" << endl;
cv::Rodrigues(rvecsMat[i], rotation_Matrix);//将旋转向量转换为相对应的旋转矩阵
cout << rotation_Matrix << endl;
fout << rotation_Matrix << endl;
cout << "第" << i + 1 << "幅图像的平移向量：" << endl;
fout << "第" << i + 1 << "幅图像的平移向量：" << endl;
cout << tvecsMat[i] << endl;
fout << tvecsMat[i] << endl;
}
cout << "结果保存完毕" << endl;
//对标定结果进行评价
cout << "开始评价标定结果......" << endl;
//计算每幅图像中的角点数量，假设全部角点都检测到了
int corner_points_counts;
corner_points_counts = pattern_size.width * pattern_size.height;
cout << "每幅图像的标定误差：" << endl;
fout << "每幅图像的标定误差：" << endl;
double err = 0;//单张图像的误差
double total_err = 0;//所有图像的平均误差
for (i = 0;i < image_num;i++)
{
vector<cv::Point2f> image_points_calculated;//存放新计算出的投影点的坐标
vector<cv::Point3f> tempPointSet = objectPoints[i];
cv::projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoefficients, image_points_calculated);
//计算新的投影点与旧的投影点之间的误差
vector<cv::Point2f> image_points_old = corner_points_of_all_imgs[i];
//将两组数据换成Mat格式
cv::Mat image_points_calculated_mat = cv::Mat(1, image_points_calculated.size(), CV_32FC2);
cv::Mat image_points_old_mat = cv::Mat(1, image_points_old.size(), CV_32FC2);
for (j = 0;j < tempPointSet.size();j++)
{
image_points_calculated_mat.at<cv::Vec2f>(0, j) = cv::Vec2f(image_points_calculated[j].x, image_points_calculated[j].y);
image_points_old_mat.at<cv::Vec2f>(0, j) = cv::Vec2f(image_points_old[j].x, image_points_old[j].y);
}
err = cv::norm(image_points_calculated_mat, image_points_old_mat, cv::NORM_L2);
err /= corner_points_counts;
total_err += err;
cout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
fout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
}
cout << "总体平均误差：" << total_err / image_num << "像素" << endl;
fout << "总体平均误差：" << total_err / image_num << "像素" << endl;
cout << "评价完成" << endl;
fout.close();
cv::Mat mapx = cv::Mat(image_size, CV_32FC1);
cv::Mat mapy = cv::Mat(image_size, CV_32FC1);
cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
cout << "保存矫正图像" << endl;
string imageFileName;
std::stringstream StrStm;
for (int i = 0;i < image_num;i++)
{
cout << "Frame #" << i + 1 << endl;
cv::initUndistortRectifyMap(cameraMatrix, distCoefficients, R, cameraMatrix, image_size, CV_32FC1, mapx, mapy);
cv::Mat src_image = cv::imread(imgList[i].c_str(), 1);
cv::Mat new_image = src_image.clone();
cv::remap(src_image, new_image, mapx, mapy, cv::INTER_LINEAR);
imshow("原始图像", src_image);
imshow("矫正后图像", new_image);
StrStm.clear();
imageFileName.clear();
StrStm << i + 1;
StrStm >> imageFileName;
imageFileName += "_d.jpg";
cv::imwrite(imageFileName, new_image);
cv::waitKey(200);
}
cout << "保存结束" << endl;
cv::waitKey(0);
return 0;
}
