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

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

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

@Author:VShawn(singlex@foxmail.com)

@Date:2016-12-12

@Lab: CvLab202@CSU

目录

前言

本文将展示一个实时相机位姿估计的例程,其中的原理在前文中已经说过了,再利用《相机位姿估计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

posted @ 2016-12-13 00:42  V·Shawn  阅读(14212)  评论(1编辑  收藏  举报