內容參考自：https://zhuanlan.zhihu.com/p/94244568
代碼參考自：https://www.cnblogs.com/zyly/p/9366080.html
張正友標定法
相機標定的目的
當我們拿到一張圖片，進行識別之后，得到的兩部分之間的距離為多少像素，但是這多少像素究竟對應實際世界中的多少米呢？這就需要利用相機標定的結果來將像素坐標轉換到物理坐標來計算距離（僅僅利用單目相機標定的結果，是無法直接從像素坐標轉化到物理坐標的，因為透視投影丟失了一個維度的坐標，所以測距其實需要雙目相機）。
相機標定的第一個目的就是獲得相機的內參矩陣和外參矩陣。
相機標定的第二個目的就是獲得相機的畸變參數進而對拍攝的圖片進行去畸變處理。
簡介
張正友標定法利用如下圖所示的棋盤格標定板，在得到一張標定板的圖像之后，可以利用相應的圖像檢測算法得到每一個角點的像素坐標\((u,v)\)
張正友標定法將世界坐標系固定於棋盤格上，則棋盤格上任一點的物理坐標\(W = 0\),由於標定板的世界坐標系是人為事先定義好的，標定板上每一個格子的大小是已知的，我們可以計算得到每一個角點在世界坐標系下的物理坐標\((U,V,W = 0)\)
我們將利用這些信息：每一個角點的像素坐標\((u,v)\)、每一個角點在世界坐標系下的物理坐標\((U,V,W = 0)\)來進行相機的標定，獲得相機的內外參矩陣、畸變參數。
求解內外參數矩陣
將世界坐標系固定於棋盤格上，則棋盤格上任一點的物理坐標\(W = 0\)，因此，原單點無畸變的成像模型可以化為下式。
其中，\(R_1,R_2\)為旋轉矩陣\(R\)的前兩列，為了方便把內參矩陣記為\(A\)。
-
對於不同的圖片，內參矩陣\(A\)為定值；
-
對於同一張圖片，內參矩陣\(A\)，外參矩陣\((R_1\quad R_2\quad T)\)為定值；
-
對於同一組圖片上的單點，內參矩陣\(A\)，外參矩陣\((R_1\quad R_2\quad T)\)，尺度因子\(Z\)為定值
將\(A(R_1\quad R_2\quad T)\)記為矩陣\(H\)，\(H\)即為內參矩陣和外參矩陣的積，記矩陣H的三列為\((H_1,H_2,H_3)\)，則有
利用上式，消去尺度因子\(Z\)，可得
此時，尺度因子\(Z\)已經被消去，因此上式對於同一張圖片上所有的角點均成立。
\((u,v)\)是像素坐標系下的標定板角點的坐標，\((U,V)\)是世界坐標系下標定板角點的坐標。
通過圖像識別算法，可以得到標定板角點的像素坐標\((u,v)\)，又由於標定板的世界坐標系是人為定義好的，標定板上每一個格子的大小是已知的，我們可以計算得到世界坐標系下的\((U,V)\)
這里的\(H\)是齊次矩陣，有8個獨立未知元素。每一個標定板角點可以提供兩個約束方程（\(u,U,V\)的對應關系提供了兩個約束方程），因此當一張圖片上的標定板角點數量等於4時，即可求得該圖片對應的矩陣\(H\)，當一張圖片上的標定板角點數量大於4時，利用最小二乘法回歸最佳的矩陣\(H\)
求解內參矩陣
已知矩陣\(H = A(R1 \quad R2 \quad T)\)，接下來需要求解相機的內參矩陣\(A\)。
利用\(R1,R2\)作為旋轉矩陣\(R\)的兩列，存在單位正交的關系，即：
由\(H\)和\(R1,R2\)的關系可知：
代入可得：
述兩個約束方程中均存在矩陣\(A^{-T}A^{-1}\)，因此，記\(A^{-T}A^{-1} = B\) ，則\(B\)為對稱陣。
先求解矩陣\(B\)，通過矩陣B再求解相機的內參矩陣\(A\)。
為了簡便，我們記相機內參矩陣\(A\)為：
則：
則用矩陣\(A\)表示矩陣\(B\)得
注意：由於\(B\)為對稱陣，上式出現了兩次\(B_{12},B_{13},B_{23}\)
這里我們可以使用\(B= A^{-T}A^{-1}\)將前面通過\(R_1,R_2\)單位正交得到的約束方程化為：
因此，為了求解矩陣\(B\)，必須計算\(H_i^{T}BH_{j}\)，則
記：
則上式化為：
此時，通過\(R_1\quad R_2\)單位正交得到的約束方程可化為：
即：
其中，矩陣\(v = \left[\begin{matrix}v_{12}^T\\v_{11}^T-v_{22}^T\end{matrix}\right]\)
由於矩陣\(H\)已知，矩陣\(v\)又全部由矩陣\(H\)的元素構成，因此矩陣\(v\)已知
此時，我們只要求解出向量\(b\)，即可得到矩陣\(B\)。每張標定板圖片可以提供一個\(vb=0\)的約束關系，該約束關系含有兩個約束方程。但是，向量\(b\)有6個未知元素。因此，單張圖片提供的兩個約束方程不足以解出向量\(b\)。因此只取3張標定板照片，得到3個\(vb=0\)的約束關系，即6個方程，即可求解向量\(b\)。
當標定板圖片的個數大於3時（事實上一般需要15到20張標定板圖片），可采用最小二乘擬合最佳的向量\(b\)，並得到矩陣\(B\)。
根據矩陣\(B\)的元素和相機內參\(\alpha,\beta,\gamma,u_0,v_0\)的對應關系（上式），可得到：
即可求得相機的內參矩陣 \(A = \left(\begin{matrix}f_u & -f_ucot\theta & u_0\\0 & \frac{f_v}{sin\theta} & v_0\\0 & 0 & 1\end{matrix}\right) =\left[\begin{matrix}\alpha & \gamma & u_0\\0 & \beta & v_0\\0 & 0 & 1\end{matrix}\right]\)
求解外參矩陣
對於同一個相機，相機的內參矩陣取決於相機的內部參數，無論標定板和相機的位置關系是怎么樣的，相機的內參矩陣不變。這也正是在第2部分“求解內參矩陣”中，我們可以利用不同的圖片（標定板和相機位置關系不同）獲取的矩陣\(H\)，共同求解相機內參矩陣\(A\)的原因
但是，外參矩陣反映的是標定板和相機的位置關系。對於不同的圖片，標定板和相機的位置關系已經改變，此時每一張圖片對應的外參矩陣都是不同的。
在關系：\(A(R_1\quad R_2\quad T) = H\)中，已經求解得到了矩陣\(H\)，（對於同一張圖片相同，對於不同的圖片不同），矩陣\(A\)（對於不同的圖片都相同）
通過公式：\((R_1\quad R_2\quad T) = A^{-1}H\)即可求得每一張圖片對應的外參矩陣\((R_1\quad R_2\quad T)\)
雖然完整的外參矩陣為\(\left(\begin{matrix}R & T\\0 & 1\end{matrix}\right)\)，但由於張正友標定板將世界坐標系的原點選取在棋盤格上，則棋盤格上任一點的物理坐標 \(W=0\)，將旋轉矩陣的\(R\)的第三列\(R_3\)消掉，因此\(R_3\)在坐標轉化中並沒有作用，但是\(R_3\)要使得\(R\)滿足旋轉矩陣的性質，即列與列之間單位正交，因此可以通過向量\(R_1,R_2\)的叉乘，即\(R_3=R_1\times R_2\)，計算得到\(R_3\)
此時，相機的內參矩陣和外參矩陣均已得到
標定相機的畸變參數
張正友標定法僅僅考慮了畸變模型中影響較大的徑向畸變。
徑向畸變公式（2階）如下：
其中，\((x,y),(\hat{x},\hat{y})\)分別為理想的無畸變的歸一化的圖像坐標、畸變后的歸一化圖像坐標，\(r\)為圖像像素點到圖像中心點的距離，即\(r^2 = x^2 + y^2\)
圖像坐標和像素坐標的轉化關系為：
其中，\((u,v)\)為理想的無畸變的像素坐標。由於\(\theta\)接近90度，則上式近似為
同理可得畸變后的像素坐標\((\hat{u},\hat{v})\)的表達式為
代入徑向畸變公式（2階）則有：
化簡得：
即為：
上式中的\(\hat{u}, \hat{v}, u,v\)可以通過識別標定板的角點獲得，每一個角點可以構造兩個上述等式。
有m幅圖像，每幅圖像上有n個標定板角點，則將得到的所有等式組合起來，可以得到個mn未知數為的\(k = [k_1,k_2]^T\)的約束方程，將約束方程系數矩陣記為\(D\)，等式右端非齊次項記為\(d\)，可將其記作矩陣形式
則使用最小二乘法可求得：
此時，相機的畸變矯正參數已經標定好
步驟
-
准備一個張正友標定法的棋盤格，棋盤格大小已知，用相機對其進行不同角度的拍攝，得到一組圖像；
-
對圖像中的特征點如標定板角點進行檢測，得到標定板角點的像素坐標值，根據已知的棋盤格大小和世界坐標系原點，計算得到標定板角點的物理坐標值；
-
求解內參矩陣與外參矩陣
根據物理坐標值和像素坐標值的關系，求出\(H\)矩陣，進而構造\(v\)矩陣，求解\(B\)矩陣，利用\(B\)矩陣求解相機內參數\(A\)，最后求解每張照片對應的相機外參矩陣\(\left(\begin{matrix}R & T\\0 & 1\end{matrix}\right)\)
-
求解畸變參數
利用\(\hat{u}, \hat{v}, u,v\)構造\(D\)矩陣，計算徑向畸變參數
-
利用L-M（Levenberg-Marquardt）算法對上述參數進行優化
代碼
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <fstream>
#include <vector>
using namespace cv;
using namespace std;
void main(char *args)
{
//保存文件名稱
std::vector<std::string> filenames;
//需要更改的參數
//左相機標定，指定左相機圖片路徑，以及標定結果保存文件
string infilename = "sample/left/filename.txt"; //如果是右相機把left改為right
string outfilename = "sample/left/caliberation_result.txt";
//標定所用圖片文件的路徑,每一行保存一個標定圖片的路徑 ifstream 是從硬盤讀到內存
ifstream fin(infilename);
//保存標定的結果 ofstream 是從內存寫到硬盤
ofstream fout(outfilename);
/*
1.讀取毎一幅圖像，從中提取出角點，然后對角點進行亞像素精確化、獲取每個角點在像素坐標系中的坐標
像素坐標系的原點位於圖像的左上角
*/
std::cout << "開始提取角點......" << std::endl;;
//圖像數量
int imageCount = 0;
//圖像尺寸
cv::Size imageSize;
//標定板上每行每列的角點數
cv::Size boardSize = cv::Size(9, 6);
//緩存每幅圖像上檢測到的角點
std::vector<Point2f> imagePointsBuf;
//保存檢測到的所有角點
std::vector<std::vector<Point2f>> imagePointsSeq;
char filename[100];
if (fin.is_open())
{
//讀取完畢？
while (!fin.eof())
{
//一次讀取一行
fin.getline(filename, sizeof(filename) / sizeof(char));
//保存文件名
filenames.push_back(filename);
//讀取圖片
Mat imageInput = cv::imread(filename);
//讀入第一張圖片時獲取圖寬高信息
if (imageCount == 0)
{
imageSize.width = imageInput.cols;
imageSize.height = imageInput.rows;
std::cout << "imageSize.width = " << imageSize.width << std::endl;
std::cout << "imageSize.height = " << imageSize.height << std::endl;
}
std::cout << "imageCount = " << imageCount << std::endl;
imageCount++;
//提取每一張圖片的角點
if (cv::findChessboardCorners(imageInput, boardSize, imagePointsBuf) == 0)
{
//找不到角點
std::cout << "Can not find chessboard corners!" << std::endl;
exit(1);
}
else
{
Mat viewGray;
//轉換為灰度圖片
cv::cvtColor(imageInput, viewGray, cv::COLOR_BGR2GRAY);
//亞像素精確化 對粗提取的角點進行精確化
cv::find4QuadCornerSubpix(viewGray, imagePointsBuf, cv::Size(5, 5));
//保存亞像素點
imagePointsSeq.push_back(imagePointsBuf);
//在圖像上顯示角點位置
cv::drawChessboardCorners(viewGray, boardSize, imagePointsBuf, true);
//顯示圖片
//cv::imshow("Camera Calibration", viewGray);
cv::imwrite("test.jpg", viewGray);
//等待0.5s
//waitKey(500);
}
}
//計算每張圖片上的角點數 54
int cornerNum = boardSize.width * boardSize.height;
//角點總數
int total = imagePointsSeq.size()*cornerNum;
std::cout << "total = " << total << std::endl;
for (int i = 0; i < total; i++)
{
int num = i / cornerNum;
int p = i%cornerNum;
//cornerNum是每幅圖片的角點個數，此判斷語句是為了輸出，便於調試
if (p == 0)
{
std::cout << "\n第 " << num+1 << "張圖片的數據 -->: " << std::endl;
}
//輸出所有的角點
std::cout<<p+1<<":("<< imagePointsSeq[num][p].x;
std::cout << imagePointsSeq[num][p].y<<")\t";
if ((p+1) % 3 == 0)
{
std::cout << std::endl;
}
}
std::cout << "角點提取完成!" << std::endl;
/*
2.攝像機標定 世界坐標系原點位於標定板左上角(第一個方格的左上角)
*/
std::cout << "開始標定" << std::endl;
//棋盤三維信息，設置棋盤在世界坐標系的坐標
//實際測量得到標定板上每個棋盤格的大小
cv::Size squareSize = cv::Size(26, 26);
//毎幅圖片角點數量
std::vector<int> pointCounts;
//保存標定板上角點的三維坐標
std::vector<std::vector<cv::Point3f>> objectPoints;
//攝像機內參數矩陣 M=[fx γ u0,0 fy v0,0 0 1]
cv::Mat cameraMatrix = cv::Mat(3, 3, CV_64F, Scalar::all(0));
//攝像機的5個畸變系數k1,k2,p1,p2,k3
cv::Mat distCoeffs = cv::Mat(1, 5, CV_64F, Scalar::all(0));
//每幅圖片的旋轉向量
std::vector<cv::Mat> tvecsMat;
//每幅圖片的平移向量
std::vector<cv::Mat> rvecsMat;
//初始化標定板上角點的三維坐標
int i, j, t;
for (t = 0; t < imageCount; t++)
{
std::vector<cv::Point3f> tempPointSet;
//行數
for (i = 0; i < boardSize.height; i++)
{
//列數
for (j = 0; j < boardSize.width; j++)
{
cv::Point3f realPoint;
//假設標定板放在世界坐標系中z=0的平面上。
realPoint.x = i*squareSize.width;
realPoint.y = j*squareSize.height;
realPoint.z = 0;
tempPointSet.push_back(realPoint);
}
}
objectPoints.push_back(tempPointSet);
}
//初始化每幅圖像中的角點數量，假定每幅圖像中都可以看到完整的標定板
for (i = 0; i < imageCount; i++)
{
pointCounts.push_back(boardSize.width*boardSize.height);
}
//開始標定
cv::calibrateCamera(objectPoints, imagePointsSeq, imageSize, cameraMatrix, distCoeffs, rvecsMat, tvecsMat);
std::cout << "標定完成" << std::endl;
//對標定結果進行評價
std::cout << "開始評價標定結果......" << std::endl;
//所有圖像的平均誤差的總和
double totalErr = 0.0;
//每幅圖像的平均誤差
double err = 0.0;
//保存重新計算得到的投影點
std::vector<cv::Point2f> imagePoints2;
std::cout << "每幅圖像的標定誤差:" << std::endl;
fout << "每幅圖像的標定誤差:" << std::endl;
for (i = 0; i < imageCount; i++)
{
std::vector<cv::Point3f> tempPointSet = objectPoints[i];
//通過得到的攝像機內外參數，對空間的三維點進行重新投影計算，得到新的投影點imagePoints2（在像素坐標系下的點坐標）
cv::projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, imagePoints2);
//計算新的投影點和舊的投影點之間的誤差
std::vector<cv::Point2f> tempImagePoint = imagePointsSeq[i];
cv::Mat tempImagePointMat = cv::Mat(1, tempImagePoint.size(), CV_32FC2);
cv::Mat imagePoints2Mat = cv::Mat(1, imagePoints2.size(), CV_32FC2);
for (int j = 0; j < tempImagePoint.size(); j++)
{
imagePoints2Mat.at<cv::Vec2f>(0, j) = cv::Vec2f(imagePoints2[j].x, imagePoints2[j].y);
tempImagePointMat.at<cv::Vec2f>(0, j) = cv::Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
}
//Calculates an absolute difference norm or a relative difference norm.
err = cv::norm(imagePoints2Mat, tempImagePointMat, NORM_L2);
totalErr += err /= pointCounts[i];
std::cout << " 第" << i + 1 << "幅圖像的平均誤差：" << err << "像素" << endl;
fout<< "第" << i + 1 << "幅圖像的平均誤差：" << err << "像素" << endl;
}
//每張圖像的平均總誤差
std::cout << " 總體平均誤差:" << totalErr / imageCount << "像素" << std::endl;
fout << "總體平均誤差:" << totalErr / imageCount << "像素" << std::endl;
std::cout << "評價完成!" << std::endl;
//保存標定結果
std::cout << "開始保存標定結果....." << std::endl;
//保存每張圖像的旋轉矩陣
cv::Mat rotationMatrix = cv::Mat(3, 3, CV_32FC1, Scalar::all(0));
fout << "相機內參數矩陣:" << std::endl;
fout << cameraMatrix << std::endl << std::endl;
fout << "畸變系數:" << std::endl;
fout << distCoeffs << std::endl << std::endl;
for (int i = 0; i < imageCount; i++)
{
fout << "第" << i + 1 << "幅圖像的旋轉向量:" << std::endl;
fout << tvecsMat[i] << std::endl;
//將旋轉向量轉換為相對應的旋轉矩陣
cv::Rodrigues(tvecsMat[i], rotationMatrix);
fout << "第" << i + 1 << "幅圖像的旋轉矩陣:" << std::endl;
fout << rotationMatrix << std::endl;
fout << "第" << i + 1 << "幅圖像的平移向量:" << std::endl;
fout << rvecsMat[i] << std::endl;
}
std::cout << "保存完成" << std::endl;
/************************************************************************
顯示定標結果
*************************************************************************/
cv::Mat mapx = cv::Mat(imageSize, CV_32FC1);
cv::Mat mapy = cv::Mat(imageSize, CV_32FC1);
cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
std::cout << "顯示矯正圖像" << endl;
for (int i = 0; i != imageCount; i++)
{
std::cout << "Frame #" << i + 1 << "..." << endl;
//計算圖片畸變矯正的映射矩陣mapx、mapy(不進行立體校正、立體校正需要使用雙攝)
initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, imageSize, CV_32FC1, mapx, mapy);
//讀取一張圖片
Mat imageSource = imread(filenames[i]);
Mat newimage = imageSource.clone();
//另一種不需要轉換矩陣的方式
//undistort(imageSource,newimage,cameraMatrix,distCoeffs);
//進行校正
remap(imageSource, newimage, mapx, mapy, INTER_LINEAR);
imshow("原始圖像", imageSource);
imshow("矯正后圖像", newimage);
waitKey();
}
//釋放資源
fin.close();
fout.close();
system("pause");
}
}