关键词:相机位姿估计 OpenCV::solvePnP labview 三维图片
文章类型:应用展示 + Demo 演示
@Author:VShawn(singlex@foxmail.com)
@Date:2016-12-12
@Lab: CvLab202@CSU
本文将展示一个实时相机位姿估计的例程,其中的原理在前文中已经说过了,再利用《》中构建的类,使得程序处理更加简单。本例程利用 HSV 空间,跟踪红色的特征点,将跟踪到的特征点用于解 PNP 问题,得到相机位姿(相机的世界坐标与相机的三个旋转角)。最后使用 labview 中的三维图片控件,对整个系统进行 3D 重建。
用流程图来表示就是:
过程非常简单,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;
- }
当用户点击了四个特征点后,程序开始运行位姿估计部分。位姿具体的过程不再叙述,请参考前面的博文:
《》
《》
位姿估计完成后,会输出两个 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 配置参考我的博客《》,Labview 则是直接安装好就行。
例程下载后,需要将图像采集部分修改为你的相机驱动,然后修改相机参数、畸变参数就能够使用了。
地址:
C++ 程序:
LabView 程序:
来源: http://www.cnblogs.com/singlex/p/pose_estimation_2.html