本文是我在学习深蓝学院的激光slam课程第六次实践的笔记。本次作业主要是熟悉并手写基于优化的 SLAM 算法，在此基础对后端优化知识及其相关常用库进行一定扩展。
基于高斯牛顿法实现图优化
补充代码,实现高斯牛顿方法对 Pose-Graph 进行优化
代码文件结构
老规矩还是先熟悉代码文件结构。项目结构比较简单：
LSSLAMProject
└── src
└── ls_slam
├── CMakeLists.txt
├── data------------------------数据文件夹，包括图中的节点的信息和边信息
│ ├── intel-e.dat
│ ├── intel-v.dat
│ ├── killian-e.dat
│ ├── killian-v.dat
│ ├── test_quadrat-e.dat
│ └── test_quadrat-v.dat
├── include---------------------项目头文件
│ └── ls_slam
│ ├── gaussian_newton.h
│ └── readfile.h
├── package.xml
└── src-------------------------项目源代码
├── gaussian_newton.cpp-----高斯牛顿法实现，算法核心部分
├── main.cpp----------------主函数，主要起数据输入输出、数据处理及可视化的作用
└── readfile.cpp------------数据文件读取函数实现
下面来看一下主要文件的结构和作用：
-
main.cpp这次的文件结构比较简单，由于数据不是通过
rosbag来进行存储，而是通过dat文件所以可以直接读取不需要订阅相关话题。并且这次也没有把相关的函数放入类中，而是直接调用gaussian_newton.h中的方法。main.cpp的工作大致如下：- 初始化 node 节点；初始化两个消息类型为
visualization_msgs::MarkerArray的 publisher 主要用来显示优化前和优化后的机器人位姿轨迹 - 定义数据文件的路径，并通过
ReadVertexInformation()和ReadEdgesInformation()从两个文件读取节点信息和边信息进相应的节点/边 vector 中，这里节点和边的定义分别为：- 节点：
Eigen::Vector3d代表$(x, y, \theta)$ - 边：
Edge包括- 测量值：
Eigen::Vector3d代表$(dx, dy, d\theta)$ - 相应的信息矩阵：
Eigen::Matrix3d
- 测量值：
- 节点：
- 通过
ComputeError()节点和边计算误差 - 通过
LinearizeAndSolve()对节点和边进行图优化 - 对优化后的节点和边重新进行误差计算
- 通过
PublishGraphForVisulization(）发布优化前后的数据，并进行可视化，这里要看看这个函数是怎么发布以及可视化方便实际做的时候正确查看结果- 函数接收
publisher，Vertexs，Edges和color作为参数 - 首先定义对应两个
visualization_msgs::Marker作为点和边- 对点而言，定义了其参考坐标系
map， marker 类型是SPHERE，命名空间ns是ls-slam - 对边而言，参考坐标系同样是
map， marker 类型是LINE_STRIP（用来显示线），命名空间定义成carto为了和点区分开
- 对点而言，定义了其参考坐标系
- 将不同节点和边放入
MarkerArray向量中：- 对节点很简单，只修改修改
id，x和y即可 - 对边而言，每条边对应两个点，所以要将两个点都放入边中，然后将边放入相应
MarkerArray中
- 对节点很简单，只修改修改
- 发布消息
- 函数接收
- 初始化 node 节点；初始化两个消息类型为
-
readfile.cpp这个文件中的函数都比较简单，主要包含三个函数：
stringToNum()：数字字符串转成数字类型（string->int/float/double）splitString()：根据给定的分隔符和一行字符串分割成多个所需字符串ReadVertexInformation()：读取节点数据，这里首先看一下节点数据文件：- 每一行 5 个部分，分别是：标识（
VERTEX2）, 节点id，x,y和theta - 根据分隔符为空格分割每一行，并转成
double存入Vector3d即可
- 每一行 5 个部分，分别是：标识（
ReadEdgesInformation()：读取边数据，同样看一下数据文件：- 每一行 12 个数据，分别是：
- 标识 (
EDGE2) - 该边对应的两个节点的
id - 观测值
dx， dy， dtheta - 信息矩阵的值
xx, xy, xt, yy, yt, tt（因为信息矩阵是对称矩阵，所以 3x3 的矩阵只需要六个值就能完全确定）
- 标识 (
- 同样依次读取每一行，分割，转类型，存入即可
- 每一行 12 个数据，分别是：
-
gaussian_newton.cpp这个文件中核心函数有两个，分别是：
ComputeError()：计算整个图的误差LinearizeAndSolve()：线性化图并通过高斯牛顿法进行求解
此外还有一些工具函数：
PoseToTrans()和TransToPose()位姿向量和转换矩阵之间的转变函数CalcJacobianAndError()：对两点以及之间的观测值计算误差和雅克比矩阵
以上就是这个项目的文件结构，缺失的函数主要是 CalcJacobianAndError() 和 LinearizeAndSolve()，现在思路比较清晰了，下面开始进行补充
高斯牛顿法相关代码
CalcJacobianAndError()
具体数学原理已经在我的前一篇笔记中 基于图优化的激光 SLAM 方法 中图构建的部分介绍过，这里直接放代码，容易漏的部分是涉及角度的时候需要正则化：
/**
* @brief CalcJacobianAndError
* 计算jacobian矩阵和error
* @param xi fromIdx
* @param xj toIdx
* @param z 观测值:xj相对于xi的坐标
* @param ei 计算的误差
* @param Ai 相对于xi的Jacobian矩阵
* @param Bi 相对于xj的Jacobian矩阵
*/
void CalcJacobianAndError(Eigen::Vector3d xi,Eigen::Vector3d xj,Eigen::Vector3d z,
Eigen::Vector3d& ei,Eigen::Matrix3d& Ai,Eigen::Matrix3d& Bi)
{
Eigen::Matrix3d Xi = PoseToTrans(xi);
Eigen::Matrix2d Ri = Xi.block(0, 0, 2, 2);
Eigen::Vector2d ti{ xi(0), xi(1)};
Eigen::Matrix3d Xj = PoseToTrans(xj);
Eigen::Matrix2d Rj = Xj.block(0, 0, 2, 2);
Eigen::Vector2d tj{ xj(0), xj(1)};
Eigen::Matrix3d Z = PoseToTrans(z);
Eigen::Matrix2d Rij = Z.block(0, 0, 2, 2);
Eigen::Vector2d tij{ z(0), z(1)};
Eigen::Matrix2d dRiT_dtheta; // derivative of Ri^T over theta
dRiT_dtheta(0, 0) = -1 * Ri(1, 0); // cosX -> -sinX
dRiT_dtheta(0, 1) = 1 * Ri(0, 0); // sinX -> cosX
dRiT_dtheta(1, 0) = -1 * Ri(0, 0); // -sinX -> -cosX
dRiT_dtheta(1, 1) = -1 * Ri(1, 0); // cosX -> -sinX
// calcuate error & normalize error on theta
ei.segment<2>(0) = Rij.transpose() * (Ri.transpose() * (tj - ti) - tij);
ei(2) = xj(2) - xi(2) - z(2);
if (ei(2) > M_PI) {
ei(2) -= 2 * M_PI;
} else if (ei(2) < -1 * M_PI) {
ei(2) += 2 * M_PI;
}
Ai.setZero();
Ai.block(0, 0, 2, 2) = -Rij.transpose() * Ri.transpose();
Ai.block(0, 2, 2, 1) = Rij.transpose() * dRiT_dtheta * (tj - ti);
Ai(2, 2) = -1.0;
Bi.setIdentity();
Bi.block(0, 0, 2, 2) = Rij.transpose() * Ri.transpose();
}
LinearizeAndSolve()
同样，大部分的数学原理参考之前做的笔记即可，这里主要是注意一下 Eigen 库当中 Matrix 和 Vector 的一些相关操作，以及怎么利用 Eigen 库进行稀疏方程组的求解，代码如下：
/**
* @brief LinearizeAndSolve
* 高斯牛顿方法的一次迭代．
* @param Vertexs 图中的所有节点
* @param Edges 图中的所有边
* @return 位姿的增量
*/
Eigen::VectorXd LinearizeAndSolve(std::vector<Eigen::Vector3d>& Vertexs,
std::vector<Edge>& Edges)
{
//申请内存
Eigen::MatrixXd H(Vertexs.size() * 3,Vertexs.size() * 3);
Eigen::VectorXd b(Vertexs.size() * 3);
H.setZero();
b.setZero();
//固定第一帧
Eigen::Matrix3d I;
I.setIdentity();
H.block(0, 0, 3, 3) += I;
//构造H矩阵 ＆ b向量
for(int i = 0; i < Edges.size();i++)
{
//提取信息
Edge tmpEdge = Edges[i];
Eigen::Vector3d xi = Vertexs[tmpEdge.xi];
Eigen::Vector3d xj = Vertexs[tmpEdge.xj];
Eigen::Vector3d z = tmpEdge.measurement;
Eigen::Matrix3d infoMatrix = tmpEdge.infoMatrix;
//计算误差和对应的Jacobian
Eigen::Vector3d ei;
Eigen::Matrix3d Ai;
Eigen::Matrix3d Bi;
CalcJacobianAndError(xi,xj,z,ei,Ai,Bi);
// 更新 b
b.segment<3>(tmpEdge.xi * 3) += Ai.transpose() * infoMatrix * ei;
b.segment<3>(tmpEdge.xj * 3) += Bi.transpose() * infoMatrix * ei;
// 更新 H
H.block(tmpEdge.xi * 3, tmpEdge.xi * 3, 3, 3) += Ai.transpose() * infoMatrix * Ai;
H.block(tmpEdge.xi * 3, tmpEdge.xj * 3, 3, 3) += Ai.transpose() * infoMatrix * Bi;
H.block(tmpEdge.xj * 3, tmpEdge.xi * 3, 3, 3) += Bi.transpose() * infoMatrix * Ai;
H.block(tmpEdge.xj * 3, tmpEdge.xj * 3, 3, 3) += Bi.transpose() * infoMatrix * Bi;
}
//求解
Eigen::VectorXd dx;
Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
Eigen::SparseMatrix<double> sp_H = H.sparseView();
solver.compute(sp_H);
if (solver.info() != Eigen::Success) {
std::cout << "decomposition failed" << std::endl;
}
dx = solver.solve(-b);
if (solver.info() != Eigen::Success) {
std::cout << "decomposition failed" << std::endl;
}
return dx;
}
位姿更新
做完了前两步之后要记得在 main.cpp 中完成位姿节点的更新，代码比较简单就这里就不放了。
结果
test_quadrat-e & test_quadrat-v
运行结果：
Edges:5
initError: 251853
Iterations: 0
Iterations: 1
Iterations: 2
FinalError: 49356.5
结果如下图所示：
intel-e & intel v
运行结果：
Edges:3070
initError: 2.05092e+06
Iterations: 0
Iterations: 1
Iterations: 2
Iterations: 3
Iterations: 4
FinalError: 65.402
结果如下图所示
killian-e & killian-v
运行结果：
Edges:3995
initError: 3.08592e+08
Iterations: 0
Iterations: 1
Iterations: 2
Iterations: 3
Iterations: 4
Iterations: 5
FinalError: 10344.7
结果如下图所示：
从上面结果来看，图优化的效果还是很显著的。
算法分析
简答题,开放性答案:你认为第一题的优化过程中哪个环节耗时最多?是否有什么改进的方法可以进行 加速?
耗时最多的环节比较显然：对 $H\Delta x = -b$ 求解的过程，其余的环节例如求雅克比，构建 H 和 b 基本只需要花费线性时间（求雅克比的时候避免对矩阵求逆，而是通过解析解的方式直接计算）。而求最小二乘解即便利用矩阵的稀疏性优化求解时间花费也是在线性时间之上。
加速的方法就是利用矩阵的稀疏性来优化求解，同时在构建 H 和 b 的过程中，可以考虑使用多线程进行构建，也能减小一部分时间。
其他非线性方法
学习相关材料。除了高斯牛顿方法,你还知道哪些非线性优化方法?请简述它们的原理
-
最速下降法： 利用
$\Delta x = -\lambda J$根据当前梯度的反方向进行迭代，通过$\lambda$控制下降速度- 优点：实现结果，不涉及复杂计算
- 缺点：优化质量不稳定，到最后会出现 ”之“ 字型下降，可能会很难收敛
-
牛顿法：将 $|| f(x) ||}^2 展开， 利用一阶和二阶梯度信息：
$\Delta x = \arg\min ({|| f(x) ||}^2 + J(x) \Delta x + \dfrac{1}{2} \Delta x^T H \Delta x ) \rightarrow H \Delta x = -J^T$- 优点：利用了二阶信息，优化质量更好
- 缺点：需要计算
$H$矩阵，计算量会很大
-
高斯牛顿法：牛顿法的一种改良，将
$|| f(x) ||$展开：$f(x + \Delta x) \approx f(x) + J(x) \Delta x \rightarrow \Delta x^{\ast} = \arg\min \dfrac{1}{2} {\mid \mid f(x) + J(x) \Delta x \mid \mid }^2$${J(x)}^T J(x) \Delta x = - {J(x)}^T f(x) \rightarrow H \Delta x = -b$- 优点：避免了直接计算
$H$而是用$J^TJ$作为其近似，减少计算量
-
列文伯格-马夸尔特方法：高斯牛顿法的一种改良，通过在
$J^TJ$的基础上添加阻尼项$\rho I$提高算法稳定性。- 利用近似值和实际值之间的差异确定阻尼系数：
$\rho = \dfrac{f(x + \Delta x) - f(x)}{J(x) \Delta x} $
- 利用近似值和实际值之间的差异确定阻尼系数：
-
DogLeg 方法： 引入信赖区域来代替 LM 算法中的阻尼项
非线性库使用
将第一题改为使用任意一种非线性优化库进行优化(比如 Ceres, Gtsam 或 G2o 等)。
- Ceres: Ceres 学习记录（一）
- Gtsam: GTSAM 学习记录（一）
- G2O: G2O 学习记录（一）