关键词：相机位姿估计 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：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