• 热门专题

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

作者:V·Shawn  发布日期:2016-12-14 20:33:17
Tag标签:相机  三维  实时  
  • 关键词:相机位姿估计 OpenCV::solvePnP labview三维图片

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

    @Author:VShawn(singlex@foxmail.com)

    @Date:2016-12-12

    @Lab: CvLab202@CSU

    前言

    本文将展示一个实时相机位姿估计的例程,其中的原理在前文中已经说过了,再利用《相机位姿估计1_1:OpenCV、solvePnP二次封装与性能测试》中构建的类,使得程序处理更加简单。本例程利用HSV空间,跟踪红色的特征点,将跟踪到的特征点用于解PNP问题,得到相机位姿(相机的世界坐标与相机的三个旋转角)。最后使用labview中的三维图片控件,对整个系统进行3D重建。

    处理流程

    首先初始化工业相机,采集到实时图像,使用imshow显示图片。 在实时的相机采图中,依次选取P1、P2、P3、P4(在前文《相机位姿估计1:根据共面四点估计相机姿态》中有提及),一定要按顺序点,否则无法获得正确位姿。选取完成后立即对该点进行追踪。 当跟踪的特征点数量达到4个时,程序开始调用PNPSolver类估计相机位姿。 将得到的位姿信息写入txt,位于D盘根目录(这就是上一篇文章中为什么要写文件的原因)。 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++程序:https://github.com/vshawn/Shawn_pose_estimation_by_opencv/tree/master/%E7%9B%B8%E6%9C%BA%E4%BD%8D%E5%A7%BF%E4%BC%B0%E8%AE%A12%EF%BC%9A%5B%E5%BA%94%E7%94%A8%5D%E5%AE%9E%E6%97%B6%E4%BD%8D%E5%A7%BF%E4%BC%B0%E8%AE%A1%E4%B8%8E%E4%B8%89%E7%BB%B4%E9%87%8D%E5%BB%BA%E7%9B%B8%E6%9C%BA%E5%A7%BF%E6%80%81

    LabView程序:https://github.com/vshawn/Shawn_pose_estimation_by_opencv/tree/master/%E7%94%A8LabView%E9%87%8D%E5%BB%BA%E7%9B%B8%E6%9C%BA%E4%BD%8D%E7%BD%AE

About IT165 - 广告服务 - 隐私声明 - 版权申明 - 免责条款 - 网站地图 - 网友投稿 - 联系方式
本站内容来自于互联网,仅供用于网络技术学习,学习中请遵循相关法律法规