这一节应该是本项目（Feature Tracking and Synchronous Scene Generation with a Single Camera）的最后一节了，实现了两种选取跟踪点和恢复跟踪的方法，顺便把AR物体换成了AR小游戏。
首先讲讲跟踪点的选取。之前的文章中我们选取ORB作为特征点检测的办法，然后手动选取N个ORB角点去利用LK光流法跟踪。这样的方法是事先定义好3D点的位置，比如正方形的四个角点（-1，-1，0），（1，-1，0），（1，1，0），（-1，1，0），然后在2D图像上手动的按顺序选取这么4个规则的2D点，然后把对应好的3D/2D点送到solvePnP函数里处理计算相机姿态。这样的方法好处是一开始我们就能手动确定一个平面，这个平面不一定平行于我们的相机，比较真实。而且可以手动控制solvePnP里处理的对应点对组数。缺点是事先定义好的3D点集合太有限，不能应对大千世界的种种。
与寻找2D点去对应定义好的3D点不同的是，我们先检测2D点，然后找一个点作为坐标原点，计算所有点到它的距离，除以一个值（大小根据远近而定）得到3D点的x,y坐标，z值为0. 这样我们也定义了一个平面，这个平面一开始是平行于我们的相机的，不太真实，但却能用于识别跟踪大千世界的种种。SolvePnP里的点对数较多，要注意根据情况持续作调整才行。这种计算3D点去对应检测到的2D点的方法利于之后我们提到的第二种特征点跟踪恢复的方法。
在具体讲述实现之前，我们先来改进一下ORB算法。根据GoodFeaturesToTrack函数中最小距离的应用，让我们想到将ORB算法做一个小小的处理。任意两个ORB角点之间最小距离为10，距离在10以内的点都被删除。实现过程类似于循环删除整形数组中的数指导任意两个数之间的距离都大于某一个值：
最小距离: 3
初始： 1 2 3 4 2 8 5 6 4 1 7 8 9 10 12
步骤1： 1 4 8 5 6 4 7 8 9 10 12
步骤2： 1 4 8 7 8 9 10 12
步骤3： 1 4 8 12
代码如下：
void DlimiteOrb(vector<Point2f>& inigoodfeatures)
{
vector<Point2f> newgoodfeatures;
Point2f temp1,temp2;
for(size_t i = 0;i<inigoodfeatures.size();i++)
{
temp1 = inigoodfeatures.at(i);
for(size_t k = 0;k<i;k++)
newgoodfeatures.push_back(inigoodfeatures.at(k));
newgoodfeatures.push_back(temp1);
for(size_t j = i+1;j<inigoodfeatures.size();j++)
{
temp2 = inigoodfeatures.at(j);
if(abs(temp1.x-temp2.x)+abs(temp1.y-temp2.y)>10)
newgoodfeatures.push_back(temp2);
}
inigoodfeatures = newgoodfeatures;
newgoodfeatures.clear();
}
}
2D点对应3D点的特征点选取前面文章也说明过，现在稍微改进一下，我们定义了九个点，选取一个中心，剩下8点按照正方形边长不断扩大的方式来寻找，如下图所示，在不断扩大的绿色正方形上寻找剩下8个点，最终在边长为84*2的正方形上找到了并用红色标出。
找到这9点2D点去对应已经定义好的3D点集，然后利用这组2D/3D点集去计算更新相机姿态。对应这种chessboard来说，特征点分布很有规律，适合这样寻找跟踪点。相应的恢复跟踪呢，我想是这样的：
跟踪点丢失前：
左丢失，x+3，然后判断丢失点的y是不是最小的，是的话+3，是不是最大的，是的话-3. 不大不小则不加不减：
右丢失，x-3，然后判断丢失点的y是不是最小的，是的话+3，是不是最大的，是的话-3. 不大不小则不加不减：
上丢失，y+3，然后判断丢失点的x是不是最小的，是的话+3，是不是最大的，是的话-3. 不大不小则不加不减：
下丢失，y-3，然后判断丢失点的x是不是最小的，是的话+3，是不是最大的，是的话-3. 不大不小则不加不减：
左丢失恢复跟踪的例子：
上丢失恢复跟踪的例子：
以上为第一种恢复跟踪的方法与实验结果，适合棋盘或者其他特征点分布有规律的物体，具体实现代码点击下载：cmonoFTSSG （我认为使用机器学习让系统学习在哪找到新的补偿点比较好，但现在还没接触过机器学习，以后会学的。）
第二种3D点对应2D点的特征点选取方法，如下图所示：
我们先检测2D点，然后找一个点作为坐标原点，计算所有点到它的距离，除以一个值（大小根据远近而定）得到3D点的x,y坐标，z值为0。结果如下，除了棋盘之外的普通场景也能捕捉跟踪：
图中的AR物体从彩色立方体变成了3D推箱子小游戏，该游戏的源代码我是在网上找的，下载链接。我对程序做了修改，换了部分贴图（ice.cc Game Studio是我2年前学游戏编程时做的小图标，当时还想组建自己的游戏工作室呢:)），更改了一些opengl参数来把它画在map上。画map时不能启动光源，画推箱子时要启动光源。w a s d可以控制箱子，其他的快捷键代码理由说明。
接下来讨论恢复跟踪。首先我们来看下图像2d点和空间3d点的关系以及推导：
展开图像2d点和空间3d点的关系中的矩阵我们得到（1）（2）（3）式，把（3）带入（1）（2）并令Zw为0，得到等式组（4），整理成矩阵形式为：
观察矩阵我们容易发现：只要得到一组2D点（x，y）在知道相机内参fx,fy,u0,v0和相机姿态R,T的情况下，我们就可以利用克莱姆法则解得此线性方程组（系数行列式不为0）的唯一解（Xw，Yw）。我们在特征点少于9个时，启动恢复跟踪，利用丢失前那帧的相机姿态R,T和当前帧的ORB特征点再结合相机内参计算出新的对应3D点集，然后送到solvePnP中去计算新的相机姿态以继续跟踪特征点。这种方法在丢失次数不多的情况下稳定，丢失次数多了之后变得不稳定起来，因为每一次计算都会存在极小误差。
结果如下图所示：
棋盘：
任意场景：
如上面所说，丢失次数少的时候具有鲁棒性，多了误差大，不稳定。
具体实现代码点击下载：cmonoFTSSG2.0
整个项目到此为止，利用LK光流法跟踪选取的ORB特征点，利用计算好的对应2D/3D点集更新相机姿态，最后在opengl里指定位置画出AR物体，系统还有一定的恢复跟踪能力。系统运行速度快，一定条件下稳定，鲁棒。在这个项目的完成过程中，我学到了很多计算机视觉的知识，这些对我的下一个项目有很大的帮助。本项目采用一个摄像头，采集到的图像为2D图像。接下俩我将采用OpenNI从XtionProLive（RGB-D相机）中获得具有深度信息的3D图像，采用一些算法实现3D重建，最后会加入一些手势识别或语音识别，做一个新的人机交互设备（类似hololens）。欢迎大家一起讨论，一起改变世界！