基于C++实现的双目摄像头三维重建源码(相机标定+矫正+获取点云坐标+体积估计)+超详细注释.zip

#include <opencv2/opencv.hpp> #include <opencv2\imgproc\imgproc.hpp> #include <opencv2\core\core.hpp> #include <opencv2\highgui\highgui.hpp> #include <opencv2\calib3d\calib3d.hpp> #include <opencv2\features2d\features2d.hpp> #include <opencv2\legacy\legacy.hpp> #include <iostream> #include <fstream> #include <vector> #include <list> #include <algorithm> #include <iterator> #include <cstdio> #include <string> using namespace cv; using namespace std; typedef unsigned int uint; Size imgSize(1280, 720); Size patSize(14, 12); //每张棋盘寻找的角点个数是14*12个 const double patLen = 5.0f; // unit: mm 标定板每个格的宽度（金属标定板） double imgScale = 1.0; //图像缩放的比例因子 //将要读取的图片路径存储在fileList中 vector<string> fileList; void initFileList(string dir, int first, int last){ fileList.clear(); for(int cur = first; cur <= last; cur++){ string str_file = dir + "/" + to_string(cur) + ".jpg"; fileList.push_back(str_file); } } // 生成点云坐标后保存 static void saveXYZ(string filename, const Mat& mat) { const double max_z = 1.0e4; ofstream fp(filename); if (!fp.is_open()) { std::cout<<"打开点云文件失败"<<endl; fp.close(); return ; } //遍历写入 for(int y = 0; y < mat.rows; y++) { for(int x = 0; x < mat.cols; x++) { Vec3f point = mat.at<Vec3f>(y, x); //三通道浮点型 if(fabs(point[2] - max_z) < FLT_EPSILON || fabs(point[2]) > max_z) continue; fp<<point[0]<<" "<<point[1]<<" "<<point[2]<<endl; } } fp.close(); } // 存储视差数据 void saveDisp(const string filename, const Mat& mat) { ofstream fp(filename, ios::out); fp<<mat.rows<<endl; fp<<mat.cols<<endl; for(int y = 0; y < mat.rows; y++) { for(int x = 0; x < mat.cols; x++) { double disp = mat.at<short>(y, x); // 这里视差矩阵是CV_16S 格式的，故用 short 类型读取 fp<<disp<<endl; // 若视差矩阵是 CV_32F 格式，则用 float 类型读取 } } fp.close(); } void F_Gray2Color(Mat gray_mat, Mat& color_mat) { color_mat = Mat::zeros(gray_mat.size(), CV_8UC3); int rows = color_mat.rows, cols = color_mat.cols; Mat red = Mat(gray_mat.rows, gray_mat.cols, CV_8U); Mat green = Mat(gray_mat.rows, gray_mat.cols, CV_8U); Mat blue = Mat(gray_mat.rows, gray_mat.cols, CV_8U); Mat mask = Mat(gray_mat.rows, gray_mat.cols, CV_8U); subtract(gray_mat, Scalar(255), blue); // blue(I) = 255 - gray(I) red = gray_mat.clone(); // red(I) = gray(I) green = gray_mat.clone(); // green(I) = gray(I),if gray(I) < 128 compare(green, 128, mask, CMP_GE); // green(I) = 255 - gray(I), if gray(I) >= 128 subtract(green, Scalar(255), green, mask); convertScaleAbs(green, green, 2.0, 2.0); vector<Mat> vec; vec.push_back(red); vec.push_back(green); vec.push_back(blue); cv::merge(vec, color_mat); } Mat F_mergeImg(Mat img1, Mat disp8){ Mat color_mat = Mat::zeros(img1.size(), CV_8UC3); Mat red = img1.clone(); Mat green = disp8.clone(); Mat blue = Mat::zeros(img1.size(), CV_8UC1); vector<Mat> vec; vec.push_back(red); vec.push_back(blue); vec.push_back(green); cv::merge(vec, color_mat); return color_mat; } // 双目立体标定 int stereoCalibrate(string intrinsic_filename="intrinsics.yml", string extrinsic_filename="extrinsics.yml") { vector<int> idx; //左侧相机的角点坐标和右侧相机的角点坐标 vector<vector<Point2f>> imagePoints[2]; //vector<vector<Point2f>> leftPtsList(fileList.size()); //vector<vector<Point2f>> rightPtsList(fileList.size()); for(uint i = 0; i < fileList.size();++i) { vector<Point2f> leftPts, rightPts; // 存储左右相机的角点位置 Mat rawImg = imread(fileList[i]); //原始图像 if(rawImg.empty()){ std::cout<<"the Image is empty..."<<fileList[i]<<endl; continue; } //截取左右图片 Rect leftRect(0, 0, imgSize.width, imgSize.height); Rect rightRect(imgSize.width, 0, imgSize.width, imgSize.height); Mat leftRawImg = rawImg(leftRect); //切分得到的左原始图像 Mat rightRawImg = rawImg(rightRect); //切分得到的右原始图像 //imwrite("left.jpg", leftRawImg); //imwrite("right.jpg", rightRawImg); //std::cout<<"左侧图像： 宽度"<<leftRawImg.size().width<<" 高度"<<rightRawImg.size().height<<endl; //std::cout<<"右侧图像： 宽度"<<rightRawImg.size().width<<" 高度"<<rightRawImg.size().height<<endl; // Mat leftImg, rightImg, leftSimg, rightSimg, leftCimg, rightCimg, leftMask, rightMask; // BGT -> GRAY if(leftRawImg.type() == CV_8UC3) cvtColor(leftRawImg, leftImg, CV_BGR2GRAY); //转为灰度图 else leftImg = leftRawImg.clone(); if(rightRawImg.type() == CV_8UC3) cvtColor(rightRawImg, rightImg, CV_BGR2GRAY); else rightImg = rightRawImg.clone(); imgSize = leftImg.size(); //图像滤波预处理 resize(leftImg, leftMask, Size(200, 200)); //resize对原图像img重新调整大小生成mask图像大小为200*200 resize(rightImg, rightMask, Size(200, 200)); GaussianBlur(leftMask, leftMask, Size(13, 13), 7); GaussianBlur(rightMask, rightMask, Size(13, 13), 7); resize(leftMask, leftMask, imgSize); resize(rightMask, rightMask, imgSize); medianBlur(leftMask, leftMask, 9); //中值滤波 medianBlur(rightMask, rightMask, 9); for (int v = 0; v < imgSize.height; v++) { for (int u = 0; u < imgSize.width; u++) { int leftX = ((int)leftImg.at<uchar>(v, u) - (int)leftMask.at<uchar>(v, u)) * 2 + 128; int rightX = ((int)rightImg.at<uchar>(v, u) - (int)rightMask.at<uchar>(v, u)) * 2 + 128; leftImg.at<uchar>(v, u) = max(min(leftX, 255), 0); rightImg.at<uchar>(v, u) = max(min(rightX, 255), 0); } } //寻找角点， 图像缩放 resize(leftImg, leftSimg, Size(), imgScale, imgScale); //图像以0.5的比例缩放 resize(rightImg, rightSimg, Size(), imgScale, imgScale); cvtColor(leftSimg, leftCimg, CV_GRAY2BGR); //转为BGR图像，cimg和simg是800*600的图像 cvtColor(rightSimg, rightCimg, CV_GRAY2BGR); //寻找棋盘角点 bool leftFound = findChessboardCorners(leftCimg, patSize, leftPts, CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_FILTER_QUADS); bool rightFound = findChessboardCorners(rightCimg, patSize, rightPts, CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_FILTER_QUADS); if(leftFound) cornerSubPix(leftSimg, leftPts, Size(11, 11), Size(-1,-1 ), TermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 300, 0.01)); if(rightFound) cornerSubPix(rightSimg, rightPts, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 300, 0.01)); //亚像素 //放大为原来的尺度 for(uint j = 0;j < leftPts.size();j++) //该幅图像共132个角点，坐标乘以2，还原角点位置 leftPts[j] *= 1./imgScale; for(uint j = 0;j < rightPts.size();j++) rightPts[j] *= 1./imgScale; //显示 string leftWindowName = "Left Corner Pic", rightWindowName = "Right Corner Pic"; Mat leftPtsTmp = Mat(leftPts) * imgScale; //再次乘以 imgScale Mat rightPtsTmp = Mat(rightPts) * imgScale; drawChessboardCorners(leftCimg, patSize, leftPtsTmp, leftFound); //绘制角点坐标并显示 imshow(leftWindowName, leftCimg); imwrite("输出/DrawChessBoard/"+to_string(i)+"_left.jpg", leftCimg); waitKey(200); drawChessboardCorners(rightCimg, patSize, rightPtsTmp, rightFound); //绘制角点坐标并显示 imshow(rightWindowName, rightCimg); imwrite("输出/DrawChessBoard/"+to_string(i)+"_right.jpg", rightCimg); waitKey(200); cv::destroyAllWindows(); //保存角点坐标 if(leftFound && rightFound) { imagePoints[0].push_back(leftPts); imagePoints[1].push_back(rightPts); //保存角点坐标 std::cout<<"图片 "<<i<<" 处理成功！"<<endl; idx.push_back(i); } } cv::dest