相機位姿估計2：[應用]實時位姿估計與三維重建相機姿態

關鍵詞：相機位姿估計 OpenCV::solvePnP labview三維圖片

文章類型：應用展示+Demo演示

@Author：VShawn([e-mail])

@Date:2016-12-12

@Lab: CvLab202@CSU

目錄

• 《相機位姿估計0：基本原理之如何解PNP問題》
• 《相機位姿估計1：根據四個特征點估計相機姿態》
• 《相機位姿估計1_1：OpenCV:solvePnP二次封裝與性能測試》
• 《相機位姿估計2：[應用]實時位姿估計與三維重建相機姿態》
• 《相機位姿估計3：根據兩幅圖像的位姿估計結果求某點的世界坐標》

前言

本文將展示一個實時相機位姿估計的例程，其中的原理在前文中已經說過了，再利用《相機位姿估計1_1：OpenCV、solvePnP二次封裝與性能測試》中構建的類，使得程序處理更加簡單。本例程利用HSV空間，跟蹤紅色的特征點，將跟蹤到的特征點用於解PNP問題，得到相機位姿（相機的世界坐標與相機的三個旋轉角）。最后使用labview中的三維圖片控件，對整個系統進行3D重建。

處理流程

1. 首先初始化工業相機，采集到實時圖像，使用imshow顯示圖片。
2. 在實時的相機采圖中，依次選取P1、P2、P3、P4（在前文《相機位姿估計1：根據共面四點估計相機姿態》中有提及），一定要按順序點，否則無法獲得正確位姿。選取完成后立即對該點進行追蹤。
3. 當跟蹤的特征點數量達到4個時，程序開始調用PNPSolver類估計相機位姿。
4. 將得到的位姿信息寫入txt，位於D盤根目錄（這就是上一篇文章中為什么要寫文件的原因）。
5. Labview程序運行后不斷讀取txt，將讀到的位姿數據應用到3D中，繪制出正確的三維場景。（這里兩個進程通過txt通訊效率很低，但我偷懶了，沒有再去編寫更好的程序）

用流程圖來表示就是：

過程非常簡單，C++程序用來計算位姿，labview程序用於顯示。

（對於不懂labview的讀者：也可以通過OpenGL來實現顯示部分）

特征點跟蹤方法

為了偷懶省事，這里的特征點跟蹤直接使用了最簡單的跟蹤顏色的方法。我做的標志圖是這樣的：

每個特征點都是紅色馬克筆塗出的紅點。

在實際操作中用戶首先在顯示界面中按照順序（程序中點的世界坐標輸入順序）點擊特征點，得到特征點的初始位置。根據初始位置，在其附近選取ROI，將BGR圖像轉為HSV圖像進行顏色分割，針對其H通道進行二值化，將紅色區域置為255，得到二值圖像。在二值圖像中查找連通域，並計算出連通域的重心G的位置，將G的坐標作為本次跟蹤結果返回，並作為下一次跟蹤的起點。

效果如下圖，圖中綠色的圈是以重心G為圓心繪制的。

函數如下：

