ORB_SLAM 初接触

MacOSX 移植版本： GitHub - meiroo/ORBSLAM2-OSX

iOS 移植版本： meiroo/ORBSLAM2-iOS · GitHub

====================================

以前读过部分PTAM代码。 不过对于ORB_SLAM还没有接触过。最近打算有时间看一看。正好赶上ORB_SLAM2，所以直接从ORB_SLAM2入手。

看代码以前，首先还是先跑起来看看效果再说。ORB_SLAM2代码结构非常好。依赖也比较少。主体依赖仅有DBoW2／g2o／eigen3。另外Map的绘制部分还依赖一个Pangolin。这几个库相对来说都比较好移植。

但是不知道为什么，使用Pangolin后在Mac上跑起来总是出现崩溃的问题。所以就没有使用Pangolin来绘制Map，而是自己随便绘制了一下Map的点，不过发现绘制出来稍微和现实有些差距（如下图线的地方可以看到特征点位置不准确的有些偏上，应该是画的时候哪里错误了 待调查）。

在Mac上总体上速度还是很快的（没有对代码进行任何优化，参数也没调整）。

接下来直接在iOS上测试了一下。iOS出现了严重的数据加载非常慢的问题。iPhone 5C 大约需要3，4分钟才能把词典文件载入。这个肯定是不行的。主要原因一个是词典太大未压缩的txt文件145M，另一点在于词典文件的读入方式效率太低。修改为二进制的方式读入（参考自一个fork）后，可以大大加快词典文件载入效率，在iPhone 5C上从3，4分钟提高到8秒左右可以加载完毕。

iPhone 5C的加载视频： Personal Projects by Meiroo 第三排的视频。

目前几个体验不太好的地方：

• 加载时间虽然提高到8秒左右，但还是稍长。
  
• 运行效率太低。计划先调整一下参数，初步调整到能较好的实时运行。
• 特征点不足时候，初始化会失败。这里体验不是很好。

这几个如果有思路修改，会发新文章。

 附录：1 

DBoW2 回环检测/重定位 算法解析

ORB-SLAM 和 Kintinuous 都使用 DBoW2 作为回环检测算法，这里对 DBoW2 算法做简要介绍。

DBoW2 论文中使用的特征检测算法是 Fast keypoint，见论文 “Machine learning for high speed corner detection” 
使用的特征描述子是 Brief 描述子，见论文 “Brief: Binary robust independent elementary features”

Brief 描述子和 ElasticFusion 中使用的 Randomized Ferns 描述子不同的是，Brief 描述子在  p  点周围选取两个点做比较，将比较的结果作为描述子的编码， 
如下： 

其中  xi  和  yi  是随机选取的服从 normal distribution 的两像素坐标， 
Randomized Ferns 回环检测算法介绍： 
http://blog.csdn.net/fuxingyin/article/details/51436430

ORB-SLAM 中用的是 ORB 特征描述子用 DBoW2 做回环。 
Kintinuous 中用的是 Surf 描述子用 DBoW2 做回环。

Bag of Words 字典建立

1、从训练图像中离线抽取特征 
2、将抽取的特征用 k-means++ 算法聚类，将描述子空间划分成 k 类 
3、将划分的每个子空间，继续利用 k-means++ 算法做聚类 
4、按照上述循环，将描述子建立树形结构，如下图所示：

字典树在建立过程中，每个叶子也就是每个 word 记录了该 word 在所有的训练图像中出现的频率，出现的频率越高，表示这个 word 的区分度越小，频率的计算公式如下：

idf(i)=logNni

在线更新字典树

当在字典树中需要插入一幅新图像  It ，在图像中提取的特征描述子按照 Hamming 距离从字典树的根部节点开始逐级向下到达叶子节点，可以计算每个叶子节点也就是每个 word 在图像  It  中的出现频率：

tf(i,It)=niItnIt  
其中  niIt  表示 word 在图像中出现的次数 
nIt  表示图像中描述子的总数

在树构建的过程中每个叶子节点存储了 inverse index，存储了到达叶子节点的图像  It  的 ID 和 word 在图像  It  描述 vector 中第  i  维的值： 
vit=tf(i,It)×idf(i)

