opencv-张氏标定法（后篇）
距上篇博客的更新已过很久，前一段时间忙于tensorflow机器学习实战一书的学习，还有上周对于之前一个demo的再次尝试，收获了一些，同时也再次备受打击，青云有点难受，不说了。
开始更新这次的博客，opencv-张氏标定法的后篇，主要就是标定程序的具体实践，在网上查阅了一些，目前认为不错的几个版本如下。
主要参考博客
Matlab标定工具箱的使用：https://blog.csdn.net/hyacinthkiss/article/details/41317087
双目视觉标定，矫正，深度图（Vs +OpenCV C++ Python实现）https://blog.csdn.net/xiao__run/article/details/78887362
牧野相机标定：https://blog.csdn.net/dcrmg/article/details/52939318?locationNum=3
相机标定很好的代码：https://gitee.com/rxdj/camera-calibration
1.目的与操作流程
相机标定的目的：获取摄像机的内参和外参矩阵（同时也会得到每一幅标定图像的选择和平移矩阵），内参和外参系数可以对之后相机拍摄的图像就进行矫正，得到畸变相对很小的图像。
相机标定的输入：标定图像上所有内角点的图像坐标，标定板图像上所有内角点的空间三维坐标（一般情况下假定图像位于Z=0平面上）。
相机标定的输出：摄像机的内参、外参系数。
这三个基础的问题就决定了使用Opencv实现张正友法标定相机的标定流程、标定结果评价以及使用标定结果矫正原始图像的完整流程：
1. 准备标定图片
2. 对每一张标定图片，提取角点信息
3.对每一张标定图片，进一步提取亚像素角点信息
4. 在棋盘标定图上绘制找到的内角点（非必须，仅为了显示）
5. 相机标定
6. 对标定结果进行评价
7. 查看标定效果——利用标定结果对棋盘图进行矫正
准备标定图片
标定图片需要使用标定板在不同位置、不同角度、不同姿态下拍摄，最少需要3张，以10~20张为宜。标定板需要是黑白相间的矩形构成的棋盘图，制作精度要求较高。在我的张氏标定法前篇中有一张标定图片可以下载然后打印标定即可。
首先，查看Matlab标定流程，按照参考博客一相信大多数人都没有问题，但是估计也有很多像我这种天生自带Bug的人。
下面这篇博客会是我们的救星。
使用MATLAB工具箱TOOLBOX_calib标定摄像头过程:
https://blog.csdn.net/moluoyu/article/details/51419024
首先，标定箱下载链接如下：
http://www.vision.caltech.edu/bouguetj/calib_doc/
可以下载最新发布的，不需要任何的积分，并且我最初在参考博客一的CSDN下载下来的标定箱出现两个问题。
第一个问题，我在标定选择角点的时候到了第三张图片就无法选择角点，相信很多人碰到了我的问题。相信很多人也遇到了这样的问题，咨询同门建议我装2016版，因为我之前是Matlab2014b版本，起初我是对于这些不太相信的，但是别人就是搞双目的，Matlab标定也做的挺好的，我选择相信了，最后。。。。。（漫长的等待）安装结束了。
出现了第二个问题，很是经典的问题
输入calib_gui,弹出
错误: BREAK 只能在 FOR 或 WHILE 循环中使用，因而只能与其对应的 FOR 或 WHILE 语句在同一文件中使用。 计算 UIControl Callback 时出错
（什么路径全部添加）
苍天啊！我不就是标定一下吗？岂能栽在Matlab上。
遇到了上面的博客，在上面的下载链接下载最新的标定工具箱，我相信肯定不是版本的问题，并且均没有出现上面的问题。
这是上面标定箱的第一个例子，假如你英文好的话，可以直接跳转去看英文的效果应该更好。
http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example.html
警告：采集图片的时候棋盘图千万不能动，此时从左右摄像头各采集一副图片，分为命名为left x与right x，然后再改变棋盘图位置，然后棋盘图又保存不动，再从左右摄像头各采集一副图片，命名为left y与right y，要不然后面你会后悔的，反正我是后悔了。
至于其他的没有什么问题，按照前面的博客一路绿灯，为防止404，摘录如下：
一、环境配置
1.解压MATLAB标定工具箱至某个文件夹，默认为：TOOLBOX_calib，将之存放在MATLAB\R2010b\toolbox下（个人喜欢，也可以放其他位置）。
2.运行Matlab并添加文件夹TOOLBOX_calib的位置到matlab路径path中，具体操作为：File->Set Path->Add Folder To Path，然后找到刚刚存放的文件夹TOOLBOX_calib，save一下就OK了。
二、文件配置
将所有要标定的棋盘图放到文件夹TOOLBOX_calib里.m文件所在的目录下（个人喜欢将左右相机的图片命名为left01.jpg和right01.jpg等的形式，看着舒服，也和后面要讲的标定结果保存对应起来），OK，万事俱备，准备开动了。
备注：这里没必要放到TOOLBOX_calib里.m文件所在的目录下，只需将当前的工作路径切换到图片存放路径下即可。
比如我的：
建议：采集棋盘图的时候要注意，尽量让棋盘占据尽可能多的画面，这样可以得到更多有关摄像头畸变方面的信息
三、单目标定
1.在Matlab命令行窗口中输入calib_gui，出现以下窗口。
2.选择Standard之后便出现以下窗口。
3.点击Image names，命令行窗口会提示你输入图片的basename以及图片的格式（比如你图片文件名是right01, right02, …, right09，basename就是right，图片格式如：jpg），然后Matlab会自动帮你读入这些图片，如下图所示，可以看到，读入了9幅右摄像头的棋盘图。
4.然后再回到主控制界面，点击Extract grid corners，提取每幅图的角点。点击完后，命令行会出现如下提示，主要是让你输入棋盘角点搜索窗口的大小。如果窗口定大一点的话提取角点会比较方便点，即便点得偏离了也能找到，但也要注意不能大过一个方格的大小。这些选项，都只要敲回车选用默认设置就可以了。
5.继上步Enter敲完后，跳出第一幅棋盘格图，按顺时针或是逆时针方向分别点选棋盘格的最外面的四个内格点，程序会自动检测出其余的角点。
6.在选完四个角点后，程序会在命令行窗口提示你输入标定板棋盘的size，输入你棋盘格的实际大小就行，比如我的棋盘格是30mm，就输入30。这步事实上相当关键，它定义了空间的尺度，如果要对物体进行测量的话，这步是必须的。Enter后同时得到角点检测结果图。
有时候也会让你指定X Y 方向内角点的数目，上图中已经有方向标示，比如我的分别是9个与6个，输入的话就输入6和5。
8.检测完所有的图像后，点击Calibration，开始摄像头标定。
9.标定完成后，可以进行各种结果的可视化观察。由Show Extrinsic得的结果图可见相机与标定板的距离约为1000mm左右，且其中各种颜色标定板对应Analyse Error的结果图的各种误差结果。
图2.Analyse Error的结果图
10.验证标定结果无误之后，就点击面板上的Save按钮，程序会把标定结果放在一个叫Calib_Result.mat中，默认存储路径为：文件夹TOOLBOX_calib里.m文件所在的目录下。为了方便后续立体标定，把这个文件名改为Calib_Result_right.mat。
11.左摄像头标定的方法与右摄像头相同，生成的Calib_Result.mat之后，将其改名为Calib_Result_left.mat就可以了。
12.验证标定结果无误之后，就点击面板上的Save按钮，程序会把标定结果放在一个叫Calib_Result.mat中，默认存储路径为：文件夹TOOLBOX_calib里.m文件所在的目录下。为了方便后续立体标定，把这个文件名改为Calib_Result_right.mat。
四、双目标定
1.左右摄像头都标定完成之后，就可以开始立体标定了。在Matlab命令行中键入stereo_gui启动立体标定面板，如下图所示。
2.点击Load left and right calibration files，在命令行窗口中会提示你分别填写左右自标定的结果，如果你按上面的文件名保存方式的话，直接Ender选择默认的文件名(Calib_Result_left.mat和Calib_Result_right.mat)，或是填写你自己保存的文件名。
3.Load成功后，就可以开始Run stereo calibration了，run之后的结果如下图所示，左右摄像头的参数都做了修正，并且也求出了两个摄像头之间的旋转和平移关系向量(om和T)。
（备注，我没有用双目相机，只是将单目放在平移台上模仿双目，可以看出焦距非常接近）
4.run成功后，就可以看标定的结果了。同样可以直观的看出相机的几乎没有相对位置差。
附录：至于如何保证双目双摄像头的焦距一致，我本身方向就是做自动对焦的，可以用一个比较不错的对焦评价函数对两个画面图像进行判断，目前这套自动对焦评价系统表现还不错。
Matlab操作起来很简单，效果也很好，但是始终是觉得要手动点那么多下很麻烦，继续侃侃OpenCV的标定代码吧！
主要用到的函数以及简要流程
1.对每一张标定图片，提取角点信息
//! finds checkerboard pattern of the specified size in the image
CV_EXPORTS_W bool findChessboardCorners( InputArray image, Size patternSize,
OutputArray corners,
int flags=CALIB_CB_ADAPTIVE_THRESH+CALIB_CB_NORMALIZE_IMAGE );第一个参数Image，传入拍摄的棋盘图Mat图像，必须是8位的灰度或者彩色图像；
第二个参数patternSize，每个棋盘图上内角点的行列数，一般情况下，行列数不要相同，便于后续标定程序识别标定板的方向；
第三个参数corners，用于存储检测到的内角点图像坐标位置，一般用元素是Point2f的向量来表示：vector<Point2f> image_points_buf;
第四个参数flage：用于定义棋盘图上内角点查找的不同处理方式，有默认值。
2.对每一张标定图片，进一步提取亚像素角点信息
为了提高标定精度，需要在初步提取的角点信息上进一步提取亚像素信息，降低相机标定偏差，常用的方法是cornerSubPix，另一个方法是使用find4QuadCornerSubpix函数，这个方法是专门用来获取棋盘图上内角点的精确位置的，或许在相机标定的这个特殊场合下它的检测精度会比cornerSubPix更高？
cornerSubPix函数原型：
//! adjusts the corner locations with sub-pixel accuracy to maximize the certain cornerness criteria
CV_EXPORTS_W void cornerSubPix( InputArray image, InputOutputArray corners,
Size winSize, Size zeroZone,
TermCriteria criteria ); 第一个参数image，输入的Mat矩阵，最好是8位灰度图像，检测效率更高；
第二个参数corners，初始的角点坐标向量，同时作为亚像素坐标位置的输出，所以需要是浮点型数据，一般用元素是Pointf2f/Point2d的向量来表示：vector
//! finds subpixel-accurate positions of the chessboard corners
CV_EXPORTS bool find4QuadCornerSubpix(InputArray img, InputOutputArray corners, Size region_size);第一个参数img，输入的Mat矩阵，最好是8位灰度图像，检测效率更高；
第二个参数corners，初始的角点坐标向量，同时作为亚像素坐标位置的输出，所以需要是浮点型数据，一般用元素是Pointf2f/Point2d的向量来表示：vector<Point2f/Point2d> iamgePointsBuf;
第三个参数region_size，角点搜索窗口的尺寸；
在其中一个标定的棋盘图上分别运行cornerSubPix和find4QuadCornerSubpix寻找亚像素角点，两者定位到的亚像素角点虽然有一定差距，但偏差基本都控制在0.5个像素之内。
4.在棋盘标定图上绘制找到的内角点（非必须，仅为了显示）
drawChessboardCorners函数用于绘制被成功标定的角点，函数原型：
//! draws the checkerboard pattern (found or partly found) in the image
CV_EXPORTS_W void drawChessboardCorners( InputOutputArray image, Size patternSize,
InputArray corners, bool patternWasFound ); 第一个参数image，8位灰度或者彩色图像；
第二个参数patternSize，每张标定棋盘上内角点的行列数；
第三个参数corners，初始的角点坐标向量，同时作为亚像素坐标位置的输出，所以需要是浮点型数据，一般用元素是Pointf2f/Point2d的向量来表示：vector<Point2f/Point2d> iamgePointsBuf;
第四个参数patternWasFound，标志位，用来指示定义的棋盘内角点是否被完整的探测到，true表示别完整的探测到，函数会用直线依次连接所有的内角点，作为一个整体，false表示有未被探测到的内角点，这时候函数会以（红色）圆圈标记处检测到的内角点；
5.相机标定
获取到棋盘标定图的内角点图像坐标之后，就可以使用calibrateCamera函数进行标定，计算相机内参和外参系数，
calibrateCamera函数原型：
//! finds intrinsic and extrinsic camera parameters from several fews of a known calibration pattern.
CV_EXPORTS_W double calibrateCamera( InputArrayOfArrays objectPoints,
InputArrayOfArrays imagePoints,
Size imageSize,
CV_OUT InputOutputArray cameraMatrix,
CV_OUT InputOutputArray distCoeffs,
OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
int flags=0, TermCriteria criteria = TermCriteria(
TermCriteria::COUNT+TermCriteria::EPS, 30, DBL_EPSILON) ); 第一个参数objectPoints，为世界坐标系中的三维点。在使用时，应该输入一个三维坐标点的向量的向量，即vector<vector<Point3f>> object_points。需要依据棋盘上单个黑白矩阵的大小，计算出（初始化）每一个内角点的世界坐标。
第二个参数imagePoints，为每一个内角点对应的图像坐标点。和objectPoints一样，应该输入vector<vector<Point2f>> image_points_seq形式的变量；
第三个参数imageSize，为图像的像素尺寸大小，在计算相机的内参和畸变矩阵时需要使用到该参数；
第四个参数cameraMatrix为相机的内参矩阵。输入一个Mat cameraMatrix即可，如Mat cameraMatrix=Mat(3,3,CV_32FC1,Scalar::all(0));
第五个参数distCoeffs为畸变矩阵。输入一个Mat distCoeffs=Mat(1,5,CV_32FC1,Scalar::all(0))即可；
第六个参数rvecs为旋转向量；应该输入一个Mat类型的vector，即vector<Mat>rvecs;
第七个参数tvecs为位移向量，和rvecs一样，应该为vector<Mat> tvecs;
第八个参数flags为标定时所采用的算法。有如下几个参数：
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是最优迭代终止条件设定。
在使用该函数进行标定运算之前，需要对棋盘上每一个内角点的空间坐标系的位置坐标进行初始化，标定的结果是生成相机的内参矩阵cameraMatrix、相机的5个畸变系数distCoeffs，另外每张图像都会生成属于自己的平移向量和旋转向量。
6.对标定结果进行评价
对标定结果进行评价的方法是通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到空间三维点在图像上新的投影点的坐标，计算投影坐标和亚像素角点坐标之间的偏差，偏差越小，标定结果越好。
对空间三维坐标点进行反向投影的函数是projectPoints，函数原型是：
//! projects points from the model coordinate space to the image coordinates. Also computes derivatives of the image coordinates w.r.t the intrinsic and extrinsic camera parameters
CV_EXPORTS_W void projectPoints( InputArray objectPoints,
InputArray rvec, InputArray tvec,
InputArray cameraMatrix, InputArray distCoeffs,
OutputArray imagePoints,
OutputArray jacobian=noArray(),
double aspectRatio=0 ); 第一个参数objectPoints，为相机坐标系中的三维点坐标；
第二个参数rvec为旋转向量，每一张图像都有自己的x旋转向量；
第三个参数tvec为位移向量，每一张图像都有自己的平移向量；
第四个参数cameraMatrix为求得的相机的内参数矩阵；
第五个参数distCoeffs为相机的畸变矩阵；
第六个参数iamgePoints为每一个内角点对应的图像上的坐标点；
第七个参数jacobian是雅可比行列式；
第八个参数aspectRatio是跟相机传感器的感光单元有关的可选参数，如果设置为非0，则函数默认感光单元的dx/dy是固定的，会依此对雅可比矩阵进行调整；
7.查看标定效果——利用标定结果对棋盘图进行矫正
利用求得的相机的内参和外参数据，可以对图像进行畸变的矫正，这里有两种方法可以达到矫正的目的，分别说明一下。
方法一：使用initUndistortRectifyMap和remap两个函数配合实现。
initUndistortRectifyMap用来计算畸变映射，remap把求得的映射应用到图像上。
initUndistortRectifyMap的函数原型：
//! initializes maps for cv::remap() to correct lens distortion and optionally rectify the image
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，输入的校正后的3X3摄像机矩阵；
第五个参数size，摄像机采集的无失真的图像尺寸；
第六个参数m1type，定义map1的数据类型，可以是CV_32FC1或者CV_16SC2；
第七个参数map1和第八个参数map2，输出的X/Y坐标重映射参数；
remap函数原型：
//! warps the image using the precomputed maps. The maps are stored in either floating-point or integer fixed-point format
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和第四个参数map2，X坐标和Y坐标的映射；
第五个参数interpolation，定义图像的插值方式；
第六个参数borderMode，定义边界填充方式；
方法二：使用undistort函数实现
undistort函数原型：
//! corrects lens distortion for the given camera matrix and distortion coefficients
CV_EXPORTS_W void undistort( InputArray src, OutputArray dst,
InputArray cameraMatrix,
InputArray distCoeffs,
InputArray newCameraMatrix=noArray() );第一个参数src，输入参数，代表畸变的原始图像；
第二个参数dst，矫正后的输出图像，跟输入图像具有相同的类型和大小；
第三个参数cameraMatrix为之前求得的相机的内参矩阵；
第四个参数distCoeffs为之前求得的相机畸变矩阵；
第五个参数newCameraMatrix，默认跟cameraMatrix保持一致；
方法一相比方法二执行效率更高一些，推荐使用。
下面是牧野的代码：
code1:
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
#include <fstream>
using namespace cv;
using namespace std;
void main()
{
ifstream fin("filename.txt"); /* 标定所用图像文件的路径 */
ofstream fout("caliberation_result_qingyun.txt"); /* 保存标定结果的文件 */
//读取每一幅图像，从中提取出角点，然后对角点进行亚像素精确化
cout << "开始提取角点………………";
int image_count = 0; /* 图像数量 */
Size image_size; /* 图像的尺寸 */
Size board_size = Size(9, 6); /* 标定板上每行、列的角点数 */
vector<Point2f> image_points_buf; /* 缓存每幅图像上检测到的角点 */
vector<vector<Point2f>> image_points_seq; /* 保存检测到的所有角点 */
string filename;
int count = -1;//用于存储角点个数。
while (getline(fin, filename))
{
image_count++;
// 用于观察检验输出
cout << "image_count = " << image_count << endl;
/* 输出检验*/
cout << "-->count = " << count;
Mat imageInput = imread(filename);
if (image_count == 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 (0 == findChessboardCorners(imageInput, board_size, image_points_buf))
{
cout << "can not find chessboard corners!\n"; //找不到角点
exit(1);
}
else
{
Mat view_gray;
cvtColor(imageInput, view_gray, CV_RGB2GRAY);
/* 亚像素精确化 */
find4QuadCornerSubpix(view_gray, image_points_buf, Size(5, 5)); //对粗提取的角点进行精确化
image_points_seq.push_back(image_points_buf); //保存亚像素角点
/* 在图像上显示角点位置 */
drawChessboardCorners(view_gray, board_size, image_points_buf, true); //用于在图片中标记角点
imshow("Camera Calibration", view_gray);//显示图片
waitKey(500);//暂停0.5S
}
}
int total = image_points_seq.size();
cout << "total = " << total << endl;
int CornerNum = board_size.width*board_size.height; //每张图片上总的角点数
for (int ii = 0; ii<total; ii++)
{
if (0 == ii%CornerNum)// 24 是每幅图片的角点个数。此判断语句是为了输出 图片号，便于控制台观看
{
int i = -1;
i = ii / CornerNum;
int j = i + 1;
cout << "--> 第 " << j << "图片的数据 --> : " << endl;
}
if (0 == ii % 3) // 此判断语句，格式化输出，便于控制台查看
{
cout << endl;
}
else
{
cout.width(10);
}
//输出所有的角点
cout << " -->" << image_points_seq[ii][0].x;
cout << " -->" << image_points_seq[ii][0].y;
}
cout << "角点提取完成！\n";
//以下是摄像机标定
cout << "开始标定………………";
/*棋盘三维信息*/
Size square_size = Size(30, 30); /* 实际测量得到的标定板上每个棋盘格的大小 */
vector<vector<Point3f>> object_points; /* 保存标定板上角点的三维坐标 */
/*内外参数*/
Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 摄像机内参数矩阵 */
vector<int> point_counts; // 每幅图像中角点的数量
Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0)); /* 摄像机的5个畸变系数：k1,k2,p1,p2,k3 */
vector<Mat> tvecsMat; /* 每幅图像的旋转向量 */
vector<Mat> rvecsMat; /* 每幅图像的平移向量 */
/* 初始化标定板上角点的三维坐标 */
int i, j, t;
for (t = 0; t<image_count; t++)
{
vector<Point3f> tempPointSet;
for (i = 0; i<board_size.height; i++)
{
for (j = 0; j<board_size.width; j++)
{
Point3f realPoint;
/* 假设标定板放在世界坐标系中z=0的平面上 */
realPoint.x = i*square_size.width;
realPoint.y = j*square_size.height;
realPoint.z = 0;
tempPointSet.push_back(realPoint);
}
}
object_points.push_back(tempPointSet);
}
/* 初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板 */
for (i = 0; i<image_count; i++)
{
point_counts.push_back(board_size.width*board_size.height);
}
/* 开始标定 */
calibrateCamera(object_points, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0);
cout << "标定完成！\n";
//对标定结果进行评价
cout << "开始评价标定结果………………\n";
double total_err = 0.0; /* 所有图像的平均误差的总和 */
double err = 0.0; /* 每幅图像的平均误差 */
vector<Point2f> image_points2; /* 保存重新计算得到的投影点 */
cout << "\t每幅图像的标定误差：\n";
fout << "每幅图像的标定误差：\n";
for (i = 0; i<image_count; i++)
{
vector<Point3f> tempPointSet = object_points[i];
/* 通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点 */
projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points2);
/* 计算新的投影点和旧的投影点之间的误差*/
vector<Point2f> tempImagePoint = image_points_seq[i];
Mat tempImagePointMat = Mat(1, tempImagePoint.size(), CV_32FC2);
Mat image_points2Mat = Mat(1, image_points2.size(), CV_32FC2);
for (int j = 0; j < tempImagePoint.size(); j++)
{
image_points2Mat.at<Vec2f>(0, j) = Vec2f(image_points2[j].x, image_points2[j].y);
tempImagePointMat.at<Vec2f>(0, j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
}
err = norm(image_points2Mat, tempImagePointMat, NORM_L2);
total_err += err /= point_counts[i];
std::cout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
fout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
}
std::cout << "总体平均误差：" << total_err / image_count << "像素" << endl;
fout << "总体平均误差：" << total_err / image_count << "像素" << endl << endl;
std::cout << "评价完成！" << endl;
//保存定标结果
std::cout << "开始保存定标结果………………" << endl;
Mat rotation_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 保存每幅图像的旋转矩阵 */
fout << "相机内参数矩阵：" << endl;
fout << cameraMatrix << endl << endl;
fout << "畸变系数：\n";
fout << distCoeffs << endl << endl << endl;
for (int i = 0; i<image_count; i++)
{
fout << "第" << i + 1 << "幅图像的旋转向量：" << endl;
fout << tvecsMat[i] << endl;
/* 将旋转向量转换为相对应的旋转矩阵 */
Rodrigues(tvecsMat[i], rotation_matrix);
fout << "第" << i + 1 << "幅图像的旋转矩阵：" << endl;
fout << rotation_matrix << endl;
fout << "第" << i + 1 << "幅图像的平移向量：" << endl;
fout << rvecsMat[i] << endl << endl;
}
std::cout << "完成保存" << endl;
fout << endl;
/************************************************************************
显示定标结果
*************************************************************************/
Mat mapx = Mat(image_size, CV_32FC1);
Mat mapy = Mat(image_size, CV_32FC1);
Mat R = Mat::eye(3, 3, CV_32F);
std::cout << "保存矫正图像" << endl;
string imageFileName;
std::stringstream StrStm;
for (int i = 0; i != image_count; i++)
{
std::cout << "Frame #" << i + 1 << "..." << endl;
initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, image_size, CV_32FC1, mapx, mapy);
StrStm.clear();
imageFileName.clear();
string filePath = "chess";
StrStm << i + 1;
StrStm >> imageFileName;
filePath += imageFileName;
filePath += ".bmp";
Mat imageSource = imread(filePath);
Mat newimage = imageSource.clone();
//另一种不需要转换矩阵的方式
//undistort(imageSource,newimage,cameraMatrix,distCoeffs);
remap(imageSource, newimage, mapx, mapy, INTER_LINEAR);
imshow("原始图像", imageSource);
imshow("矫正后图像", newimage);
waitKey();
StrStm.clear();
filePath.clear();
StrStm << i + 1;
StrStm >> imageFileName;
imageFileName += "_d.jpg";
imwrite(imageFileName, newimage);
}
std::cout << "保存结束" << endl;
return;
}整套代码注释的非常详细，用到的函数原型上面都可以找到，文件的读取以及参数的输出格式这部分也做的比较好。
但是相比较于下面这个代码就做的更好了，但是注释偏少，建议与牧野的代码对比看，会有更好的效果。
code2:
Main函数
/***********
张正友标定法与两种畸变校正模型示例代码 OpenCV3.4.0 + VS2015（备注：我是使用的opencv3.1.0）
代码主要有两部分组成，1.张正友摄像机标定 2.畸变校正。分别包含普通摄像机模型（CV）和鱼眼摄像机模型(fisheye)下两种标定和校正方法。
各个文件简要说明
*main.cpp：主要为接口函数，以及一些接口参数的设置。
包括：patternImgPath----标定板图像存放文件夹路径（标定板图像文件名按照0.jpg, 1.jpg...如此命名）；
calibResultPath----摄像机标定内部参数文件保存路径，fx, fy, cx,cy, 以及畸变系数(k1,k2,p1,p2,k3 或者 k1,k2,k3,k4);
srcImgPath----相机拍摄测试图片保存路径，用来测试标定和畸变校正结果；
boardSize----标定板内角点行和列个数；
CCalibration类实现摄像机标定功能，calibration.run()执行读取标定板图片、角点检测、亚像素精确化、摄像机标定、计算重投影误差、保存标定参数功能；
CUndistort类实现畸变校正功能，undistort.run()执行读取内部参数、读取畸变图像、畸变校正、显示校正结果功能。
*calibration.h, calibration.cpp：实现摄像机标定，包含CV模型和Fisheye模型。默认为CV模型，如需更换为Fisheye模型，
请将calibration.h文件中的( #define CV )替换为（ #define FISHEYE ）即可，内部代码会根据宏定义来执行对应的标定和畸变校正模型代码。
*undistort.h, undistort.cpp：实现畸变校正，包含CV模型和Fisheye模型。
************/
#include <opencv2\opencv.hpp>
#include <string>
#include <iostream>
#include "calibration.h"
#include "undistort.h"
using namespace std;
using namespace cv;
int main()
{
string patternImgPath = "c:/users/ltc/Desktop/data/pattern/"; //标定板图像存放文件夹路径（标定板图像文件名按照0.jpg, 1.jpg...如此命名）；
string calibResultPath = "c:/users/ltc/Desktop/data/results/"; //摄像机标定内部参数文件保存路径，fx, fy, cx,cy, 以及畸变系数(k1,k2,p1,p2,k3 或者 k1,k2,k3,k4);
string srcImgPath = "c:/users/ltc/Desktop/data/srcImg/0.jpg"; //相机拍摄测试图片保存路径，用来测试标定和畸变校正结果；
Size boardSize = Size(9, 6); //标定板内角点行和列个数
CCalibration calibration(patternImgPath, calibResultPath, boardSize);
calibration.run();
CUndistort undistort(srcImgPath, calibResultPath);
undistort.run();
}Calibration.h
#pragma once
#include <opencv2\opencv.hpp>
#include <string>
#include <fstream>
#include <iostream>
using namespace std;
using namespace cv;
#define CV
//#define FISHEYE
class CCalibration
{
public:
CCalibration(string patternImgPath, string CalibResultPath, Size boardSize)
{
this->patternImgPath = patternImgPath;
this->calibResultPath = CalibResultPath;
this->boardSize = boardSize;
}
~CCalibration() {}
private:
vector<cv::Point3f> singlePatternPoints;
vector<Mat> patternImgList;
int imgHeight;
int imgWidth;
int imgNum;
float scale = 0.25;
float errThresh = 3000;
string patternImgPath;
string calibResultPath;
Size boardSize;
Mat camK;
Mat camDiscoeff;
private:
int evaluateCalibrationResult(vector<vector<cv::Point3f>> objectPoints, vector<vector<cv::Point2f>> cornerSquare, vector<int> pointCnts, vector<cv::Vec3d> _rvec,
vector<cv::Vec3d> _tvec, cv::Mat _K, cv::Mat _D, int count, vector<int> &outLierIndex, int errThresh);
bool testCorners(vector<cv::Point2f>& corners, int patternWidth, int patternHeight);
void init3DPoints(cv::Size boardSize, cv::Size squareSize, vector<cv::Point3f> &singlePatternPoint);
public:
bool writeParams();
bool readPatternImg();
void calibProcess();
void run();
};Calibration.cpp
#include "calibration.h"
bool CCalibration::writeParams()
{
camK.convertTo(camK, CV_32FC1);
camDiscoeff.convertTo(camDiscoeff, CV_32FC1);
ofstream out;
out.open(calibResultPath + "calibResult.txt", ios::out);
out << camK.at<float>(0, 0) << endl;
out << camK.at<float>(1, 1) << endl;
out << camK.at<float>(0, 2) << endl;
out << camK.at<float>(1, 2) << endl;
#ifdef CV
out << camDiscoeff.at<float>(0, 0) << endl;
out << camDiscoeff.at<float>(0, 1) << endl;
out << camDiscoeff.at<float>(0, 2) << endl;
out << camDiscoeff.at<float>(0, 3) << endl;
out << camDiscoeff.at<float>(0, 4) << endl;
#elif defined FISHEYE
out << camDiscoeff.at<float>(0, 0) << endl;
out << camDiscoeff.at<float>(0, 1) << endl;
out << camDiscoeff.at<float>(0, 2) << endl;
out << camDiscoeff.at<float>(0, 3) << endl;
#endif
out.close();
return true;
}
bool CCalibration::readPatternImg()
{
int imgNum = 0;
string baseName = "right0"; /* 标定图像文件的基础命名*/
string imgType = ".jpg"; /*标定图像文件的格式*/
Mat img;
do
{
stringstream ss;
ss << imgNum;
string path = patternImgPath + baseName + ss.str() + imgType;
//string path = patternImgPath + ss.str() + ".jpg";
img = imread(path, 0);
if (!img.data)
{
break;
}
patternImgList.push_back(img);
imgNum++;
} while (true);
if (imgNum == 0)
{
cout << " error! No pattern imgs!" << endl;
return false;
}
this->imgNum = imgNum;
imgHeight = patternImgList[0].rows;
imgWidth = patternImgList[0].cols;
return true;
}
//通过计算三个相邻角点构成的两个向量之间的夹角判断角点连接性
bool CCalibration::testCorners(vector<cv::Point2f>& corners, int patternWidth, int patternHeight)
{
if (corners.size() != patternWidth * patternHeight)
{
return false;
}
double dx1, dx2, dy1, dy2;
double cosVal;
for (int i = 0; i < patternHeight; ++i)
{
for (int j = 0; j < patternWidth - 2; ++j)
{
dx1 = corners[i*patternWidth + j + 1].x - corners[i*patternWidth + j].x;
dy1 = corners[i*patternWidth + j + 1].y - corners[i*patternWidth + j].y;
dx2 = corners[i*patternWidth + j + 2].x - corners[i*patternWidth + j + 1].x;
dy2 = corners[i*patternWidth + j + 2].y - corners[i*patternWidth + j + 1].y;
cosVal = (dx1 * dx2 + dy1 * dy2) / sqrt((dx1 * dx1 + dy1 * dy1) * (dx2 * dx2 + dy2 * dy2));
if (fabs(cosVal) < 0.993)
{
return false;
}
}
}
for (int i = 0; i < patternHeight - 2; ++i)
{
for (int j = 0; j < patternWidth; ++j)
{
dx1 = corners[(i + 1)*patternWidth + j].x - corners[i*patternWidth + j].x;
dy1 = corners[(i + 1)*patternWidth + j].y - corners[i*patternWidth + j].y;
dx2 = corners[(i + 2)*patternWidth + j].x - corners[(i + 1)*patternWidth + j].x;
dy2 = corners[(i + 2)*patternWidth + j].y - corners[(i + 1)*patternWidth + j].y;
cosVal = (dx1 * dx2 + dy1 * dy2) / sqrt((dx1 * dx1 + dy1 * dy1) * (dx2 * dx2 + dy2 * dy2));
if (fabs(cosVal) < 0.993)
{
return false;
}
}
}
return true;
}
//初始化角点的三维坐标
void CCalibration::init3DPoints(cv::Size boardSize, cv::Size squareSize, vector<cv::Point3f> &singlePatternPoint)
{
for (int i = 0; i<boardSize.height; i++)
{
for (int j = 0; j<boardSize.width; j++)
{
cv::Point3f tempPoint;//单个角点的三维坐标
tempPoint.x = float(i * squareSize.width);
tempPoint.y = float(j * squareSize.height);
tempPoint.z = 0;
singlePatternPoint.push_back(tempPoint);
}
}
}
void CCalibration::calibProcess()
{
//***************摄像机标定****************//
double time0 = (double)getTickCount();
vector<Point2f> corners;//存储一幅棋盘图中的所有角点二维坐标
vector<vector<Point2f>> cornersSeq;//存储所有棋盘图角点的二维坐标
vector<Mat> image_Seq;//存储所有棋盘图
int successImgNum = 0;
int count = 0;
cout << "********开始提取角点！********" << endl;
Mat image, scaleImg;
for (int i = 0; i<imgNum; i++)
{
cout << "Image#" << i << "......." << endl;
image = patternImgList[i].clone();
//降采样原图,加快角点提取速度
cv::resize(image, scaleImg, cv::Size(), scale, scale, CV_INTER_LINEAR);
/**********************提取角点*************************/
bool patternfound = findChessboardCorners(scaleImg, boardSize,
corners, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_FAST_CHECK);
if (!patternfound)
{
cout << "Can not find chess board corners!\n" << endl;
continue;
}
else
{
//上采样corner
for (int num = 0; num < corners.size(); num++)
{
cv::Point2f tempPoint = corners[num];
corners[num] = cv::Point2f(tempPoint.x / scale, tempPoint.y / scale);
}
/************************亚像素精确化******************************/
cornerSubPix(image, corners, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
bool good = testCorners(corners, boardSize.width, boardSize.height);
if (false == good) continue;
/************************绘制检测到的角点并显示******************************/
Mat cornerImg = image.clone();
cvtColor(cornerImg, cornerImg, CV_GRAY2BGR);
for (int j = 0; j< corners.size(); j++)
{
circle(cornerImg, corners[j], 20, Scalar(0, 0, 255), 2, 8, 0);
}
namedWindow("CirclePattern", 0);
imshow("CirclePattern", cornerImg);
cout << "press any key to see next pattern image" << endl;
waitKey(1);
count += (int)corners.size();//所有棋盘图中的角点个数
successImgNum++;//成功提取角点的棋盘图个数
cornersSeq.push_back(corners);
image_Seq.push_back(image);
}
}
cout << "*******角点提取完成！******" << endl;
/**************************摄像机标定******************************/
Size squre_size = Size(30, 30);//棋盘格尺寸
vector<vector<Point3f>> object_points;//所有棋盘图像的角点三维坐标
vector<int> pointCounts;
//初始化单幅靶标图片的三维点
init3DPoints(boardSize, squre_size, singlePatternPoints);
//初始化标定板上的三维坐标
for (int n = 0; n<successImgNum; n++)
{
object_points.push_back(singlePatternPoints);
pointCounts.push_back(boardSize.width * boardSize.height);
}
/***开始标定***/
cout << "*****开始标定!******" << endl;
Size imgSize = Size(imgWidth, imgHeight);
vector<Vec3d> rotation;//旋转向量
vector<Vec3d> translation;//平移向量
#ifdef CV
int flags = 0;
cv::calibrateCamera(object_points, cornersSeq, imgSize, camK, camDiscoeff,
rotation, translation, flags);
#elif defined FISHEYE
int flags = 0;
cv::fisheye::calibrate(object_points, cornersSeq, imgSize, camK,
camDiscoeff, rotation, translation, flags, cv::TermCriteria(3, 20, 1e-6));
#endif
cout << "*****标定完成！*****" << endl;
double time1 = getTickCount();
cout << "Calibration time :" << (time1 - time0) / getTickFrequency() << "s" << endl;
//评价
cout << "*****开始评价标定结果*****" << endl;
vector<int> outLierIndex;
evaluateCalibrationResult(object_points, cornersSeq, pointCounts, rotation, translation,
camK, camDiscoeff, successImgNum, outLierIndex, errThresh);
//删除误差大的角点图
vector<vector<cv::Point2f>> newCornersSeq;
successImgNum = 0;
for (int i = 0; i < cornersSeq.size(); i++)
{
if (outLierIndex[i] == 0)
{
newCornersSeq.push_back(cornersSeq[i]);
successImgNum++;
}
}
vector<vector<cv::Point3f>> newObjectPoints;
for (int n = 0; n<successImgNum; n++)
{
newObjectPoints.push_back(singlePatternPoints);
}
cout << "*****评价完成！*****" << endl;
//重新标定
cout << "*****开始重新标定*****" << endl;
#ifdef CV
cv::calibrateCamera(newObjectPoints, newCornersSeq, imgSize, camK, camDiscoeff,
rotation, translation, flags);
#elif defined FISHEYE
cv::fisheye::calibrate(newObjectPoints, newCornersSeq, imgSize, camK, camDiscoeff,
rotation, translation, flags, cv::TermCriteria(3, 20, 1e-6));
#endif
//重新计算重投影误差
outLierIndex.clear();
evaluateCalibrationResult(newObjectPoints, newCornersSeq, pointCounts, rotation, translation,
camK, camDiscoeff, successImgNum, outLierIndex, errThresh);
#ifdef DEBUG
//通过畸变校正效果查看摄像机标定效果
cv::Mat R = cv::Mat::eye(3, 3, CV_32FC1);
cv::Mat mapx, mapy, newCamK, undistortImg, showImg;
cv::initUndistortRectifyMap(camK, camDiscoeff, R, camK, imgSize, CV_32FC1, mapx, mapy);
cv::remap(image_Seq[0], undistortImg, mapx, mapy, CV_INTER_LINEAR);
cv::resize(undistortImg, showImg, cv::Size(), 0.25, 0.25, CV_INTER_LINEAR);
string winName = "undistortImg";
cv::namedWindow(winName, 1);
cv::imshow(winName, showImg);
cv::waitKey(0);
#endif
}
//估计重投影误差，并排除误差大于设定阈值的靶标图片
int CCalibration::evaluateCalibrationResult(vector<vector<cv::Point3f>> objectPoints, vector<vector<cv::Point2f>> cornerSquare, vector<int> pointCnts, vector<cv::Vec3d> _rvec,
vector<cv::Vec3d> _tvec, cv::Mat _K, cv::Mat _D, int count, vector<int> &outLierIndex, int errThresh)
{
string evaluatPath = calibResultPath + "evaluateCalibrationResult.txt";
ofstream fout(evaluatPath);
double total_err = 0.0;//所有图像的平均误差和
double err = 0.0;//单幅图像的平均误差
vector<cv::Point2f> proImgPoints;
for (int i = 0; i< count; i++)
{
float maxValue = -1;
vector<cv::Point3f> tempPointSet = objectPoints[i];
#ifdef CV
cv::projectPoints(tempPointSet, _rvec[i], _tvec[i], _K, _D, proImgPoints);
#elif defined FISHEYE
cv::fisheye::projectPoints(tempPointSet, proImgPoints, _rvec[i], _tvec[i], _K, _D);
#endif
vector<cv::Point2f> tempImgPoint = cornerSquare[i];
cv::Mat tempImgPointMat = cv::Mat(1, tempImgPoint.size(), CV_32FC2);
cv::Mat proImgPointsMat = cv::Mat(1, proImgPoints.size(), CV_32FC2);
for (int j = 0; j != tempImgPoint.size(); j++)
{
proImgPointsMat.at<cv::Vec2f>(0, j) = cv::Vec2f(proImgPoints[j].x, proImgPoints[j].y);
tempImgPointMat.at<cv::Vec2f>(0, j) = cv::Vec2f(tempImgPoint[j].x, tempImgPoint[j].y);
float dx = proImgPoints[j].x - tempImgPoint[j].x;
float dy = proImgPoints[j].y - tempImgPoint[j].y;
float diff = sqrt(dx*dx + dy*dy);
if (diff > maxValue)
{
maxValue = diff;
}
}
fout << "第" << i << "幅图像的最大重投影误差：" << maxValue << "像素" << endl;
//找出重投影误差大于errThresh的图
if (maxValue > errThresh)
{
outLierIndex.push_back(-1);
}
else
{
outLierIndex.push_back(0);
}
}
fout.close();
return 0;
}
void CCalibration::run()
{
bool readSuccess = readPatternImg();
if (!readSuccess)
{
cout << "Fail! No Pattern Imgs !" << endl;
getchar();
}
calibProcess();
writeParams();
}undistort.h
#include <opencv2\opencv.hpp>
#include <fstream>
#include <string>
#include <iostream>
#include "calibration.h"
using namespace std;
using namespace cv;
class CUndistort
{
public:
CUndistort(string srcImgPath, string calibResultPath)
{
this->srcImgPath = srcImgPath;
this->calibResultPath = calibResultPath;
this->K = Mat::eye(Size(3, 3), CV_32FC1);
this->discoeff = Mat::zeros(Size(1, 5), CV_32FC1);
}
~CUndistort() {}
private:
string srcImgPath;
string calibResultPath;
vector<Mat> srcImgList;
vector<Mat> dsrImgList;
Mat K;
Mat R;
Mat discoeff;
public:
bool readParams();
bool undistProcess();
void run();
};
undistort.cpp
#include "undistort.h"
bool CUndistort::readParams()
{
ifstream in;
in.open(calibResultPath + "calibResult.txt", ios::in);
in >> K.at<float>(0, 0);
in >> K.at<float>(1, 1);
in >> K.at<float>(0, 2);
in >> K.at<float>(1, 2);
#ifdef CV
in >> discoeff.at<float>(0, 0);
in >> discoeff.at<float>(1, 0);
in >> discoeff.at<float>(2, 0);
in >> discoeff.at<float>(3, 0);
in >> discoeff.at<float>(4, 0);
#elif defined FISHEYE
in >> discoeff.at<float>(0, 0);
in >> discoeff.at<float>(1, 0);
in >> discoeff.at<float>(2, 0);
in >> discoeff.at<float>(3, 0);
#endif
in.close();
return true;
}
bool CUndistort::undistProcess()
{
//***************畸变校正****************//
R = Mat::eye(Size(3, 3), CV_32FC1);
Mat mapx, mapy;
Mat srcImg = imread(srcImgPath);
Mat dstImg;
#ifdef CV
cv::initUndistortRectifyMap(K, discoeff, R, K, srcImg.size(), CV_32FC1, mapx, mapy);
#elif defined FISHEYE
cv::fisheye::initUndistortRectifyMap(K, discoeff, R, K, srcImg.size(), CV_32FC1, mapx, mapy);
#endif
remap(srcImg, dstImg, mapx, mapy, CV_INTER_LINEAR);
cv::resize(dstImg, dstImg, cv::Size(), 0.25, 0.25, CV_INTER_LINEAR);
cv::namedWindow("show", 1);
imshow("show", dstImg);
waitKey(0);
return true;
}
void CUndistort::run()
{
bool readSuccess = readParams();
if (!readSuccess)
{
cout << "read Params Failed!" << endl;
getchar();
}
undistProcess();
}
说说这套代码的神奇之处吧！
1. 体现了C++代码类的优良封装性，代码整体很顺畅简洁优雅
2. 降采样加快角点提取速度，这一步十分必要，如我的手机拍出来的图片都是4192x3104像素的，如果不进行降采样提取角点的速度是及其之慢的。（注意降采样检测出来的角点之后要记得上采样，要不然出来的结果将会截然不同）
3. 代码将鱼眼模型以及普通镜头模型整合到了一起，十分方便
4. 通过计算三个相邻角点构成的两个向量之间的夹角判断角点连接性进行角点的筛选
5. 计算重投影误差过后再标定再计算的一个反复优化过程
双目标定这块就差LM算法优化这篇番外了，其他的就到这里吧！
休息休息！整理了好久。
推荐阅读