//跟蹤特征點
//在輸入點附近查找紅色區域，求出重心，作為特征點新的位置
//輸入為，1當前圖像，2被跟蹤特征點上一輪的位置
//返回本次跟蹤結果
cv::Point2f tracking(cv::Mat image, const cv::Point2f lastcenter)
{
	//cv::GaussianBlur(image, image, cv::Size(11, 11), 0);
	/***********初始化ROI**********/
	const int r = 100;//檢測半徑
	const int r2 = r * 2;

	int startx = lastcenter.x - r;
	int starty = lastcenter.y - r;
	if (startx < 0)
		startx = 0;
	if (starty < 0)
		starty = 0;

	int width = r2;
	int height = r2;
	if (startx + width >= image.size().width)
		startx = image.size().width - 1 - width;
	if (starty + height >= image.size().height)
		starty = image.size().height - 1 - height;

	cv::Mat roi = image(cv::Rect(startx, starty, width, height));
	cv::Mat roiHSV;
	cv::cvtColor(roi, roiHSV, CV_BGR2HSV);//將BGR圖像轉為HSV圖像

	vector<cv::Mat> hsv;
	cv::split(roiHSV, hsv);//將hsv三個通道分離
	cv::Mat h = hsv[0];
	cv::Mat s = hsv[1];
	cv::Mat v = hsv[2];


	cv::Mat roiBinary = cv::Mat::zeros(roi.size(), CV_8U);//二值圖像，255的地方表示判斷為紅色

	/*************判斷顏色****************/
	const double ts = 0.5 * 255;//s閾值，小於該值不判斷
	const double tv = 0.1 * 255;//v閾值，小於該值不判斷
	const double th = 0 * 180 / 360;//h中心
	const double thadd = 30 * 180 / 360;//h范圍在th±thadd內的才被算作是紅色

	//通過特定閾值，對HSV圖像進行二值化
	for (int i = 0; i < roi.size().height; i++)
	{
		uchar *ptrh = h.ptr<uchar>(i);
		uchar *ptrs = s.ptr<uchar>(i);
		uchar *ptrv = v.ptr<uchar>(i);
		uchar *ptrbin = roiBinary.ptr<uchar>(i);

		for (int j = 0; j < roi.size().width; j++)
		{
			if (ptrs[j] < ts || ptrv[j] < tv)
				continue;
			if (th + thadd > 180)
				if (ptrh[j] < th - thadd && ptrh[j] > th + thadd - 180)
					continue;
			if (th - thadd < 0)
				if (ptrh[j] < th - thadd + 180 && ptrh[j] > th + thadd)
					continue;

			ptrbin[j] = 255;//找出紅色的像素點，在對應位置標記255
		}
	}

	/*****************對二值化圖像求出連通域 重心****************/
	std::vector<std::vector<cv::Point>> contours;
	cv::findContours(roiBinary.clone(), contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);

	//可能會有多個連通域，分別計算出他們的重心
	std::vector<cv::Point2f> gravityCenters;//重心點集
	for (int i = 0; i < contours.size(); i++)
	{
		if (contours[i].size() < 10)//連通域太小
			continue;

		int xsum = 0;
		int ysum = 0;
		for (int j = 0; j < contours[i].size(); j++)
		{
			xsum += contours[i][j].x;
			ysum += contours[i][j].y;
		}
		double gpx = xsum / contours[i].size();
		double gpy = ysum / contours[i].size();
		gravityCenters.push_back(cv::Point2f(gpx + startx, gpy + starty));
	}

	/*********************返回最優點******************/
	//找到重心跟上一輪位置最接近的那個
	cv::Point ret = lastcenter;
	double dist = 1000000000;
	double distX = 1000000000;
	double distY = 1000000000;
	for (int i = 0; i < gravityCenters.size(); i++)
	{
		if (distX > abs(lastcenter.x - gravityCenters[i].x) && distY > abs(lastcenter.y - gravityCenters[i].y))
		{
			double newdist = sqrt((lastcenter.x - gravityCenters[i].x)*(lastcenter.x - gravityCenters[i].x) + (lastcenter.y - gravityCenters[i].y)*(lastcenter.y - gravityCenters[i].y));
			if (dist > newdist)
			{
				distX = abs(lastcenter.x - gravityCenters[i].x);
				distY = abs(lastcenter.y - gravityCenters[i].y);
				dist = newdist;
				ret = gravityCenters[i];
			}
		}
	}
	return ret;
}

位姿估計

當用戶點擊了四個特征點后，程序開始運行位姿估計部分。位姿具體的過程不再敘述，請參考前面的博文：

《相機位姿估計1：根據四個特征點估計相機姿態》

《相機位姿估計1_1：OpenCV:solvePnP二次封裝與性能測試》

三維顯示

位姿估計完成后，會輸出兩個txt用於記錄相機當前的位姿。

Labview程序就是讀取這兩個txt的信息，進而顯示出三維空間。labview程序的編程過程比較難敘述，思路便是首先建立世界坐標系，然后在世界坐標系中創建一個三維物體作為相機的三維模型。然后根據txt中的信息，設置這個模型所在的位置（也就是三維坐標），再設置該模型的三個自旋角，完成三維繪制。

上述流程可以運行項目文件夾中的：

~\用LabView重建相機位置\世界-手動調整參數設置相機位姿.vi

來手動設置參數，體驗繪圖的流程。

 

對該部分感興趣的人可以參考文檔：

http://zone.ni.com/reference/zhs-XX/help/371361J-0118/lvhowto/create_3d_scene/

效果演示

這演示以前也有放出來過，就是實時跟蹤特征點，再在右邊重建相機姿態。

 

程序下載

最后給出例程，例程C++部分基於VS2013開發，使用的是OpenCV2.4.X，三維重建部分使用Labview2012開發。OpenCV配置參考我的博客《OpenCV2+入門系列（一）：OpenCV2.4.9的安裝與測試》，Labview則是直接安裝好就行。

例程下載后，需要將圖像采集部分修改為你的相機驅動，然后修改相機參數、畸變參數就能夠使用了。

地址：

C++程序：Github

LabView程序：Github