相机位姿估计2：[应用]实时位姿估计与三维重建相机姿态

关键词：相机位姿估计 OpenCV::solvePnP labview三维图片

文章类型：应用展示+Demo演示

@Author：VShawn(singlex@foxmail.com)

@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