实时计算标靶位姿

 之前有段时间在做相机标定的实验，用单目相机来获得标靶的位姿，需要用到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();		// 获得显示控件的 DC
	HDC 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 here
	if (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 here
	UpdateData(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<unsigned char>(i, j) = 255 - grayImage.at<unsigned char>(i, j);
		//	}
		//}
		imwrite("2.png", grayImage);
		vector<Point3f> 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<FeatureDetector> blobDetector = new SimpleBlobDetector(params);//检测特征点
		vector<Point2f> 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<double>(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<double>(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;
}