基于C++ opencv的双目视觉进行三维物体测量

// opencv_test.cpp : 定义控制台应用程序的入口点。 // #include <tchar.h> #include <opencv2/core/core.hpp> #include <opencv2/opencv.hpp> #include <opencv2/features2d.hpp> #include <highgui.hpp> #include "cv.h" #include <cv.hpp> #include <iostream> #include <stdio.h> #include <time.h> #include "global.h" #include "opencv2/core/utility.hpp" #include "opencv2/core/ocl.hpp" #include "opencv2/imgcodecs.hpp" #include "opencv2/calib3d.hpp" #include "opencv2/imgproc.hpp" #include "opencv2/flann.hpp" #include "opencv2/ml.hpp" using namespace std; using namespace cv; /*计算标定板上模块的实际物理坐标*/ void calRealPoint(vector<vector<Point3f>>& obj, int boardwidth, int boardheight, int imgNumber, int squaresize) { // Mat imgpoint(boardheight, boardwidth, CV_32FC3,Scalar(0,0,0)); vector<Point3f> imgpoint; for (int rowIndex = 0; rowIndex < boardheight; rowIndex++) { for (int colIndex = 0; colIndex < boardwidth; colIndex++) { // imgpoint.at<Vec3f>(rowIndex, colIndex) = Vec3f(rowIndex * squaresize, colIndex*squaresize, 0); imgpoint.push_back(Point3f(rowIndex * squaresize, colIndex * squaresize, 0)); } } for (int imgIndex = 0; imgIndex < imgNumber; imgIndex++) { obj.push_back(imgpoint); } } /*输出和保存数据*/ void outputCameraParam(void) { /*保存数据*/ cv::FileStorage fs;//初始化 fs.open("xml\\cameraMatrix_l.xml", FileStorage::WRITE); fs << "cameraMatrix_l" << cameraMatrix_l; fs.open("xml\\cameraDistoration_l.xml", FileStorage::WRITE);//初始化 fs << "distCoeff_l" << distCoeff_l; fs.open("xml\\rotatoVector_l.xml", FileStorage::WRITE);//初始化 fs << "rvecs_l" << rvecs_l; fs.open("xml\\translationVector_l.xml", FileStorage::WRITE);//初始化 fs << "tvecs_l" << tvecs_l; fs.open("xml\\cameraMatrix_r.xml", FileStorage::WRITE); fs << "cameraMatrix_l" << cameraMatrix_r; fs.open("xml\\cameraDistoration_r.xml", FileStorage::WRITE);//初始化 fs << "distCoeff_r" << distCoeff_r; fs.open("xml\\rotatoVector_r.xml", FileStorage::WRITE);//初始化 fs << "rvecs_r" << rvecs_r; fs.open("xml\\translationVector_r.xml", FileStorage::WRITE);//初始化 fs << "tvecs_r" << tvecs_r; fs.release(); /*输出数据*/ /*fx 0 cx 0 fy cy 0 0 0 */ cout << "fx_l :" << cameraMatrix_l.at<double>(0, 0) << endl << "fy_l :" << cameraMatrix_l.at<double>(1, 1) << endl; cout << "cx_l :" << cameraMatrix_l.at<double>(0, 2) << endl << "cy_l :" << cameraMatrix_l.at<double>(1, 2) << endl; cout << "k1_l :" << distCoeff_l.at<double>(0, 0) << endl; cout << "k2_l :" << distCoeff_l.at<double>(0, 1) << endl; cout << "p1_l :" << distCoeff_l.at<double>(0, 2) << endl; cout << "p2_l :" << distCoeff_l.at<double>(0, 3) << endl; cout << "p3_l :" << distCoeff_l.at<double>(0, 4) << endl; cout << "fx_r :" << cameraMatrix_r.at<double>(0, 0) << endl << "fy_r :" << cameraMatrix_r.at<double>(1, 1) << endl; cout << "cx_r :" << cameraMatrix_r.at<double>(0, 2) << endl << "cy_r :" << cameraMatrix_r.at<double>(1, 2) << endl; cout << "k1_r :" << distCoeff_r.at<double>(0, 0) << endl; cout << "k2_r :" << distCoeff_r.at<double>(0, 1) << endl; cout << "p1_r :" << distCoeff_r.at<double>(0, 2) << endl; cout << "p2_r :" << distCoeff_r.at<double>(0, 3) << endl; cout << "p3_r :" << distCoeff_r.at<double>(0, 4) << endl; } //获取图片 int getPicture(void){ int pic_num = 1; VideoCapture cap1; VideoCapture cap2; cap2.open(1); cap1.open(0); if (cap1.isOpened() && cap2.isOpened()) { double w = 640, h = 480; cap1.set(CAP_PROP_FRAME_WIDTH, w);//设置显示界面的宽高   cap1.set(CAP_PROP_FRAME_HEIGHT, h); cap2.set(CAP_PROP_FRAME_WIDTH, w); cap2.set(CAP_PROP_FRAME_HEIGHT, h); Mat frame1, frame2; namedWindow("Video1"); namedWindow("Video2"); while (1) { cap1 >> frame1; imshow("Video1", frame1); cap2 >> frame2; imshow("Video2", frame2); waitKey(30); char c = cvWaitKey(100); if (c == 's') { destroyWindow("Video1"); destroyWindow("Video2"); break; } if (c == ' ') { sprintf(name_l, "image\\leftPic%d.jpg", pic_num); sprintf(name_r, "image\\rightPic%d.jpg", pic_num); cout << "done" << endl; pic_num++; imwrite(name_l, frame1); imwrite(name_r, frame2); } if (c == 'n') { sprintf(name_l, "pic\\leftPic.jpg"); sprintf(name_r, "pic\\rightPic.jpg"); imwrite(name_l, frame1); imwrite(name_r, frame2); cout << "done" << endl; } } return pic_num; } } /*****单目标定*****/ void SingleCalibration_l() { Mat img_l; //定义图片 int goodFrameCount_l= 0; namedWindow("ImageL"); cout << "按Q退出 ..." << endl; while (goodFrameCount_l< frameNumber) //frameNumber表示图片数量 { char *filename_l = (char *)malloc(sizeof(char) * 100); sprintf(filename_l, "image\\leftPic%d.jpg", goodFrameCount_l + 1); rgbImage_l = imread(filename_l, CV_LOAD_IMAGE_COLOR); //图片转为3信道 cvtColor(rgbImage_l, grayImage_l, COLOR_BGR2GRAY); //转为灰度图后赋给grayImage imshow("Camera_l", grayImage_l); bool isFind_l = findChessboardCorners(rgbImage_l, boardSize, corner_l, CV_CALIB_CB_NORMALIZE_IMAGE); if (isFind_l == true) //所有角点都被找到 说明这幅图像是可行的 { /* Size(5,5) 搜索窗口的一半大小 Size(-1,-1) 死区的一半尺寸 TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 20, 0.1)迭代终止条件 */ cornerSubPix(grayImage_l, corner_l, Size(3, 3), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 20, 0.1)); //将角点精确到亚像素级 drawChessboardCorners(rgbImage_l, boardSize, corner_l, isFind_l); imshow("chessboard_l", rgbImage_l); corners_l.push_back(corner_l);//把所有角点信息保存起来 goodFrameCount_l++; cout << "The image is good" << endl; } else { cout << "The image is bad please try again" << endl; } if (waitKey(10) == 'q') { break; } } /* 图像采集完毕 接下来开始摄像头的校正 calibrateCamera() 输入参数 objectPoints 角点的实际物理坐标 imagePoints 角点的图像坐标 imageSize 图像的大小 输出参数 cameraMatrix 相机的内参矩阵 distCoeffs 相机的畸变参数 rvecs 旋转矢量(外参数) tvecs 平移矢量(外参数） */ /*设置实际初始参数 根据calibrateCamera来 如果flag = 0 也可以不进行设置*/ /*计算实际的校正点的三维坐标*/ calRealPoint(objRealPoint_l, boardWidth, boardHeight, frameNumber, squareSize); cout << "cal real successful" << endl; /*标定摄像头*/ calibrateCamera(objRealPoint_l, corners_l, Size(imageWidth, imageHeight), cameraMatrix_l, distCoeff_l, rvecs_l, tvecs_l, 0); cout << "calibration successful" << endl; /*显示畸变校正效果*/ Mat cImage_l; undistort(rgbImage_l, cImage_l, cameraMatrix_l, distCoeff_l); imshow("Corret Image_l", cImage_l); } void SingleCalibration_r() { Mat img_r; //定义图片 int goodFrameCount_r = 0; namedWindow("ImageR"); cout << "按Q退出 ..." << endl; while (goodFrameCount_r < frameNumber) //frameNumber表示图片数量 { char *filename_r = (char *)malloc(sizeof(char) * 100); sprintf(filename_r, "image\\rightPic%d.jpg", goodFrameCount_r + 1); rgbImage_r = imread(filename_r, CV_LOAD_IMAGE_COLOR); cvtColor(rgbImage_r, grayImage_r, COLOR_BGR2GRAY); imshow("Camera_r", grayImage_r); bool isFind_r = findChessboardCorners(rgbImage_r, boardSize, corner_r, 0); if (isFind_r == true) //所有角点都被找到 说明这幅图像是可行的 { cornerSubPix(grayImage_r, corner_r, Size(3, 3), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 20, 0.1)); drawChessboardCorners(rgbImage_r, boardSize, corner_r, isFind_r); imshow("chessboard_r", rgbImage_r); corners_r.push_back(corner_r); goodFrameCount_r++; cout << "The image is good" << endl; } else { cout << "The image is bad please try again" << endl; } i