双目测距，鼠标点击输出三维坐标

#include <opencv2/opencv.hpp> #include <iostream> #include <stdio.h> #include <cv.h> #include <highgui.h> #include <stdio.h> #include"TCHAR.H" #include"5555.h" #include <opencv2/legacy/legacy.hpp> using namespace std; using namespace cv; Mat disp1,_Q; CvStereoBMState *m_BMstate = cvCreateStereoBMState(); void stereo_match(int) { //m_BMstate->SADWindowSize =2*blockSize+5; m_BMstate->SADWindowSize =21; m_BMstate->minDisparity = 0; //m_BMstate->uniquenessRatio = uniquenessRatio; m_BMstate->uniquenessRatio = 17; m_BMstate->preFilterCap = 31; //m_BMstate->numberOfDisparities = numDisparities * 16 + 16; m_BMstate->numberOfDisparities = 272; m_BMstate->textureThreshold = 10; m_BMstate->speckleWindowSize = 100; m_BMstate->speckleRange = 32; m_BMstate->disp12MaxDiff = -1; } static void onMouse(int event, int x, int y, int , void* ) { if (event == CV_EVENT_LBUTTONDOWN) { pic_info[0] = x; pic_info[1] = y; cout << "x:" << pic_info[0] << "y:" << pic_info[1] << endl; left_mouse = true; CvScalar scal = cvGet2D(xyz1,y, x); cout << "点（" << x << "," << y << ")的三维坐标为：" << endl << scal.val[0]*1.6 << " " << scal.val[1]*1.6 << " " << scal.val[2]*1.6<< endl; cout << "in world coordinate: " << xyz.at<Vec3f>(pic_info[1], pic_info[0])<< endl;//获取三维坐标方式2 cout << "x: " << xyz.at<Vec3f>(pic_info[1], pic_info[0])[0] << endl; cout << "y: " << xyz.at<Vec3f>(pic_info[1], pic_info[0])[1] << endl; cout << "z: " << xyz.at<Vec3f>(pic_info[1], pic_info[0])[2] << endl; } else if (event == CV_EVENT_LBUTTONUP) { left_mouse = false; } else if ((event == CV_EVENT_MOUSEMOVE) && (left_mouse == true)) { } } //int getPointClouds(cv::Mat& disparity, cv::Mat& pointClouds) //{ // if (disparity.empty()) // { // return 0; // } // // //计算生成三维点云 // reprojectImageTo3D(disparity, pointClouds, Q, true); // // pointClouds *= 1.6; // for (int y = 0; y < pointClouds.rows; ++y) // { // for (int x = 0; x < pointClouds.cols; ++x) // { // cv::Point3f point = pointClouds.at<cv::Point3f>(y, x); // point.y = -point.y; // pointClouds.at<cv::Point3f>(y, x) = point; // } // } // pointClouds.copyTo(xyz); //获取三维坐标方式2 // // split(pointClouds, xyzSet);//获取三维坐标方式1 // xyzSet[0].copyTo(depth0); // xyzSet[1].copyTo(depth1); // xyzSet[2].copyTo(depth2); // return 1; //} int main(int argc, char* argv[]) { //cout << "测试两个摄像头同时读取数据" << endl; //读取内部参数 CvMat *Intrinsics_Camera_Left = (CvMat *)cvLoad("Intrinsics_Camera_Left.xml"); CvMat *Intrinsics_Camera_Right = (CvMat *)cvLoad("Intrinsics_Camera_Right.xml"); CvMat *Distortion_Camera_Left = (CvMat *)cvLoad("Distortion_Camera_Left.xml"); CvMat *Distortion_Camera_Right = (CvMat *)cvLoad("Distortion_Camera_Right.xml"); CvMat *Translation_matlab = (CvMat *)cvLoad("Translation.xml"); CvMat *RotRodrigues_matlab = (CvMat *)cvLoad("RotRodrigues.xml"); CvMat *R_opencv = cvCreateMat(3, 3, CV_64F); cvRodrigues2(RotRodrigues_matlab, R_opencv); CvCapture* cap_left; CvCapture* cap_right; IplImage *img0,*img1; cap_left = cvCreateCameraCapture(0); assert(cap_left != NULL); cap_right = cvCreateCameraCapture(1); assert(cap_right != NULL); img0 = cvRetrieveFrame(cap_left); //img1 = cvRetrieveFrame(cap_right); IplImage *img_left_Change=cvCreateImage(cvGetSize(img0),img0->depth,1); IplImage *img_right_Change=cvCreateImage(cvGetSize(img0),img0->depth,1); xyz1=cvCreateMat(img0->height,img0->width, CV_32FC3); CvSize imSize=cvSize(img0->width,img0->height ); //CvSize imSize=cvSize(640 ,480); CvMat* mxl = cvCreateMat(imSize.height, imSize.width, CV_32F); CvMat* myl = cvCreateMat(imSize.height, imSize.width, CV_32F); CvMat* mxr = cvCreateMat(imSize.height, imSize.width, CV_32F); CvMat* myr = cvCreateMat(imSize.height, imSize.width, CV_32F); CvMat* img1r = cvCreateMat(imSize.height, imSize.width, CV_8U); CvMat* img2r = cvCreateMat(imSize.height, imSize.width, CV_8U); CvMat* disp = cvCreateMat(imSize.height, imSize.width, CV_16S);//视察映射 CvMat* vdisp = cvCreateMat(imSize.height, imSize.width, CV_8U);//归一化后的视差映射 CvMat *Rl = cvCreateMat(3, 3, CV_64F); CvMat *Rr = cvCreateMat(3, 3, CV_64F); CvMat *Pl = cvCreateMat(3, 4, CV_64F); CvMat *Pr = cvCreateMat(3, 4, CV_64F); CvMat* pair;//定义一个矩阵用于存放校正后的两张图片 CvMat part;//定义一个矩阵用于存放校正后的一张图片 pair = cvCreateMat(imSize.height, imSize.width * 2, CV_8UC3); cvStereoRectify(Intrinsics_Camera_Left, Intrinsics_Camera_Right, Distortion_Camera_Left, Distortion_Camera_Right, imSize, R_opencv, Translation_matlab, Rl, Rr, Pl, Pr,Q); cvStereoRectify(Intrinsics_Camera_Left, Intrinsics_Camera_Right, Distortion_Camera_Left, Distortion_Camera_Right, imSize, R_opencv, Translation_matlab, Rl, Rr, Pl, Pr, Q, 1024, 0);//增加图像缩放，去除死区 cvInitUndistortRectifyMap(Intrinsics_Camera_Left, Distortion_Camera_Left, Rl, Pl, mxl, myl); cvInitUndistortRectifyMap(Intrinsics_Camera_Right, Distortion_Camera_Right, Rr, Pr,mxr, myr); cvNamedWindow("disparity", CV_WINDOW_AUTOSIZE); // 创建SAD窗口 Trackbar cvCreateTrackbar("BlockSize:\n", "disparity",&blockSize, 8, stereo_match); // 创建视差唯一性百分比窗口 Trackbar cvCreateTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, 50, stereo_match); // 创建视差窗口 Trackbar cvCreateTrackbar("NumDisparities:\n", "disparity", &numDisparities, 50, stereo_match); //鼠标响应函数setMouseCallback(窗口名称, 鼠标回调函数, 传给回调函数的参数，一般取0) cvSetMouseCallback("disparity", onMouse, 0); while(1) { int64 t = getTickCount(); cvGrabFrame(cap_left); cvGrabFrame(cap_right); img0 = cvRetrieveFrame(cap_left); img1 = cvRetrieveFrame(cap_right); if( !img0 || !img1) { cout << "camera0 error" << endl; break; } cvCvtColor(img0,img_left_Change,CV_BGR2GRAY); cvCvtColor(img1,img_right_Change,CV_BGR2GRAY); cvRemap(img_left_Change, img1r, mxl, myl); cvRemap(img_right_Change, img2r, mxr, myr); //stereo_match(0); cvFindStereoCorrespondenceBM(img1r, img2r, disp, m_BMstate);//计算视差映射 cvNormalize(disp, vdisp, 0, 256, CV_MINMAX);//归一化视差映射 cvReprojectImageTo3D(disp,xyz1,Q,1);//c实现输出坐标 disp1=Mat(disp,true);//C++实现输出坐标 _Q=Mat(Q,true); reprojectImageTo3D(disp1,xyz,_Q,true); xyz*=1.6; //pair的左侧复制左相机立体校正后的照片 cvGetCols(pair, &part, 0, imSize.width);/*CvMat* cvGetCols( const CvArr* arr, CvMat* submat, int start_col, int end_col ); 返回数组的一定跨度内的列 arr-输入数组 submat-指向结果子数组头指针. start_col-跨度的开始列（包括该列）索引下标，该下标以0为基准。 end_col-跨度的结束列（不包括该列）索引下标，该下标以0为基准。*/ cvCvtColor(img1r, &part, CV_GRAY2BGR);//GRAY->RGB //pair右侧复制右相机立体校正后的照片 cvGetCols(pair, &part, imSize.width, imSize.width * 2); cvCvtColor(img2r, &part, CV_GRAY2BGR);//GRAY->RGB //在一定距离间隔处绘制水平直线 for (int j = 0; j < imSize.height; j += 16) cvLine(pair, cvPoint(0, j), cvPoint(imSize.width * 2, j), CV_RGB(0, 255, 0)); /