对于一幅图像所有的描述子，做上述操作，可以得到每个 word 的值，将这些值构成图像的描述向量  vt 。

对两幅图像比较计算其相似度时，两幅图像相似度计算公式如下： 
s(v1, v2)=1−12|v1|v1|−v2|v2||  
两幅图像越相似得分越高。

字典树除了存储了 inverse index，还存储了 direct index 如上图所示，direct index 方便两幅图像特征搜索，建立特征之间的对应，计算两帧之间的位姿转换。

Database query

由于在计算相似度时，相似度的大小和字典树、图像等有一定关系，这里采用归一化的方式，消除这两种因素的影响： 
归一化相似度计算公式如下：

η(vt,vtj)=s(vt,vtj)s(vt,vt−Δt)

其中  vt−Δt  表示上一帧图像，上式含义是上一帧图像和当前帧图像是最为相似度的，用和上一帧图像计算的相似度来归一化和字典树中图像计算的相似度。

当  s(vt,vt−Δt)  较小时（机器人做旋转时），会把总体的得分拉的很高，论文里面剔除这种因素，选取阈值  α ，当前帧和上一帧图像相似度小于  α  时不做回环检测。

Matching group

假设图像  vt  和图像  vni  相似度很大，那么和图像  vni  周围的图像也会有很高的相似度，这里将相邻的得分都很高的图像 group 在一起构成 island，得分是 group 中图像得分的总和。

Temporal consistency

假设图像  vt  和 island  Vt1  之间相似度很大，那么图像  vt−kΔt  和  Vtk  像素度也应该很大（ k  小于 一定值），相当于两串图像间会有 overlap，利用这个条件作为 consistency 的约束。

Efficient geometrical consistency

1、对于一幅新图像  Ii ，用字典树建立对图像的描述，并且计算和字典树中以前存储的图像之间的得分。

inverse index 加快待比较的图像搜索速度 
由于 inverse index 存储了哪些图像也到达该叶子节点，在选择待比较的图像时，只需要比较到达相同叶子节点的图像，不需要和存储的每幅进行比较，从而加快比较速度。

direct index 加快特征比较速度 
假设图像  Ii  和  Ij  得分最高，在两幅图像特征匹配时，只需要比较 direct index 中属于同一个 node 的图像特征，node 指字典树的一层，如果是叶子节点层，那么选择是同一个 word 的特征做匹配。

参考文献： 
“Real-Time Loop Detection with Bags of Binary Words” 
“Bags of Binary Words for Fast Place Recognition in Image Sequences” 
“Brief: Binary robust independent elementary features” 
“Machine learning for high speed corner detection”

2

g2o(General Graph Optimization)—通用图优化算法

g2o的本质：g2o是一个算法集的C++实现，而并不是在算法理论上的创新，即根据前人求解非线性最小二乘的理论，根据具体的问题，选用最合适的算法。

它是一个平台，你可以加入你自己的线性方程求解器，编写自己的优化目标函数，确定更新的方式。g2o的作者说Guassian-Newton和Levenberg-Marquardt方法比较naive，但是g2o的本质就是这些算法的实现。事实上，g2o iSAM SPA和 sSPA等非线性优化算法只是在非线性问题线性化时处理得不一样，在线性化后要求解线性方程都是利用了已有的linear solver库来求解，如 CSparse CHOLMOD PCG等，他们都需要依靠Eigen这个线性代数库。

g2o的用途：很多机器人的应用如SLAM（同步定位与制图）还有计算机视觉中的光束优化（bundle adjustment 参考http://blog.csdn.net/abcjennifer/article/details/7588865；

http://courses.cs.washington.edu/courses/cse576/05sp/projects/proj3/artifacts/yongjoon/index.html）都会涉及到最小化非线性误差函数的问题。这类应用中，非线性误差函数可以用图(graph)的形式来表征。整个问题的求解就是要找到最符合观测量的相机参数或机器人状态。

g2o的实现：

