从零开始做自动驾驶定位(十): 后端优化 - 网站源码_资源分享

本该文基础建设源标识符门牌号：https://github.com/Little-Potato-1990/localization_in_auto_driving

原始数据：https://pan.baidu.com/s/1TyXbifoTHubu3zt4jZ90Wg抽取码: n9ys

本篇该文相关联的标识符Tag为 10.0

标识符在先期可能会有修正，如和该文有进出，以实际标识符为依据

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

一、简述

其实，对于两个建图控制系统来说，有了后端才是有了核心。他们后面用了三则该文如是说了工程建设是不是建、后端鸭岛是不是搭，而且还专门用三篇该文修正了标识符内部结构，虽然止到这篇该文以后，那个控制系统里仍然只同时实现了后端鸭岛机能，但是标识符内部结构早已接近最终的状态了，各组件社会分工明确，这些都为他们加进后端强化演算法奠定了好的基础。因此这次他们只是在那次的内部结构其内DR2100填标识符就可以了。

后端通过图强化同时实现消解总计数值的机能，目前有三个盛行的库：g2o、gtsam、ceres，该处他们换用g2o，它的一些如是说网路上都有，假如不太了解，就先另行搜寻一下吧。按照他们以后的设计路子，程序设计如果具有可扩展性，即假如以后想改成其他库，如果努力做到不须要更动过多标识符就可同时实现。执行这一路子的作法就是采用隐式，即Mauvezin两个tcsh表述强化演算法的USB，然后g2o作为它的常量同时实现那个USB。详尽基本原理和作法在以前该文里如是说过了，该处不再多次重复。

g2o的美版莫拉，只提供了se3的三角形和边，他们这里要用gnss位置做探测，因此须要自己表述两个与探测相相关联的边。

为了保持组件的自主性，他们把强化演算法的具体同时实现原则上封装成两个组件，放在models配置文档中，back_end再初始化那个组件。

二、工程建设实用性与文档内部结构

1. 工程建设实用性

他们的g2o采用的是晚期版，现在捷伊版更动早已比较大了，假如校对过程中g2o收起，能用本工程建设提供的g2o安装文档再安装一次。安装文档放在了setup_file配置文档下。

为了引用g2o，他们在工程建设目录cmake配置文档下加进了g2o.cmake，然后CMakeLists.txt文档再初始化这个cmake，以同时实现依赖。

2. 文档内部结构

除了上面说的工程建设实用性以外，他们把强化演算法放在models/graph_optimizer配置文档下，USB类（即基类）直接在配置文档一级目录里，g2o相关的PCB放在models/graph_optimizer/g2o配置文档下。自表述的边放在models/graph_optimizer/g2o/edge配置文档下。这样层级分布如果还算明确。

对强化演算法的初始化仍然在以后介绍的back_end组件的back_end.cpp文档里。

三、标识符同时实现

详尽来说，该处的标识符同时实现，重点包括三部分：一是g2o自表述边，二是g2o强化演算法PCB，三是back_end对强化演算法的初始化。

1. g2o自表述边

这里须要表述的边是用来输入探测量的先验边。

kitti数据集可用的探测量包括GNSS位置和GNSS姿态两个，对那个问题，他们做两个小的讨论。

后端鸭岛加进探测消解数值，其实就是两个融合，融合后的位姿用来拼接点云。在这样的任务里，对姿态探测准确度的要求要远大于对位置探测量准确度的要求。另一方面，GNSS组合导航控制系统中，位置探测往往都是有RTK高精度信息做基础的，相对更准确，而姿态是通过导航控制系统里对位置数值的探测反向推导的，往往稍差，但这并不是说姿态不能用来做探测，而是在采用以后要对导航控制系统做足够的评估，确认其姿态精度是能够达到做探测的要求的，那个评估往往又须要两个更高精度的导航控制系统做基准。对于kitti数据集提供的数据，他们也无法再去做这样的工作，因此该处他们仅以位置作为探测量，而不采用姿态探测。

假如各位有意愿，就是非常想尝试一下姿态探测的效果，我在标识符里也表述了姿态探测的边，您能直接采用那个另行把姿态探测加进去，本篇该文中就不再对那个内容做过多如是说了。

每条边的内容，本质就是加进探测和计算数值，在位置探测相关联的边里，这两步的标识符如下

