双目三维重建/基于opencv的BM法，匹配两幅图像，并将三维点保存为txt文件

最新推荐文章于 2023-08-15 10:45:52 发布

qq_41862779

最新推荐文章于 2023-08-15 10:45:52 发布

阅读量1.3k

点赞数 1

本文链接：https://blog.csdn.net/qq_41862779/article/details/83411883

版权

/* 使用matlab标定工具箱得到的相机参数*/

两张图片尺寸，640*360. 下载地址

https://download.csdn.net/download/qq_41862779/10750370

*/
#include <opencv2/opencv.hpp>
#include <iostream>

using namespace std;
using namespace cv;

const int imageWidth = 640; //摄像头单目的分辨率########--【需要调整参数的位置1】--#############
const int imageHeight = 360;

Size imageSize = Size(imageWidth, imageHeight);

Mat rgbImageL, grayImageL;
Mat rgbImageR, grayImageR;
Mat rectifyImageL, rectifyImageR;

Rect validROIL; //图像校正之后，会对图像进行裁剪，这里的validROI就是指裁剪之后的区域
Rect validROIR;

Mat mapLx, mapLy, mapRx, mapRy; //映射表
Mat Rl, Rr, Pl, Pr, Q; //校正旋转矩阵R，投影矩阵P, 重投影矩阵Q
Mat xyz; //三维坐标
string point_cloud;

Point origin; //鼠标按下的起始点
Rect selection; //定义矩形选框
bool selectObject = false; //是否选择对象

int blockSize = 1, uniquenessRatio = 38, numDisparities = 4; //与算法相关的参数，【需要调整参数的位置2】--############
//int blockSize = 6, uniquenessRatio = 30, numDisparities = 9;
Ptr<StereoBM> bm = StereoBM::create(16, 9);

static void saveXYZ(const Mat& mat)
{
   const double max_z = 1.0e4;
   FILE* fp = fopen("point_cloud.txt", "wt");
   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;
           fprintf(fp, "%f %f %f\n", point[0], point[1], point[2]);
       }
   }
   fclose(fp);
}

/*
Intrinsic parameters of left camera :

Focal Length : fc_left = [570.53941 570.43311] ? [1.23458 1.25747]
Principal point : cc_left = [338.37062 157.92971] ? [1.22416 0.85779]
Skew : alpha_c_left = [0.00000] ? [0.00000] = > angle of pixel axes = 90.00000 ? 0.00000 degrees
Distortion : kc_left = [0.07076 - 0.11917 - 0.00301 - 0.00229 0.00000] ? [0.00686 0.03596 0.00061 0.00089 0.00000]

Intrinsic parameters of right camera :

Focal Length : fc_right = [574.57663 574.96555] ? [1.20778 1.23299]
Principal point : cc_right = [331.42666 161.20258] ? [1.23783 0.87354]
Skew : alpha_c_right = [0.00000] ? [0.00000] = > angle of pixel axes = 90.00000 ? 0.00000 degrees
Distortion : kc_right = [0.04656 - 0.10958 - 0.00220 - 0.00203 0.00000] ? [0.00693 0.03685 0.00059 0.00085 0.00000]

Extrinsic parameters(position of right camera wrt left camera) :

Rotation vector : om = [0.00867 0.00611 - 0.00192] ? [0.00196 0.00292 0.00016]
Translation vector : T = [-59.69230 - 0.06008 0.68340] ? [0.17030 0.15721 0.78782]

Note : The numerical errors are approximately three times the standard deviations(for reference).
* /

/*左目相机标定参数------------------------
fc_left_x 0 cc_left_x
0 fc_left_y cc_left_y
0 0 1
-----------------------------------------*/

Mat cameraMatrixL = (Mat_<double>(3, 3) << 570.53941, 0, 338.37062,
0, 570.43311, 157.92971,
0, 0, 1);

Mat distCoeffL = (Mat_<double>(5, 1) << 0.07076, -0.11917, -0.00301, -0.00229, 0.00000);
//[kc_left_01, kc_left_02, kc_left_03, kc_left_04, kc_left_05]

/*右目相机标定参数------------------------
fc_right_x 0 cc_right_x
0 fc_right_y cc_right_y
0 0 1
-----------------------------------------*/
Mat cameraMatrixR = (Mat_<double>(3, 3) << 574.57663, 0, 331.42666,
0, 574.96555, 161.20258,
0, 0, 1);

