【泡泡机器人原创专栏】PTAM跟踪过程中的旋转预测方法与跟踪失败后的重定位

泡泡机器人SLAM · 公众号 · 机器人 · 2017-02-17 07:52

正文

各位“泡芙”大家好，这次给大家带来的是PTAM中跟踪过程的旋转预测方法与跟踪失败后如何重定位的介绍。

PTAM是一个很有年代的 SLAM Tracker。这里只选择了一个小点，就是PTAM是如何在跟踪过程中应对摄像头旋转的？

此预测方法的效果视频：

视频中演示的就是基于PTAM旋转预测思路的跟踪效果。每一帧跟踪算法所用的时间小于0.3毫秒，可以说非常快。（PC平台 i5 CPU）

BTW 视频演示的全部代码已经上传到github。其中跟踪算法代码约80行左右。

PTAM在进行跟踪的每一帧都会首先调用一个CalcSBIRotation的方法，最开始读PTAM代码我总是很疑惑这个方法的作用。最后发现这个方法会计算一下当前帧相对于上一帧的Rotation。也就是旋转角度。计算完毕后再根据这个旋转角度进行调整，然后使用基于SSD的跟踪算法。因为预先计算了Rotation，当然SSD的结果也就会更加准确。当用户旋转摄像头的时候也就更加不容易丢失跟踪目标。

此方法的输入输出可以简单描述如下：

输入1 ：图像1。（基准图片，缩放后的灰度图，分辨率40x30）

输入2 ：图像2。（当前图片，缩放后的灰度图，分辨率40x30）

输出：图像2 相对于图像1 的变换矩阵（Pose Mat）

如上图所示，图像1也就是基准图片为左上角那片灰色图。不过真实大小是灰色区域面积的1/9（因为太小所以放大后显示的）。而图像2就是上图大图缩放后的灰度图。基于输出的Pose矩阵计算出白色的线框，它标识了基准图片的位置。

此方法采用的跟踪算法为 ESM 跟踪算法。跟踪算法可以分为很多类别，本方法是基于一张完整图的灰度信息进行计算。计算的理论依据可以查阅相关论文（论文见附录）。在此并不针对算法的理论进行分析，由于理论薄弱，算法推导部分我也没有完全理解，如果有对这个算法理解深入的还希望给予帮助和指导。

该算法的数学模型为

PTAM对此算法的实现基于CVD，TooN等库。我使用OpenCV重写此算法，重写后的算法关键步骤如下：

//基于传入的Pose 多次迭代
    for(int i = 0; i
        //用上次的结果Pose，对图片变换。
        cv::warpAffine(...);

        //准备数学模型需要的参数，记录变换后图片和基准图片的差异程度
        cv::Vec3f v3Accum   //JT We
        cv::Vec6f v6Triangle(0, 0, 0, 0, 0, 0); //JT J

        //利用参数，根据数学模型解方程.
        cv::Mat update; //∆p
        cv::solve(m3, v3Accum, update);

        //基于方程结果构建差异矩阵 3x3
        cv::Mat updateMat = cv::Mat::zeros(3, 3, CV_32F);

        //用差异矩阵更新 Pose矩阵进一步提高准确度最终Pose矩阵为2x3
        Pose = Pose * updateMat;

    }

//返回Pose

完整算法：

https://github.com/meiroo/ComputerCoil/blob/master/ThumbTracker/ThumbTracker.cpp

需要注意的是，重写后的算法相对于PTAM的算法有部分修改，也简化了一些。具体效果可以参考上面的视频，可以看到此方法有如下特点：