- 1.C++相关
- (1)继承
- (2)Eigen库数据结构内存对齐
- (3)C++模板类
- (4)虚函数
- (5)static_cast关键字
- (6)Eigen中的数据类型
- (7)SIZE_T
- (8)循环
- 2.g2o库简介与编译安装
- 3.图优化曲线拟合
- 4.图优化曲线拟合实例
- 5.复杂一点的例子
- 6.g2o库结构
- 7.双视图Bundle Adjustment(光束法平差)
- 代码实现
- 8.参考资料
1.C++相关
(1)继承
单继承的定义格式如下
常使用如下三种关键字给予表示：
- public 表示公有基类；
- private 表示私有基类；
- protected 表示保护基类；
(2)Eigen库数据结构内存对齐
在自定义的类中，如果成员中有Eigen的数据结构，可能会出现内存不对齐的问题。
在使用这个类的时候用到了new方法，这个方法是开辟一个内存，但是在上面的代码中没有自定义构造函数，所以在new的时候会调用默认的构造函数，调用默认的构造函数的错误在于内存的位数不对齐，所以会导致程序运行的时候出错。解决的方法就是在类中加入宏。
(3)C++模板类
之前介绍了C++中的模板函数，但其实模板类也是类似的。和之前相同，模板参数列表里可以有不止一个参数。例如下面就是一个例子：
(4)虚函数
在基类用声明成员函数为虚函数。这样就可以在派生类中重新定义此函数，为它赋予新的功能，并能方便被调用。
在派生类中重新定义此函数，要求函数名，函数类型，函数参数个数和类型全部与基类的虚函数相同，并根据派生类的需要重新定义函数体。c++规定，当一个成员函数被声明为虚函数后，其派生类的同名函数都自动成为虚函数。因此在派生类重新声明该虚函数时，可以加，也可以不加，但习惯上一般在每层声明该函数时都加上，使程序更加清晰。如果再派生类中没有对基类的虚函数重新定义，则派生类简单的继承起基类的虚函数。
基类中若虚函数=0,则在派生类中必须要重新定义该虚函数，例如下面这几个函数都是基类中的虚函数，且需要在派生类中重新定义。
(5)static_cast关键字
用法
叫做编译时类型检查。该运算符把转换为类型，这样做的目的是在程序还没有运行时进行类型检查，来保证转换的安全性。
而与之对应的，关键字（运行时类型检查）主要用于类层次结构中父类和子类之间指针和引用的转换，由于具有运行时类型检查，因此可以保证下行转换的安全性。即转换成功就返回转换后的正确类型指针，如果转换失败，则返回NULL。
所谓上行转换是指由子类指针到父类指针的转换，下行转换是指由父类指针到子类指针的转换。
(6)Eigen中的数据类型
在Eigen中已经定义好了很多与旋转、变换等有关的数据类型，我们直接使用就可以了，不需要再重新定义了。
- 旋转矩阵（3 3）：Eigen::Matrix3d
- 旋转向量(3 1)：Eigen::AngleAxisd
- 欧拉角（3 1）：Eigen::Vector3d
- 四元数（4 1 ）：Eigen::Quaterniond
- 欧式变换矩阵（4 4）：Eigen::Isometry3d
- 仿射变换矩阵（4 4）：Eigen::Affine3d
- 射影变换矩阵（4 4）：Eigen::Projective3d
(7)SIZE_T
它可以用来表示非常大的数字范围，可以充分利用计算机的位数，比int、long等范围更大。
(8)循环
C++中for循环的简略写法，类似Python中的循环写法。
2.g2o库简介与编译安装
g2o全名叫General Graph Optimization，通用图优化。由字面意思即可理解，即只要是一个优化问题能转成图优化的形式就可以使用g2o来解决。
首先从g2o的Github网页上下载源码，解压。然后cd到解压目录下，如果不想麻烦，直接,然后，最后即可大功告成。g2o编译依赖于Cmake和Eigen两个库，这两个库都非常好安装，如果没有安装，百度一下如何安装就可以了。 
3.图优化曲线拟合
将曲线拟合问题抽象成图优化，只需要记住节点为优化变量，边为误差项。
在曲线拟合问题中，整个问题只有一个顶点：曲线模型参数，而各个带噪声的数据点，构成了一个个误差项，也即图优化的边。但这里的边是一元边(Unary Edge)，即只连接一个顶点。因为整个图只有一个顶点，所以只能画成自己连接到自己的样子。在图优化问题中，一条边可以连接一个、两个或多个顶点，这反映了每个误差与多少个优化变量有关。这种边也叫做超边(Hyper Edge)，整个图叫超图(Hyper Graph)。
4.图优化曲线拟合实例
下面针对一个具体的问题来实际练习g2o的使用，拟合函数
[y = exp(3.5x^3+1.6x^2+0.3x+7.8)]
整个图优化的流程如下：
- (1)定义顶点和边的类型
- (2)构建图
- (3)旋转优化算法
- (4)调用g2o优化，返回结果
首先新建一个项目，然后在CMakeLists文件里引用g2o和相关库。
然后编写代码文件。
执行结果如下： 
5.复杂一点的例子
前面的例子是利用g2o拟合曲线，但我们更想用它来优化位姿。下面的代码通过装载已知数据集的数据并对其进行优化。
利用g2o viewer可以方便的对图进行可视化。首先是，优化之前是这样的。 
优化完成后控制台输出如下 
优化后可视化的图如下 
同理，将上述代码稍作修改，对数据集优化。优化前图如下 
优化后如下 
这段代码相对于曲线拟合反而要少一些，原因是这些顶点和边的类型在库中已经定义好了，不需要我们再重新去写了。
6.g2o库结构
在上面对g2o有了简单的了解以后，再看下面这张图会相对容易一些。 
它表示了g2o中的类结构。 首先根据前面的代码经验可以发现，我们最终使用的是一个对象，因此我们要维护的就是它(对它进行各种操作)。 一个SparseOptimizer是一个可优化图(OptimizableGraph)，也是一个超图(HyperGraph)。而图中有很多顶点(Vertex)和边(Edge)。顶点继承于BaseVertex，边继承于BaseUnaryEdge、BaseBinaryEdge或BaseMultiEdge。它们都是抽象的基类，实际有用的顶点和边都是它们的派生类。我们用和向一个图中添加顶点和边，最后调用完成优化。
在优化之前还需要制定求解器和迭代算法。一个拥有一个，它继承自Gauss-Newton, Levernberg-Marquardt, Powell’s dogleg三者之一。同时，这个拥有一个，它含有两个部分。一个是 ，用于计算稀疏的雅可比和海塞矩阵；一个是线性方程求解器，可从PCG、CSparse、Choldmod三选一，用于求解迭代过程中最关键的一步：
[HDelta x=-b]
因此理清了g2o的结构，也就知道了其使用流程。在之前已经说过了，这里就再重复一遍。
- (1)选择一个线性方程求解器，PCG、CSparse、Choldmod三选一，来自文件夹
- (2)选择一个BlockSolver，用于求解雅克比和海塞矩阵，来自文件夹
- (3)选择一个迭代算法，GN、LM、DogLeg三选一，来自文件夹
7.双视图Bundle Adjustment(光束法平差)
如有两帧影像，我们的目标是估计相机在两帧影像间的运动。已知数据是相机内参(认为固定不变)和两张影像。 求解这个问题可以有两大类思路，一是通过特征点来求解，称为Sparse方式，特征点法。第二种是求解全部像素，称为Dense方式，直接法。 目前主流仍然在使用特征点法。所以这里就采用主流方法。
特征点方法的观点认为一个图像可以用几百个具有代表性的、比较稳定的点来表示。一旦有了这些点，就可以忽略图中的其余部分，而只关注这些点。（dense思路则反对这一观点，认为它丢弃了图像大部分信息，毕竟一个640x480的图有30万个点，而特征点只有几百个）。
在特征点的思路下，问题可以这样描述。有两张影像Img1、Img2,通过特征匹配共找到了N对同名点。
[P_1=left { p_1^1,p_1^2,...,p_1^N ight },P_2=left { p_2^1,p_2^2,...,p_2^N ight }]
其中
[p_1^i=begin{pmatrix}
u_1^i\
v_1^i
end{pmatrix},p_2^i=begin{pmatrix}
u_2^i\
v_2^i
end{pmatrix}]
已知相机内参矩阵为K，求解两张影像中相机的运动R,t。u、v为像素坐标。
假设某个特征点j在真实世界坐标系下的坐标为(X^j),在两幅影像的像素坐标系下分别为(p_1^j,p_2^j)。 我们要找的是真实坐标与像素坐标间的联系。 根据前面的知识可以知道，要想由真实坐标转到像素坐标，需要依次经过如下几步：
真实世界坐标->相机坐标->图像坐标->像素坐标
由相机坐标转像素坐标在这篇博客小孔成像模型里已经介绍了，这里直接给出公式
[begin{pmatrix}
u\
v\
1
end{pmatrix}=frac{1}{Z}Kbegin{pmatrix}
X\
Y\
Z
end{pmatrix}]
这里u、v为像素坐标，X、Y、Z为相机坐标系坐标。也就是说我们只需要再找到相机坐标与真实世界坐标间的变换矩阵T，既可以实现由真实坐标转像素坐标的流程了。而这个变换矩阵T其实就可以看成是相机此时的位姿或者说外方位元素。而这个很不巧，仅仅根据两张图片是无法获得相机的位姿的。但我们又必须要知道这个变换矩阵T来表示相机位姿，否则就没法继续算下去。所以没有办法，只能假设第一帧的这个变换矩阵T为单位阵。那么这样的话可以理解为真实世界坐标系与第一帧的相机坐标系重叠，也即真实世界坐标系下的坐标与第一帧相机坐标系下的坐标相等。
[begin{matrix}
X_{camera} =T_{world}^{camera}X_{world}\
T=I\
X_{camera}=X_{world}\
end{matrix}]
所以，由上面的小孔成像模型则有
[Z_1^jbegin{pmatrix}
u_1^j\
v_1^j\
1
end{pmatrix}=Kbegin{pmatrix}
X_1^j\
Y_1^j\
Z_1^j
end{pmatrix}]
整理简写一下就是
[Z_1^jbegin{pmatrix}
p_1^j\
1
end{pmatrix}=KX_1^j]
注意这里X不是单独的X变量，而是表示一个点坐标。同理，对于第二张影像我们也可以写出这个式子。
[Z_2^jbegin{pmatrix}
p_2^j\
1
end{pmatrix}=KX_2^j]
而我们在之前假设，相机从图像1到2的运动为R，t。相机的运动其实可以看作是相机坐标系的运动，所以两个坐标系之间有下面的等式成立
[X_2^j=RX_1^j+t]
将这个式子带入模型中去，有
[Z_2^jbegin{pmatrix}
p_2^j\
1
end{pmatrix}=K(RX_1^j+t)]
[Z_1^jbegin{pmatrix}
p_1^j\
1
end{pmatrix}=KX_1^j]
这个问题的传统做法是把两个方程中的(X^j)消去，得到只关于p、R、t的关系式，然后优化求解。这便是之前说的对极几何和基础矩阵(Essential Matrix)。理论上只需要大于等于8对同名点就能计算出R,t。当然尺度问题令说。
但在图优化中，我们可以这样写
[min_{X^j,R,t}(begin{Vmatrix}
frac{1}{Z_1^j}K{X_1^j} - (p_1^j,1)^T
end{Vmatrix}^2+
begin{Vmatrix}
frac{1}{Z_2^j}Kleft({R{X_1^j} + t} ight) - (p_2^j,1)^T
end{Vmatrix}^2)]
由于各种噪声存在，投影关系不能完美满足，所以转而优化它们误差的二范数。因此对于每一个特征点都能写出一个二范数误差项。对它们求和，便得到了整个优化问题。
[min_{X,R,t}sum_{j=1}^{N}(begin{Vmatrix}
frac{1}{Z_1^j}K{X_1^j} - (p_1^j,1)^T
end{Vmatrix}^2+
begin{Vmatrix}
frac{1}{Z_2^j}Kleft({R{X_1^j} + t} ight) - (p_2^j,1)^T
end{Vmatrix}^2)]
这便是最小化重投影误差问题(Minimization of Reprojection Error)。它是一个非线性、非凸的优化问题。它的误差项是将像素坐标(观测到的投影位置)与3D点按照当前估计的位姿进行投影得到的位置相比较得到的误差，所以称为重投影误差。在实际操作中，通过调整每个(X^j)，使得它们更符合每一次观测(p^j)，使每个误差项都尽可能的小。
在这个双视图BA中，一共有两种节点：
- 相机位姿节点，表达相机所在位置，是一个SE(3)元素
- 特征点的空间位置节点，是一个XYZ坐标
边主要表示空间点到像素坐标的投影关系，也就是成像模型
[Z^jbegin{pmatrix}
p^j\
1
end{pmatrix}=K(RX^j+t)]
代码实现
CMakeList
这里的list和上面第5部分的是一样的。尤其需要注意target link里面的各种库，Cholmod等等，如果少了可能编译会不通过。
C++代码
代码中同时将利用对极约束求解位姿作为对比，同时运行了基于对极几何和基于Bundle Adjustment的求解，运行部分结果如下 
项目放在Github上了，点击查看。
8.参考资料
- https://blog.csdn.net/rs_huangzs/article/details/
- https://blog.csdn.net/tian_/article/details/
- https://blog.csdn.net/_/article/details/
- https://blog.csdn.net/heyijia0327/article/details/
版权声明：
本文来源网络，所有图片文章版权属于原作者，如有侵权，联系删除。
本文网址：https://www.mushiming.com/mjsbk/14950.html