Mat distCoeffR = (Mat_<double>(5, 1) << 0.04656, -0.10958, -0.00220, -0.00203, 0.00000);
//[kc_right_01, kc_right_02, kc_right_03, kc_right_04, kc_right_05]

Mat T = (Mat_<double>(3, 1) << -59.69230, -0.06008, 0.68340); //T平移向量
//[T_01, T_02, T_03]

Mat rec = (Mat_<double>(3, 1) << 0.00867, 0.00611, -0.00192); //rec旋转向量
//[rec_01, rec_02, rec_03]

//########--【以下双目的标定参数为：需要调整参数的位置3】--#############
//相机双目标定的结果与如下各参数的对应关系见：双目标定结果说明.pdf，pdf文档位于main.cpp（即本文档）同级文件夹--#############

/*左目相机标定参数------------------------
fc_left_x 0 cc_left_x
0 fc_left_y cc_left_y
0 0 1
-----------------------------------------*/
/*
Mat cameraMatrixL = (Mat_<double>(3, 3) << 1450.45938, 0, 579.88716,
0, 1452.62035, 376.32695,
0, 0, 1);

Mat distCoeffL = (Mat_<double>(5, 1) << 0.03569, 0.29314, -0.00011, -0.00491, 0.00000);
//[kc_left_01, kc_left_02, kc_left_03, kc_left_04, kc_left_05]

*/
/*右目相机标定参数------------------------
fc_right_x 0 cc_right_x
0 fc_right_y cc_right_y
0 0 1
-----------------------------------------*/
/*
Mat cameraMatrixR = (Mat_<double>(3, 3) << 1451.78149, 0, 684.04159,
0, 1453.54807, 350.52935,
0, 0, 1);

Mat distCoeffR = (Mat_<double>(5, 1) << 0.09596, -0.42760, -0.00378, -0.00112, 0.00000);
//[kc_right_01, kc_right_02, kc_right_03, kc_right_04, kc_right_05]

Mat T = (Mat_<double>(3, 1) << -28.11909, 0.11966, 0.21590); //T平移向量
//[T_01, T_02, T_03]

Mat rec = (Mat_<double>(3, 1) << -0.01688, -0.00781, -0.00766); //rec旋转向量
//[rec_01, rec_02, rec_03]
*/
//########--双目的标定参数填写完毕----------------------------------------------

Mat R; //R矩阵，用于中间计算

//--立体匹配--------------------------------------------------------------------
void stereo_match(int, void*)
{
   bm->setBlockSize(2 * blockSize + 5); //SAD窗口大小，5~21之间为宜
   bm->setROI1(validROIL);
   bm->setROI2(validROIR);
   bm->setPreFilterCap(31);
   bm->setMinDisparity(0); //最小视差，默认值为0, 可以是负值，int型
   bm->setNumDisparities(numDisparities * 16 + 16); //视差窗口，即最大视差值与最小视差值之差,窗口大小必须是16的整数倍，int型
   bm->setTextureThreshold(10);
   bm->setUniquenessRatio(uniquenessRatio); //uniquenessRatio主要可以防止误匹配
   bm->setSpeckleWindowSize(100);
   bm->setSpeckleRange(32);
   bm->setDisp12MaxDiff(-1);
   Mat disp, disp8;
   bm->compute(rectifyImageL, rectifyImageR, disp); //输入图像必须为灰度图
   disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16)*16.)); //计算出的视差是CV_16S格式
   reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
   xyz = xyz * 16;
   /*
   for (int y = 0; y < xyz.rows; y++)
   {
       for (int x = 0; x < xyz.cols; x++)
       {
           Vec3f point = xyz.at<Vec3f>(y, x);
           if (fabs(point[2] - 1.0e4) < FLT_EPSILON || fabs(point[2]) > 1.0e4) continue;
           cout << point[0] << " " << point[1] << " " << point[2] << endl;
       }
   }
   */
   //cout << xyz;

imshow("disparity", disp8);
}

//--描述：鼠标操作回调--------------------------------------------------
static void onMouse(int event, int x, int y, int, void*)
{
   if (selectObject)
   {
       selection.x = MIN(x, origin.x);
       selection.y = MIN(y, origin.y);
       selection.width = std::abs(x - origin.x);
       selection.height = std::abs(y - origin.y);
   }

   switch (event)
   {
   case EVENT_LBUTTONDOWN: //鼠标左按钮按下的事件
       origin = Point(x, y);
       selection = Rect(x, y, 0, 0);
       selectObject = true;
       cout << origin << "in world coordinate is: " << xyz.at<Vec3f>(origin) << endl;
       break;
   case EVENT_LBUTTONUP: //鼠标左按钮释放的事件
       selectObject = false;
       if (selection.width > 0 && selection.height > 0)
           break;
   }
}