类似算法：iSAM (参考http://openslam.org/iSAM.html）

                    SPA2d (参考http://users.ics.forth.gr/~lourakis/sba/）

                    HOG_Man

g2o与其他算法的效果对比：（具体参考论文：《g2o:a general graph optimization》)

Fig1：

Fig2：

Fig3：isam与其他算法的对比（参考：http://people.csail.mit.edu/kaess/isam/comparison.html）

3  

C++矩阵处理工具——Eigen

最近和一些朋友讨论到了C++中数学工具的问题，以前总是很2地自己写矩阵运算，或者有时候在matlab里计算了一些数据再往C程序里倒，唉~想想那些年，我们白写的代码啊……人家早已封装好了！首先推荐几个可以在C++中调用的数学平台：eigen、bias、lapack、svd、CMatrix，本文着重eigen做以讲解，希望对各位有所帮助。

下面是本文主线，主要围绕下面几点进行讲解：

**********************************************************************************************

Eigen是什么？

Eigen3哪里下载？

Eigen3的配置

Eigen3 样例代码有没有？

去哪里更深入学习？

**********************************************************************************************

Eigen是什么？

Eigen是C++中可以用来调用并进行矩阵计算的一个库，里面封装了一些类，需要的头文件和功能如下：

Eigen的主页上有一些更详细的Eigen介绍。

Eigen3哪里下载？

这里是我下好的，这里是官网主页，请自行下载，是个code包，不用安装。

Eigen的配置

直接上图了，附加包含目录那里填上你放Eigen文件夹的位置即可。

Eigen的样例代码有没有？

当然有，这篇文章重点就是这里！

以下是我整理的一些常用操作，基本的矩阵运算就在下面了，算是个入门吧~主要分以下几部分：

建议大家放到编译环境里去看，因为我这里有一些region的东西，编译器下更方便看~

[cpp]  view plain  copy

1. #include <iostream>  
2. #include <Eigen/Dense>  
3.   
4. //using Eigen::MatrixXd;  
5. using namespace Eigen;  
6. using namespace Eigen::internal;  
7. using namespace Eigen::Architecture;  
8.   
9. using namespace std;  
10.   
11.   
12. int main()  
13. {  
14.   
15. #pragma region one_d_object  
16.   
17.     cout<<"*******************1D-object****************"<<endl;  
18.   
19.     Vector4d v1;  
20.     v1<< 1,2,3,4;  
21.     cout<<"v1=\n"<<v1<<endl;  
22.   
23.     VectorXd v2(3);  
24.     v2<<1,2,3;  
25.     cout<<"v2=\n"<<v2<<endl;  
26.   
27.     Array4i v3;  
28.     v3<<1,2,3,4;  
29.     cout<<"v3=\n"<<v3<<endl;  
30.   
31.     ArrayXf v4(3);  
32.     v4<<1,2,3;  
33.     cout<<"v4=\n"<<v4<<endl;  
34.   
35. #pragma endregion  
36.   
37. #pragma region two_d_object  
38.       
39.     cout<<"*******************2D-object****************"<<endl;  
40.     //2D objects:  
41.     MatrixXd m(2,2);  
42.   
43.     //method 1  
44.     m(0,0) = 3;  
45.     m(1,0) = 2.5;  
46.     m(0,1) = -1;  
47.     m(1,1) = m(1,0) + m(0,1);  
48.   
49.     //method 2  
50.     m<<3,-1,  
51.         2.5,-1.5;  
52.     cout <<"m=\n"<< m << endl;  
53.   
54. #pragma endregion  
55.   
56. #pragma region Comma_initializer  
57.   
58.     cout<<"*******************Initialization****************"<<endl;  
59.   
60.     int rows=5;  
61.     int cols=5;  
62.     MatrixXf m1(rows,cols);  
63.     m1<<( Matrix3f()<<1,2,3,4,5,6,7,8,9 ).finished(),  
64.         MatrixXf::Zero(3,cols-3),  
65.         MatrixXf::Zero(rows-3,3),  
66.         MatrixXf::Identity(rows-3,cols-3);  
67.     cout<<"m1=\n"<<m1<<endl;  
68.   
69. #pragma endregion  
70.   
71. #pragma region Runtime_info  
72.       
73.     cout<<"*******************Runtime Info****************"<<endl;  
74.   
75.     MatrixXf m2(5,4);  
76.     m2<<MatrixXf::Identity(5,4);  
77.     cout<<"m2=\n"<<m2<<endl;  
78.   
79.     MatrixXf m3;  
80.     m3=m1*m2;  
81.     cout<<"m3.rows()="<<m3.rows()<<"  ;  "  
82.              <<"m3.cols()="<< m3.cols()<<endl;  
83.       
84.     cout<<"m3=\n"<<m3<<endl;  
85.   
86. #pragma endregion  
87.       
88. #pragma region Resizing  
89.       
90.     cout<<"*******************Resizing****************"<<endl;  
91.   
92.     //1D-resize   
93.     v1.resize(4);  
94.     cout<<"Recover v1 to 4*1 array : v1=\n"<<v1<<endl;  
95.   
96.     //2D-resize  
97.     m.resize(2,3);  
98.     m.resize(Eigen::NoChange, 3);  
99.     m.resizeLike(m2);  
100.     m.resize(2,2);  
101.       
102. #pragma endregion  
103.   
104. #pragma region Coeff_access  
105.       
106.     cout<<"*******************Coefficient access****************"<<endl;  
107.   
108.     float tx=v1(1);  
109.     tx=m1(1,1);  
110.     cout<<endl;  
111.   
112. #pragma endregion  
113.   
114. #pragma  region Predefined_matrix  
115.   
116.     cout<<"*******************Predefined Matrix****************"<<endl;  
117.   
118.     //1D-object  
119.     typedef  Matrix3f   FixedXD;  
120.     FixedXD x;  
121.       
122.     x=FixedXD::Zero();  
123.     x=FixedXD::Ones();  
124.     x=FixedXD::Constant(tx);//tx is the value  
125.     x=FixedXD::Random();  
126.     cout<<"x=\n"<<x<<endl;  
127.   
128.     typedef ArrayXf Dynamic1D;  
129.     //或者 typedef VectorXf Dynamic1D  
130.     int size=3;  
131.     Dynamic1D xx;  
132.     xx=Dynamic1D::Zero(size);  
133.     xx=Dynamic1D::Ones(size);  
134.     xx=Dynamic1D::Constant(size,tx);  
135.     xx=Dynamic1D::Random(size);  
136.     cout<<"xx=\n"<<x<<endl;  
137.   
138.     //2D-object  
139.     typedef MatrixXf Dynamic2D;  
140.     Dynamic2D y;  
141.     y=Dynamic2D::Zero(rows,cols);  
142.     y=Dynamic2D::Ones(rows,cols);  
143.     y=Dynamic2D::Constant(rows,cols,tx);//tx is the value  
144.     y=Dynamic2D::Random(rows,cols);  
145.   
146. #pragma endregion  
147.   
148. #pragma region Arithmetic_Operators  
149.   
150.     cout<<"******************* Arithmetic_Operators****************"<<endl;  
151.   
152.     //add & sub  
153.     MatrixXf m4(5,4);  
154.     MatrixXf m5;  
155.     m4=m2+m3;  
156.     m3-=m2;  
157.   
158.     //product  
159.     m3=m1*m2;  
160.    
161.     //transposition  
162.     m5=m4.transpose();  
163.     //m5=m.adjoint();//伴随矩阵   
164.       
165.     //dot product  
166.     double xtt;  
167.     cout<<"v1=\n"<<v1<<endl;  
168.     v2.resize(4);  
169.     v2<<VectorXd::Ones(4);  
170.     cout<<"v2=\n"<<v2<<endl;  
171.   
172.     cout<<"*************dot product*************"<<endl;  
173.     xtt=v1.dot(v2);  
174.     cout<<"v1.*v2="<<xtt<<endl;  
175.   
176.     //vector norm  
177.   
178.     cout<<"*************matrix norm*************"<<endl;  
179.     xtt=v1.norm();  
180.     cout<<"norm of v1="<<xtt<<endl;  
181.     xtt=v1.squaredNorm();  
182.     cout<<"SquareNorm of v1="<<xtt<<endl;  
183.   
184. #pragma endregion  
185.   
186. cout<<endl;  
187. }  

去哪里更深入学习？

Please refer to Eigen中的类及函数、Eigen的官方教程，和一些教程上的相关内容。