实时计算标靶位姿

 之前有段时间在做相机标定的实验，用单目相机来获得标靶的位姿，需要用到Opencv中solvePnP这个函数，现在还没有想到要用这个结果来做什么，仅仅是做了一个Demo。

实验程序中使用了线程，但自己对线程的理解和使用并不是很深刻，所以在程序中偶尔出现Bug，如反复点击打开摄像头、关闭摄像头，偶尔会出现内存泄漏，但具体原因和如何修改目前还不清楚。麻烦知道的请在下面留言，不甚感激。

下面为Demo的部分程序：

头文件添加的变量与函数；

全局变量与相关定义：

函数部分：

void Cdeme_threadDlg::ShowImage(Mat& src, UINT ID)
{if (src.empty())return;Mat dst = src.clone();if (dst.channels() == 1)cvtColor(dst, dst, CV_GRAY2BGR);CDC* pDC = GetDlgItem(ID)->GetDC();		// 获得显示控件的 DCHDC hDC = pDC->GetSafeHdc();			// 获取 HDC(设备句柄) 来进行绘图操作CRect rect;GetDlgItem(ID)->GetClientRect(&rect);	// 获取控件尺寸位置CvvImage cimg;IplImage cpy = dst;cimg.CopyOf(&cpy);						// 复制图片cimg.DrawToHDC(hDC, &rect);				// 将图片绘制到显示控件的指定区域内ReleaseDC(pDC);
}void Cdeme_threadDlg::OnBnClickedOpencam()
{// TODO: Add your control notification handler code hereif (i_caculate == true){AfxMessageBox(_T("请先停止RT的计算！"));return;}	cap.open(0);if (!cap.isOpened()){AfxMessageBox(_T("打开摄像头失败！！"));return;}if (flag){GetDlgItem(IDC_OPENCAM)->SetWindowText(_T("打开摄像头"));flag = false;cap.release();}else{flag = true;AfxBeginThread(imageshow, this);GetDlgItem(IDC_OPENCAM)->SetWindowText(_T("关闭摄像头"));}
}UINT Cdeme_threadDlg::imageshow(void *param)
{Cdeme_threadDlg* dlg = (Cdeme_threadDlg*)param;while (true){if (!dlg->flag){Mat background = imread("black.png", 1);dlg->ShowImage(background, IDC_SHOWPIC);break;}if (dlg->cap.isOpened()){dlg->cap >> frame;dlg->ShowImage(frame, IDC_SHOWPIC);}	}return 0;
}void Cdeme_threadDlg::OnBnClickedCaculate()
{// TODO: Add your control notification handler code hereUpdateData(TRUE);if (!cap.isOpened()){AfxMessageBox(_T("请打开摄像头！"));return;}if (i_caculate){i_caculate = false;GetDlgItem(IDC_CACULATE)->SetWindowText(_T("计算RT"));}else{i_caculate = true;AfxBeginThread(caculateRT, this);GetDlgItem(IDC_CACULATE)->SetWindowText(_T("停止计算"));}
}UINT Cdeme_threadDlg::caculateRT(void *param)
{Cdeme_threadDlg* dlg = (Cdeme_threadDlg*)param;CEdit* RTbox = (CEdit*)dlg->GetDlgItem(IDC_RTValue);Mat matrix, distcoeffs;FileStorage fs("assistCam_intrinsic.xml", FileStorage::READ);if (fs.isOpened()){matrix = Mat(3, 3, CV_64F);fs["camera_matrix"] >> matrix;distcoeffs = Mat(5, 1, CV_64F);fs["distortion_coefficients"] >> distcoeffs;}fs.release();Size boardSize(dlg->m_Pointwidth, dlg->m_PointHeight);float squaresize = dlg->m_PointDistance;while (dlg->i_caculate){Sleep(2000);dlg->cap >> frame;imwrite("1.png", frame);//进行反色处理Mat grayImage;cvtColor(frame, grayImage, CV_BGR2GRAY);//for (int i = 0; i != grayImage.rows;i++)//{//	for (int j = 0; j != grayImage.cols;j++)//	{//		grayImage.at(i, j) = 255 - grayImage.at(i, j);//	}//}imwrite("2.png", grayImage);vector objectPoints;for (int i = 0; i < boardSize.height; i++){for (int j = 0; j < boardSize.width; j++){objectPoints.push_back(Point3f(j*squaresize, i*squaresize, 0));}}SimpleBlobDetector::Params params;params.minArea = 3.141592653 * 10 * 10;params.maxArea = 3.141592653 * 110 * 110;params.filterByArea = true;Ptr blobDetector = new SimpleBlobDetector(params);//检测特征点vector imagePoints;bool ok;switch (dlg->m_boardtype){case CHESSBOARD:ok = findChessboardCorners(grayImage, boardSize, imagePoints,CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);break;case CIRCLE:ok = findCirclesGrid(grayImage, boardSize, imagePoints, CALIB_CB_SYMMETRIC_GRID, blobDetector);break;}if (ok){cout << "success to find the circles\n" << endl;drawChessboardCorners(grayImage, boardSize, imagePoints, ok);imwrite("3.png", grayImage);Mat r, R, T;solvePnP(objectPoints, imagePoints, matrix, distcoeffs, r, T);       // 从3D-2D点找出一个物体姿态，求出旋转矩阵和平移矩阵Rodrigues(r, R);                      //将旋转矩阵转换为旋转矢量或反之FileStorage res("RT_result.xml", FileStorage::WRITE);res << "R" << R;res << "T" << T;res.release();float str;CString RT, temp, RT_old;RT += "R:";for (int i = 0; i < R.rows; i++){for (int j = 0; j < R.cols; j++){str = R.at(i, j);temp.Format(_T("%f"), str);temp += ",";RT += temp;}}RT += "T:";for (int i = 0; i < T.rows; i++){for (int j = 0; j < T.cols; j++){str = T.at(i, j);temp.Format(_T("%f"), str);temp += ",";RT += temp;}}RTbox->GetWindowText(RT_old);if (RT_old==""){RTbox->SetWindowText(RT);}else{		RT = RT_old + "\r\n" + RT;                     //需要设置Edit控件为多行RTbox->SetWindowText(RT);}}else{cout << "fail to find the circles\n" << endl;AfxMessageBox(_T("角点提取失败,请将标靶放在图像显示区域！"));}}return 0;
}

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