//--主函数---------------------------------------------------------------------
int main()
{

   //--立体校正-------------------------------------------------------------------
   Rodrigues(rec, R); //Rodrigues变换
   stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
       0, imageSize, &validROIL, &validROIR);
   initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pr, imageSize, CV_32FC1, mapLx, mapLy);
   initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);

   //--读取图片，【需要调整参数的位置4】----------------------------------------------------------------
   rgbImageL = imread("left02.jpg", CV_LOAD_IMAGE_COLOR);
   cvtColor(rgbImageL, grayImageL, CV_BGR2GRAY);
   rgbImageR = imread("right02.jpg", CV_LOAD_IMAGE_COLOR);
   cvtColor(rgbImageR, grayImageR, CV_BGR2GRAY);
   cout << rgbImageL.size() << endl;
   cout << rgbImageR.size() << endl;
   namedWindow("ImageL Before Rectify", WINDOW_NORMAL); imshow("ImageL Before Rectify", grayImageL);
   namedWindow("ImageR Before Rectify", WINDOW_NORMAL); imshow("ImageR Before Rectify", grayImageR);

   //--经过remap之后，左右相机的图像已经共面并且行对准----------------------------------------------
   remap(grayImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
   remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);

   //--把校正结果显示出来---------------------------------------------------------------------------
   Mat rgbRectifyImageL, rgbRectifyImageR;
   cvtColor(rectifyImageL, rgbRectifyImageL, CV_GRAY2BGR);
   cvtColor(rectifyImageR, rgbRectifyImageR, CV_GRAY2BGR);

namedWindow("ImageL After Rectify", WINDOW_NORMAL); imshow("ImageL After Rectify", rgbRectifyImageL);
namedWindow("ImageR After Rectify", WINDOW_NORMAL); imshow("ImageR After Rectify", rgbRectifyImageR);

   //--显示在同一张图上-----------------------------------------------------------------------------
   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); //注意通道

//--左图像画到画布上-----------------------------------------------------------------------------
   Mat canvasPart = canvas(Rect(w * 0, 0, w, h)); //得到画布的一部分
   resize(rgbRectifyImageL, canvasPart, canvasPart.size(), 0, 0, INTER_AREA); //把图像缩放到跟canvasPart一样大小
   Rect vroiL(cvRound(validROIL.x*sf), cvRound(validROIL.y*sf), //获得被截取的区域
       cvRound(validROIL.width*sf), cvRound(validROIL.height*sf));
   rectangle(canvasPart, vroiL, Scalar(0, 0, 255), 3, 8);
   cout << "Painted ImageL" << endl;

   //--右图像画到画布上-----------------------------------------------------------------------------
   canvasPart = canvas(Rect(w, 0, w, h)); //获得画布的另一部分
   resize(rgbRectifyImageR, canvasPart, canvasPart.size(), 0, 0, INTER_LINEAR);
   Rect vroiR(cvRound(validROIR.x * sf), cvRound(validROIR.y*sf),
       cvRound(validROIR.width * sf), cvRound(validROIR.height * sf));
   rectangle(canvasPart, vroiR, Scalar(0, 0, 255), 3, 8);
   cout << "Painted ImageR" << endl;

   //--画上对应的线条-------------------------------------------------------------------------------
   for (int i = 0; i < canvas.rows; i += 16)
       line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);
   namedWindow("rectified", WINDOW_NORMAL); imshow("rectified", canvas);

//--显示结果-------------------------------------------------------------------------------------
namedWindow("disparity", WINDOW_NORMAL);

//--创建SAD窗口 Trackbar-------------------------------------------------------------------------
createTrackbar("BlockSize:\n", "disparity", &blockSize, 8, stereo_match);

//--创建视差唯一性百分比窗口 Trackbar------------------------------------------------------------
createTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, 50, stereo_match);

//--创建视差窗口 Trackbar------------------------------------------------------------------------
createTrackbar("NumDisparities:\n", "disparity", &numDisparities, 16, stereo_match);

   //--鼠标响应函数setMouseCallback(窗口名称, 鼠标回调函数, 传给回调函数的参数，一般取0)------------
   setMouseCallback("disparity", onMouse, 0);
   stereo_match(0, 0);

   saveXYZ(xyz); //在主函数中调用函数。
   waitKey(0);
   return 0;
}