最近做双目项目需要进行标定,网上查看一些资料。目前所用是先进行单目标定,然后在进行双目标定。代码如下,配以后使用
//biaoding.cpp
#include<opencv2/calib3d/calib3d.hpp>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
#include<opencv2/opencv.hpp>
#include <opencv2/imgproc/types_c.h>
#include <opencv2/calib3d/calib3d_c.h>
#include<iostream>
#include <dirent.h>
using namespace std;
using namespace cv;
int n = 1;
int m = 1;
int i = 1;
int j = 1;
//需要按实际情况修改=======
const int boardwidth = 8; //棋盘格内角个数
const int boardhight = 6; //棋盘格内角个数
const int squaresize = 27.5; //棋盘格变长,mm
cv::Size imagesize ;
const cv::Size boardsize = cv::Size(boardwidth,boardhight);
//======================
vector<cv::Point3f> objectpoint;
vector<vector<cv::Point3f>> objpoint;
vector<cv::Point2f> cornerL;
vector<cv::Point2f> cornerR;
vector<vector<cv::Point2f>> imagepointL;
vector<vector<cv::Point2f>> imagepointR;
Mat cameraMatrixL = Mat(3, 3, CV_32FC1, Scalar::all(0));
Mat cameraMatrixR = Mat(3, 3, CV_32FC1, Scalar::all(0));
vector<Mat> rvecsMat_left; // 存放所有图像的旋转向量,每一副图像的旋转向量为一个mat
vector<Mat> tvecsMat_left; // 存放所有图像的平移向量,每一副图像的平移向量为一个mat
Mat distcoeffL = Mat(1, 5, CV_32FC1, Scalar::all(0));
Mat distcoeffR = Mat(1, 5, CV_32FC1, Scalar::all(0));
vector<Mat> rvecsMat_right; // 存放所有图像的旋转向量,每一副图像的旋转向量为一个mat
vector<Mat> tvecsMat_right; // 存放所有图像的平移向量,每一副图像的平移向量为一个mat
cv::Mat R,T,E,F;
cv::Mat R1, R2, P1, P2 , Q;
cv::Mat maplx, maply, maprx, mapry;
cv::Mat imageL, grayimageL;
cv::Mat imageR, grayimageR;
cv::Rect validROIL, validROIR;
//单目标定
void m_calibration(vector<vector<Point2f>>& image_points_seq, Size board_size, Size square_size, Mat& cameraMatrix, Mat& distCoeffs, vector<Mat>& rvecsMat, vector<Mat>& tvecsMat,int image_count)
{
/*棋盘三维信息*/
vector<vector<Point3f>> object_points_seq; // 保存标定板上角点的三维坐标
for (int t = 0; t < image_count; t++)
{
vector<Point3f> object_points;
for (int i = 0; i < board_size.height; i++)
{
for (int 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;
object_points.push_back(realPoint);
}
}
object_points_seq.push_back(object_points);
}
/* 运行标定函数 */
double err_first = calibrateCamera(object_points_seq, image_points_seq, imagesize, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, CV_CALIB_FIX_K3);
cout << "重投影误差1:" << err_first << "像素" << endl << endl;
cout << "标定完成!!!" << endl;
cout << "开始评价标定结果………………";
double total_err = 0.0; // 所有图像的平均误差的总和
double err = 0.0; // 每幅图像的平均误差
double totalErr = 0.0;
double totalPoints = 0.0;
vector<Point2f> image_points_pro; // 保存重新计算得到的投影点
for (int i = 0; i < image_count; i++)
{
projectPoints(object_points_seq[i], rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points_pro); //通过得到的摄像机内外参数,对角点的空间三维坐标进行重新投影计算
err = norm(Mat(image_points_seq[i]), Mat(image_points_pro), NORM_L2);
totalErr += err * err;
totalPoints += object_points_seq[i].size();
err /= object_points_seq[i].size();
//fout << "第" << i + 1 << "幅图像的平均误差:" << err << "像素" << endl;
total_err += err;
}
cout << "重投影误差2:" << sqrt(totalErr / totalPoints) << "像素" << endl << endl;
cout << "重投影误差3:" << total_err / image_count << "像素" << endl << endl;
//保存定标结果
cout << "开始保存定标结果………………" << endl;
Mat rotation_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 保存每幅图像的旋转矩阵 */
cout << "相机内参数矩阵:" << endl;
cout << cameraMatrix << endl << endl;
cout << "畸变系数:\n";
cout << distCoeffs << endl << endl << endl;
for (int i = 0; i < image_count; i++)
{
Rodrigues(rvecsMat[i], rotation_matrix);
}
cout << "定标结果完成保存!!!" << endl;
}
void worldpoint(int framenumber)
{
for (int i = 0; i < boardhight; i++)
{
for (int j = 0; j < boardwidth; j++)
{
objectpoint.push_back(cv::Point3f(i*squaresize,j*squaresize,0.0f));
}
}
for (int w = 1; w <= framenumber;w++)
{
objpoint.push_back(objectpoint);
}
}
void outputparam()
{
cv::FileStorage fs("intrinsics.yml", cv::FileStorage::WRITE);
fs << "cameraMatrixL" << cameraMatrixL << "cameraDistcoeffL" << distcoeffL << "cameraMatrixR" << cameraMatrixR << "cameraDistcoeffR" << distcoeffR;
fs.release();
std::cout << "cameraMatrixL=:" << cameraMatrixL << endl << "cameraDistcoeffL=:" << distcoeffL << endl << "cameraMatrixR=:" << cameraMatrixR << endl << "cameraDistcoeffR=:" << distcoeffR << endl;
fs.open("extrinsics.yml", cv::FileStorage::WRITE);
fs << "R" << R << "T" << T << "Rl" << R1 << "Rr" << R2 << "Pl" << P1 << "Pr" << P2 << "Q" << Q;
std::cout << "R=" << R << endl << "T=" << T << endl << "Rl=" << R1 << endl << "Rr=" << R2 << endl << "Pl=" << P1 << endl << "Pr=" << P2 << endl << "Q=" << Q << endl;
fs.release();
}
int getFilesName(string path)
{
DIR *pDir;
std::vector<std::string> filenames;
struct dirent* ptr;
if(!(pDir = opendir(path.c_str())))
return -1;
while((ptr = readdir(pDir))!=0) {
if (strcmp(ptr->d_name, ".") != 0 && strcmp(ptr->d_name, "..") != 0)
filenames.push_back(path + "/" + ptr->d_name);
}
closedir(pDir);
return filenames.size();
}
int main()
{
//读取图片
char* leftStr="/home/×××/×××/biaoding/left/";
char* rightStr="/home/×××/×××/biaoding/right/";
int framenumberleft = getFilesName(leftStr);
int framenumberright = getFilesName(leftStr);
assert(framenumberleft==framenumberright);
int framenumber = framenumberleft;
int current = 1;
std::cout<<framenumber<<endl;
while (current <= framenumber)
{
char frameL[50];
snprintf(frameL, sizeof(frameL),"%s%d.jpg", leftStr,n++);
imageL = cv::imread(frameL);
cv::cvtColor(imageL,grayimageL,cv::ColorConversionCodes::COLOR_BGR2GRAY);
char frameR[50];
snprintf(frameR, sizeof(frameR), "%s%d.jpg",rightStr, m++);
imageR = cv::imread(frameR);
if (current == 1) //读入第一张图片时获取图像宽高信息
{
imagesize = cv::Size(imageL.cols,imageL.rows);
cout << "image_size.width = " << imagesize<< endl;
}
cv::cvtColor(imageR, grayimageR, cv::ColorConversionCodes::COLOR_BGR2GRAY);
bool foundL, foundR;
foundL = cv::findChessboardCorners(imageL,boardsize,cornerL);
foundR = cv::findChessboardCorners(imageR, boardsize, cornerR);
if (foundL == true && foundR == true)
{
cv::cornerSubPix(grayimageL,cornerL,cv::Size(11,11),cv::Size(-1,-1),cv::TermCriteria(cv::TermCriteria::MAX_ITER|cv::TermCriteria::EPS, 30, 1e-6));
cv::cornerSubPix(grayimageR, cornerR, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS, 30, 1e-6));
cv::drawChessboardCorners(imageL,boardsize,cornerL,foundL);
cv::imshow("L", imageL);
cv::waitKey(10);
cv::drawChessboardCorners(imageR, boardsize, cornerR, foundR);
cv::imshow("R", imageR);
cv::waitKey(10);
imagepointL.push_back(cornerL);
imagepointR.push_back(cornerR);
cout << "The image " << current << " is good" << endl;
}
else
{
std::cout << "The image is bad please try again" << endl;
}
current++;
}
cout << "左右单目开始标定" << endl;
//进行left标定
std::cout<<"current:"<<current<<endl;
//左图
std::cout<<"============================ left start==========================="<<std::endl;
m_calibration(imagepointL, boardsize, Size(squaresize,squaresize), cameraMatrixL, distcoeffL, rvecsMat_left, tvecsMat_left,current-1);
//右图
std::cout<<"============================ right start==========================="<<std::endl;
m_calibration(imagepointR, boardsize, Size(squaresize,squaresize), cameraMatrixR, distcoeffR, rvecsMat_right, tvecsMat_right,current-1);
std::cout<<"============================ left re==========================="<<std::endl;
cout << "相机内参数矩阵:" << endl;
cout << cameraMatrixL << endl << endl;
cout << "畸变系数:\n";
cout << distcoeffL << endl << endl << endl;
std::cout<<"============================ right re==========================="<<std::endl;
cout << "相机内参数矩阵:" << endl;
cout << cameraMatrixR << endl << endl;
cout << "畸变系数:\n";
cout << distcoeffR << endl << endl << endl;
worldpoint(framenumber);
cout << "双目开始标定" << endl;
double err = cv::stereoCalibrate(objpoint, imagepointL, imagepointR, cameraMatrixL, distcoeffL, cameraMatrixR, distcoeffR, imagesize, R, T, E, F,
CALIB_USE_INTRINSIC_GUESS,
cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 1e-6));
cout << "The err = " << err << endl;
cv::stereoRectify(cameraMatrixL,distcoeffL, cameraMatrixR,distcoeffR,imagesize,R,T,R1,R2,P1,P2,Q, cv::CALIB_ZERO_DISPARITY, -1, imagesize, &validROIL, &validROIR);
cv::initUndistortRectifyMap(cameraMatrixL,distcoeffL,R1,P1,imagesize, CV_32FC1,maplx,maply);
cv::initUndistortRectifyMap(cameraMatrixR,distcoeffR,R2,P2,imagesize,CV_32FC1,maprx,mapry);
outputparam();
cv::Mat canvas;
double sf;
int w, h;
sf = 600. / MAX(imagesize.width, imagesize.height);
w = cvRound(imagesize.width * sf);
h = cvRound(imagesize.height * sf);
canvas.create(h, w * 2, CV_8UC3);
int currents = 1;
while (currents <= framenumber)
{
char frameL[50];
snprintf(frameL, sizeof(frameL), "%s%d.jpg",leftStr, i++);
imageL = cv::imread(frameL);
char frameR[50];
snprintf(frameR, sizeof(frameR), "%s%d.jpg",rightStr, j++);
imageR = cv::imread(frameR);
cv::Mat rectifyImageL2, rectifyImageR2;
cv::remap(imageL, rectifyImageL2, maplx, maply, cv::InterpolationFlags::INTER_LINEAR);
cv::remap(imageR, rectifyImageR2, maprx, mapry, cv::InterpolationFlags::INTER_LINEAR);
cv::Mat canvasPart = canvas(cv::Rect(w * 0, 0, w, h));
resize(rectifyImageL2, canvasPart, canvasPart.size(), 0, 0, cv::INTER_AREA);
cv::Rect vroiL(cvRound(validROIL.x*sf), cvRound(validROIL.y*sf),
cvRound(validROIL.width*sf), cvRound(validROIL.height*sf));
cv::rectangle(canvasPart, vroiL, cv::Scalar(0, 0, 255), 3, 8);
canvasPart = canvas(cv::Rect(w, 0, w, h));
resize(rectifyImageR2, canvasPart, canvasPart.size(), 0, 0, cv::INTER_LINEAR);
cv::Rect vroiR(cvRound(validROIR.x * sf), cvRound(validROIR.y*sf),
cvRound(validROIR.width * sf), cvRound(validROIR.height * sf));
cv::rectangle(canvasPart, vroiR, cv::Scalar(0, 255, 0), 3, 8);
for (int i = 0; i < canvas.rows; i += 16)
line(canvas, cv::Point(0, i), cv::Point(canvas.cols, i), cv::Scalar(0, 255, 0), 1, 8);
cv::imshow("rectified", canvas);
if (cv::waitKey() > 0)
{
currents++;
}
}
return 0;
}
//编译
g++ biaoding.cpp -o biao `pkg-config opencv4 --cflags --libs`