void setMeasurement(const Eigen::Vector3d& m) override { _measurement = m; } void computeError() override { const g2o::VertexSE3* v1 = static_cast<const g2o::VertexSE3*>(_vertices[0]); Eigen::Vector3d estimate = v1->estimate().translation(); _error = estimate – _measurement; }

其中位置以三维向量的形式存在，位置数值就是探测向量和状态估计向量之差。

2. 强化演算法PCB

他们把强化演算法的USBPCB成以下函数

// 强化 bool Optimize(); // 输出数据 bool GetOptimizedPose(std::deque<Eigen::Matrix4f>& optimized_pose); int GetNodeNum(); // 加进节点、边、鲁棒核 void SetEdgeRobustKernel(std::string robust_kernel_name, double robust_kernel_size); void AddSe3Node(const Eigen::Isometry3d &pose, bool need_fix); void AddSe3Edge(int vertex_index1, int vertex_index2, const Eigen::Isometry3d &relative_pose, const Eigen::VectorXd noise); void AddSe3PriorXYZEdge(int se3_vertex_index, const Eigen::Vector3d &xyz, Eigen::VectorXd noise); void AddSe3PriorQuaternionEdge(int se3_vertex_index, const Eigen::Quaterniond &quat, Eigen::VectorXd noise);

这里面须要说明的是，每条边的输入参数里有两个noise，那个就是这条边相关联的信息的噪声，在g2o里体现为信息矩阵，其实就是该信息在强化过程中的权重。

噪声的大小可以在实用性文档里设置，鸭岛、GNSS、闭环检测各相关联两个噪声向量。

3. 强化演算法初始化

他们把加进三角形、边的机能PCB在两个函数里，当有捷伊关键帧到来时初始化该函数

bool BackEnd::AddNodeAndEdge(const PoseData& gnss_data) { Eigen::Isometry3d isometry; // 加进关键帧节点 isometry.matrix() = current_key_frame_.pose.cast<double>(); graph_optimizer_ptr_->AddSe3Node(isometry, false); new_key_frame_cnt_ ++; // 加进激光鸭岛相关联的边 static KeyFrame last_key_frame = current_key_frame_; int node_num = graph_optimizer_ptr_->GetNodeNum(); if (node_num > 1) { Eigen::Matrix4f relative_pose = last_key_frame.pose.inverse() * current_key_frame_.pose; isometry.matrix() = relative_pose.cast<double>(); graph_optimizer_ptr_->AddSe3Edge(node_num-2, node_num-1, isometry, graph_optimizer_config_.odom_edge_noise); } last_key_frame = current_key_frame_; // 加进gnss位置相关联的先验边 if (graph_optimizer_config_.use_gnss) { Eigen::Vector3d xyz(static_cast<double>(gnss_data.pose(0,3)), static_cast<double>(gnss_data.pose(1,3)), static_cast<double>(gnss_data.pose(2,3))); graph_optimizer_ptr_->AddSe3PriorXYZEdge(node_num – 1, xyz, graph_optimizer_config_.gnss_noise); new_gnss_cnt_ ++; } return true; }

剩下的就是强化了。

由于强化本身要耗一定时间，而且强化完之后，强化后的位姿要发送给viewer组件，后者利用强化后的信息生成全局点云地图，并在rviz上显示。全局点云地图本身比较大，产生和发送地图都非常耗时间，因此他们目前把强化控制在两个很低的频率上。

那个强化频率在实用性文档里有相关联的参数，含义如下：

optimize_step_with_key_frame：当新到关键帧计数达到那个值时做一次强化

optimize_step_with_gnss：当新产生产生的gnss探测技术达到那个值时做一次强化

optimize_step_with_loop：当新得到闭环检测边的计数达到那个值时做一次强化

当这三个条件满足其中之一时，即做强化，并把三个计数均清零。

但是，这会带来两个情况，就是当bag文档播放完之后，关键帧里的后面几帧可能并没有得到强化，因为上面三个条件计数还没达到新一次强化所需的数量要求。为了解决那个情况，他们提供了两个指令

rosservice call /optimize_map

输入那个指令之后，它会对当前所有的关键帧统一做一次强化，并用强化后的位姿重新产生一次全局地图，发送在rviz上。

同时，以前该文所提到的保存点云地图的指令仍然有效

rosservice call /save_map