可用的双目标定代码（先单目标定再双目标定）

最近做双目项目需要进行标定，网上查看一些资料。目前所用是先进行单目标定，然后在进行双目标定。代码如下，配以后使用

//biaoding.cpp
#include
#include
#include
#include
#include
#include 
#include 
#include
#include 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;                                          // 存放所有图像的旋转向量，每一副图像的旋转向量为一个matvector<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`

参考

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！