lvi-sam 代码阅读 和 总结

Lvi-sam

lidar-visual-inertial odometry and mapping system

• 激光-视觉-IMU 里程计和建图，结合了lio-sam和vins-mono系统的优点。

• 总体框架示意图：
  

• 各个节点 数据传输示意图
  

心得

• 为相机特征点预估深度
  • 3D激光点的共视区域给相机中每个像素点预估深度，通过多帧激光帧融合确保每个像素值都有值
  • 无激光运行时，采用 三角化 进行计算每个特征点深度（初始时对极几何）
• 视觉里程计为3D激光去畸变，位置和角度
  • 若无视觉里程计，则只能imu去畸变，只去角度
• 激光优化中，视觉里程计为初始值提供预估，若没有视觉里程计，则imu提供角度，imu没有则上一帧位姿作为当前位姿
  • 回环：视觉可提供回环，激光也能提供回环
  • 激光优化回调算例 很吃力啊，构建submap并优化，全局因子图优化，很吃算力

依赖运行

• 依赖：

  • ros 测试： kinetic and melodic (16.04 和20.04)

  • gtsam：https://gtsam.org/get_started/

    • sudo add-apt-repository ppa:borglab/gtsam-release-4.0
    • sudo apt install libgtsam-dev libgtsam-unstable-dev

  • ceres：https://github.com/ceres-solver/ceres-solver/releases

     sudo apt-get install -y libgoogle-glog-devsudo apt-get install -y libatlas-base-devwget -O ~/Downloads/ceres.zip https://github.com/ceres-solver/ceres-solver/archive/1.14.0.zipcd ~/Downloads/ && unzip ceres.zip -d ~/Downloads/cd ~/Downloads/ceres-solver-1.14.0mkdir ceres-bin && cd ceres-bincmake ..sudo make install -j4```

• 运行：

  • 编译：catkin build (catkin_make也一样)
  • 下载bag：https://drive.google.com/drive/folders/1q2NZnsgNmezFemoxhHnrDnp1JV_bqrgV
  • 运行：
    • 运行bag：rosbag play handheld.bag
    • 运行程序：roslaunch lvi_sam run.launch

vins

visual_feature

主函数：

• 初始化Ros节点

  • 设置Log等级

    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn);

  • 读取参数 每个节点都读取一遍，好费劲

• 读取相机参数 N个相机参数单独读取

  • 如果有鱼眼相机时，读取鱼眼mask

• 初始化深度寄存器(在读取参数后) DepthRegister

• 订阅 激光和图像 话题，若不适用激光时，sub_lidar.shutdown();

  • img_callback
  • lidar_callback （去过畸变的点云）

• 发布topic：

  • feature，restart（视觉里程计用）、feature_img（rviz）

• 两个线程，MultiThreadedSpinner， 用于并行处理（图像和激光雷达）

lidar_callback

• 1、跳帧，++lidar_count % (LIDAR_SKIP+1) != 0

• 2、得到 vins_world 到 body的转换关系transNow

  • tf listen 读取失败时 return
  • 转换为 Eigen::Affine3f

• 3、点云数据处理

  • laser cloud 转换为 pcl
  • 降采样 (0.2,0.2,0.2)
  • 点云滤波(仅在相机视图中保留点) x>=0&&y/x<=10&&z/x<=10
  • 由激光坐标系转换为 相机坐标系 pcl::transformPointCloud
  • 转换到全局里程计坐标系，使用了 tf接听的transNow

• 4、保存点云队列 点云+time 两个队列 cloudQueue、timeQueue

• 5、弹出队列中老的数据，保留5s数据

• 6、融合队列中的点云数据depthCloud，即将队列中所有点云数据相加

• 7、融合后的点云数据降采样，(0.2,0.2,0.2)

img_callback

• 若 first_image_flage 时，赋值 first_image_time、last_image_time 返回
• 相机数据流稳定性检测，时间间隔>1s 或者 时间回跳
  • 异常时，发送 restart标志，并return
• 发布当前帧频率控制PUB_THIS_FRAME，发布时 ++pub_count
  • round(1.0 * pub_count / (cur_img_time - first_image_time)) <= FREQ
  • 重置 pub_count ，first_image_time
• image数据转换为cv::Mat，并trackerData[i].readImage，核心：readImage
• PUB_THIS_FRAME时，发布topic，pub_feature，注：depthRegister->get_depth

readImage

• 直方图均衡化，参数：cv::createCLAHE(3.0, cv::Size(8, 8)

• 若 forw_img是空， 则 prev_img = cur_img = forw_img = img;

• 若 cur_pts.size() > 0时，光流跟踪，当前跟踪特征点forw_pts

  • 光流跟踪 cv::calcOpticalFlowPyrLK
  • 删除 无效的特征点

• 若发布 该帧时

  • 设置Mask，非极大值抑制
  • 若 该帧特征点个数小于预设最大值时，进行额外提取 cv::goodFeaturesToTrack
  • 并添加额外增加的点

• 赋值，并去畸变 undistortedPoints

  • cv::undistortPoints不过使用相机模型中：m_camera->liftProjective
  • 若有上一帧有匹配点时，进行速度预测

get_depth

• 初始化深度 通道，为返回做准备

  • name = "depth",values.resize(features_2d.size(), -1)

• 若无深度点云时，直接返回了，深度点云由lidar_callback得到

• 得到当前时间段 body到世界坐标系的位姿 transNow

• 将点云从 世界坐标系转换到相机坐标系 transNow.inverse()

• 将特征点投影到单位球面上，z 总是为1 features_3d_sphere

  • 转换到ros标准坐标系，x = z, y =-x,z=-y
  • 标准：前x,左y，上z，相机：前z，右x，下y
  • 强度用来存储深度，赋值 -1

• 定义求取深度的图片(-90°,90°)，分辨率 bin_res =180/360

• 遍历所有的深度点，计算raw_id，col_id，若在图像范围内，仅保留最近的点

  • row_angle =atan2(p.z, sqrt(p.x * p.x + p.y * p.y)) * 180.0 / M_PI + 90.0
  • 为了转换到 [0,180]，故需要加90°
  • col_angle = atan2(p.x, p.y) * 180.0 / M_PI;
  • row_id = row_angle / bin_res，col_id=col_angle/bin_res。
  • 若已经更新时，只取最近的

• depth_cloud_local赋值，发布深度到vins_body_ros坐标系

• 将深度点云图 depth_cloud_local进行归一化，得到 depth_cloud_unit_sphere

  • x,y,z/range ，强度保存了深度值，

• 通过归一化深度图 depth_cloud_unit_sphere，创建 kd_tree

• 遍历 归一化特征点features_3d_sphere，得到各个点的深度

  • 在 kd_tree中找到3个临近的点，阈值5个像素的平方
  • 可以找到3个且距离小于阈值时，做如下操作：
    • 取3个点数据：A、B、C, 每个点的三维坐标(归一化坐标*深度)和深度r
    • 归一化特征点 $V$（归一化坐标）
    • 计算ABC确定的法向量 $N$
    • 计算原点到 平面的距离 (N(0) * A(0) + N(1) * A(1) + N(2) * A(2))
    • 计算原点到 归一化特征点与法向量$N$确定的平面的距离 (N(0) * V(0) + N(1) * V(1) + N(2) * V(2))
    • 得到 归一化特征点的深度 s = 上述二者相除
    • 若 3个点的深度相差2m 或 深度小于 0.5m时，s不变
    • s若深度大于3个点的最大深度，则赋最大深度，若小于最小深度时赋值最小深度
    • 还原特征的3d信息 (归一化数据乘以深度值) features_3d_sphere，

• 若发布深度图，则赋值不同颜色显示，并发布

• 跟新各个特征的深度点depth_of_point，并返回

visual_odometry

主函数：

• 构造 Estimator estimator 全局变量
• 初始化 ros
• 读取参数，并 estimator.setParameter()
  • 相机外参，td，信息矩阵
• 订阅 restart_callback
  • imu_callback
  • odom_callback
  • feature_back
• 若不使用 激光时 sub_odom.shutdown();
• 定义主线程 measurement_process{process};
• 两个线程，MultiThreadedSpinner， 用于并行处理

imu_callback

• 若imu 数据 时间回跳或不变时，直接 打印警告并return
• 将imu数据push 到 imu_buf 中，互斥锁 m_buf
• 条件唤醒主线程
• 发布 最近的里程计，用于rviz显示

odom_callback

• 将数据放入 odomQueue 中，互斥锁 m_odom

feature_callback

• 将数据放入 feature_buf 中，互斥锁 m_buf
• 条件唤醒主线程

process main_thread

• while ros::ok
  • 条件唤醒 measurements !=0
    • measurements =getMeasurements
  • 遍历 measurements
    • imu 预积分
      • imu_msg.time <= img_msg.time estimator.processIMU
      • 否则，基于上次线加速度和角加速度 使得二者完全对齐
    • image[feature_id]构造
  • 从激光雷达里程计获取初始化信息 odomRegister->getOdometry
    • 由于用到odometry数据，因此 互斥锁 m_odom
  • 处理图像 processImage
  • 可视化
    • 发布里程计，关键帧Pose，相机Pose，发布Tf，发布关键帧

other_function

getMeasurements

• while 1循环
  • imu_buf 和 feature_buf 有一个为空时 return
  • imu_buf.back未包含 feature_buf.fornt 时间时，return
    • imu_buf结束时间未包含要打包的 feature数据，跳过
  • imu_buf.front 未包含 feature_buf.fornt 时间时，feature_buf弹出，continue
    • imu_buf起始时间未包含feature数据时，将其弹出
    • 因为数据时间是递增的，永远不会包含，扔掉
  • 打包 imu_buf小于 feaure_buf.font的数据，即：[Imus,feaure_buf.font]

getOdometry

• 重置 odometry_channel(18,-1)

  • id(1), P(3), Q(4), V(3), Ba(3), Bg(3), gravity(1)

• 激光里程计部位空时，丢掉里程计较老的帧，odom<img_time-0.05? pop_fornt

• 激光里程计为空时，直接返回

• 得到 最接近的 q_odom_lidar

  • 找到最接近图像时间的里程计 odomCur ，小于图像视觉的最近里程计帧
  • 若里程计 odomCur 与img时间间隔大于 0.05，直接return

• 将其转换到 激光坐标系 q_odom_cam

• 转换 里程计位姿从激光坐标系到 相机坐标系

  • odomCur 转换为 p_eigen，v_eigen
  • p_eigen，v_eigen = q_lidar_to_cam_eigen* p_eigen，v_eigen
  • p_eigen，v_eigen 转换为 odomCur

• 返回 odometry_channel，由 odomCur转换而来

processImage

• addFeatureCheckParallax 添加特征到feature，并计算跟踪的次数和视差，评判出是否为关键帧
  • 若为关键帧，边缘化老帧；否则边缘化新帧
• 如果有有激光里程计且初始化有效时，边缘化老帧
• 将该帧添加到 all_image_frame中，并重新开始预积分
• 若需标定外参时，进行旋转外参标定 CalibrationExRotation 标定成功后改变状态
• 若系统为初始化状态时：
  • 若滑窗内帧个数不足预设值时，push帧
  • 进行初始化 initialStructure
  • 初始化成功后，状态为非线性优化，并进行
    • 求解里程计 solveOdometry
    • 移动滑窗 slideWindow
    • 移除为跟踪的特征点 f_manager.removeFailures()
    • 赋值
  • 否则：移动滑窗 slideWindow
• 否则系统费初始化状态：
  • 求解里程计 solveOdometry
  • 若求取失败，则重启 vins 系统
  • 移动滑窗 slideWindow
  • 移除为跟踪的特征点 f_manager.removeFailures()
  • 赋值滑窗，准备VINS的输出

initialStructure

• 激光初始化
  • 清除容器中的关键帧
    • 遍历容器中的所有帧，is_key_frame=false
  • 检测 窗口内的激光信息是否有效，无效时break
  • 若窗口内激光信息有效时：
    • 更新滑窗内的状态
    • 更新重力方向
    • 重置所有特征的深度，并进行三角化 triangulate
      • 若该点特征深度有效时，则跳过三角化
    • 返回true
• 检测imu 的可观性
  • 计算 帧间imu预积分 的加速度 (delta_v/dt)
  • 计算 imu预积分 的加速度标准差钱
  • 若标准差小于 0.25，则返回(已注销该句)
• 全局 sfm
  • 遍历 所有特征点，添加观测约束 imu_j
    • 遍历imu_j++，为该特征添加所有约束
  • 足够的视差恢复 R,t relativePose
  • 纯视觉恢复 滑窗位姿及特征 construct
• Pnp 求解所有帧
• 视觉Imu 对齐

triangulate

• 特征三角化，与原不同的是若该特征有深度时，直接跳过

visual_loop

主函数：

• ros初始化，初始化节点+句柄+log等级显示

• 加载参数

  • 评判参数路径是否正确
  • 闭环所用到的参数 yaml

• 如果 需闭环：参数设置

  • 初始化词袋
  • 初始化 brief 提取
  • 初始化相机模型

• 订阅话题：

  • image_call
  • 视觉里程计的关键帧 位姿 pose_callback
  • 视觉里程计的关键帧 特征点 point_callback
  • 视觉里程计的估计外参 extrinsic_callback

• 发布话题：

  • 闭环匹配图片 pub_match_img
  • 闭环匹配frame pub_match_msg
  • 闭环关键帧位姿 pub_key_pose

• 若无闭环时，上述订阅发布话题都 shutdown

• 构建主线程 std::thread(process);

callback

• 回调函数就是将数据放入 buf中

pose_callback

• 无闭环时，直接return
• 将数据放入 pose_buf，互斥锁 m_buf

point_callback

• 无闭环时，直接return
• 将数据放入 point_buf，互斥锁 m_buf

image_callback

• 无闭环时，直接return
• 将数据放入 image_buf，互斥锁 m_buf
• 检测 相机数据流的稳定性
  • 检测图片 时间间隔和回跳
  • 间隔>1s 或回跳时，所有队列都情况

extrinsic_callback

• 无闭环时，直接return
• 赋值 tic, qic，互斥锁 m_process

Process

• 无闭环时，直接return
• while ok
  • 数据对齐
    • 找到 image_msg、pose_msg、point_msg
    • 三者时间一致，且互斥锁 m_buf
  • 若 pose_msg != Null 时，即赋值了：
    • 跳过前十帧 static int
    • 限制频率，跳过一些帧（与降频还不一样）
    • 得到关键帧的位姿 pose_msg -> R ，T
    • 添加关键帧
      • 图片
      • 关键帧的所有地图点
      • 构造新关键帧
      • m_process.addKeyFrame，互斥锁 m_process
      • 可视化 关键帧位姿 visualizeKeyPoses
  • 5S执行一次 ，sleep_fors

addKeyFrame

• 根据 检测闭环标志 确定是否检测闭环，每帧都检测闭环
  • 若检测闭环，则执行detectLoop
  • 否则，只添加关键帧到词袋 addKeyFrameIntoVoc
• 如果检测到闭环，则 取到相应的帧，并为该帧找链接 findConnection
• 将当前帧放入 关键帧list中

detectLoop

• 1、brief词袋检测，并 返回结果，200frame以前
• 2、将该特征的描述子加入 该词袋中
• 3、遍历候选值，若 第一得分超过 0.05时，且临近超 0.015是，认为闭环成立
• 4、赋值闭环，找到距离最远的 闭环Id 得分需超 0.015

lio-sam

ImageProjection

与 lio-sam基本无差异，里程计由原 后端提供改为 视觉提供

structure

• 订阅 Topic：
  • 订阅imu原始数据 imuHandler
  • 订阅由vins提供的ros 里程计， odometryHandler
  • 订阅雷达 cloudHandler
• 发布 Topic：
  • 去完畸变后的点云 deskew/cloud_deskewed
  • 去完畸变，lvi-sam 的点云 deskew/cloud_info
• 为数据分配内存 allocateMemory
• 重置参数 resetParameters

odometryHandler

• 数据 push 进 odomQueue 队列
• 互斥锁 odoLock 单独的

imuHandler

• 先进行imu数据的转换，由imu坐标系到 ros坐标系
• 数据 push 进 imuQueue 队列
• 互斥锁 imuLock 单独的

cloudHandler

• 添加一帧激光点云到队列，取出最早一帧作为当前帧，计算起止时间戳，检查数据有效性

  cachePointCloud

• 处理当前激光帧起止时刻对应的IMU数据、IMU里程计数据

  deskewInfo

• 当前帧激光点云运动畸变校正projectPointCloud

• 提取有效激光点，存extractedCloud cloudExtraction

• 发布当前帧校正后点云，有效点 publishClouds

• 重置参数，接收每帧lidar数据都要重置这些参数 resetParameters

mapOptimization

• 定义ros节点，lidar
• 申明 mapOptimization 类
• 定义两个线程
  • 闭环线程 loopClosureThread
  • 全局地图可视化线程 visualizeGlobalMapThread

mapOptimization

• 订阅话题
  • 激光特征 信息回调 laserCloudInfoHandler
  • GPS 信号回调 gpsHandler
  • 闭环信号 loopHandler
• 发布话题
  • 轨迹、全局地图、里程计、路径
  • 点云：历史点云、修正点云、约束
• 数据重置和内存分配

gpsHandler

• 数据放入 gpsQueue 队列中

loopHandler

• 控制闭环频率，只有上次闭环检测与上一次相差5s时才进行
• 执行闭环操作 performLoopClosure

performLoopClosure

• 得到激光关键帧Id loopFindKey，操作如下
  • 闭环帧当前时间 和 先前时间 loop_time_cur，loop_time_pre
  • 从后往前遍历 点云关键帧位姿队列，找到 第一个＞ loop_time_cur 的frame：key_cur
  • 从前往后遍历 点云关键帧位姿队列，找到 第一个＜ loop_time_pre 的frame：key_pre
• 若当前帧已经找到闭环时，则不在进行闭环检测
  • 因为：如果图像循环闭合频率很高，许多图像循环可能指向同一个key_cur
• 得到激光关键帧点云
  • 得到 key_cur 临近的点云 loopFindNearKeyframes 临近 0
  • 得到 key_pre 临近的点云，临近 25帧
  • 若 当前帧点云个数＜300，或者先前点云小于1000时，直接return
  • 发布历史帧先前点云，rviz中，方便调试
• 得到关键帧位姿 pose_cur、pose_pre、pose_diff_t
  • 从位姿队列中直接读取，差异只取 位置xyz
• 设置 ICP 匹配，并match
  • 设置匹配半径 historyKeyframeSearchRadius * 2=40
  • 最大迭代：100，ransac：0，Epsilo：1e-3
  • 初始化当前点云 cureKeyframeCloud_new，基于pose_diff_t 进行平移
  • 使用icp进行匹配 align
• 添加图约束
  • 若icp后得分 小于 阈值，则开始添加约束
    • 取icp 平移量，进行位姿约束添加，由上面平移了pose_diff_t需注意
    • 取 icp的噪声 getFitnessScore，作为噪声
• 可视化闭环约束，并发布

loopFindNearKeyframes

• 基于查询个数遍历，找到符合的角点和平面点
  • 遍历范围 [-searchNum,searchNum]
  • 角点和平面点基于位姿转换到 全局坐标系
  • 角点和平面点放到 同一容器中 nearKeyframes
• 降采样数据 downSizeFilterICP，数据赋值 nearKeyframes

laserCloudInfoHandler

• 取当前 数据的视觉戳 timeLaserInfoCur
• 取当前 数据的 角点特征和平面点特征
• 若与上次激光数据映射 超过阈值时，进行如下的操作
  • updateInitialGuess() 当前帧初始化
    • 如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分）
    • 后续帧，若vins_odom有效，则使用vins帧作用前一帧的激光位姿，得到当前帧激光位姿
    • 否则，用imu里程计计算两帧之间的增量位姿变换，得到当前帧激光位姿
  • extractSurroundingKeyFrames() 提取局部角点、平面点云集合，加入局部地图
    • 对最近的一帧关键帧，搜索时空纬度上相邻的关键帧集合，降采样一下
    • 对关键帧集合中的每一帧，提取对应的角点、平面点，加入局部地图中
    • 先nearby 再extractcloud 最后结束，这个就是对周围的关键帧做一个处理
  • downsampleCurrentScan() 当前激光帧角点、平面点集合降采样
  • scan2MapOptimization() 激光位姿图优化
  • saveKeyFramesAndFactor() 因子图优化，更新所有关键帧位姿
  • correctPoses() 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹
  • 发布显示：
    • publishOdometry()
    • publishFrames()

updateInitialGuess

• 若 cloudKeyPoses3D为空，即第一帧时，当前帧作为初始帧
  • 位置 Transform （0.0.0）
  • 角度 Rotation 与重力对齐 (imuInit)
  • return
• 若vins_odom有效时，得到当前帧的位姿：
  • ${}_{now}^{last}{T}_{vins}={{}_{last}^{Odom}{T}_{vins}}^{-1}{\ast }_{now}^{Odom}{T}_{vins}$
  • ${}_{now}^{Odom}{T}_{lidar}={}_{last}^{Odom}{T}_{lidar}{\ast }_{now}^{last}{T}_{vins}$
• 否则，若imu有效时，通过imu得到当前帧的位姿
  • ${}_{now}^{last}{T}_{imu}={{}_{last}^{Odom}{T}_{imu}}^{-1}{\ast }_{now}^{Odom}{T}_{imu}$
  • ${}_{now}^{last}{T}_{imu}={(}_{now}^{last}{R}_{imu},t(0,0,0))$
  • ${}_{now}^{Odom}{T}_{lidar}={}_{last}^{Odom}{T}_{lidar}{\ast }_{now}^{last}{T}_{imu}$
• 否则不更新了，我认为可以基于前后帧位姿预估一下比较好

extractSurroundingKeyFrames

• cloudKeyPoses3D中点云为空时，直接return
• 否则执行：extractNearby()

extractNearby()

• 找最新关键帧周围位姿，并降采样
  • 将 点云关键帧位姿数据放入 kdTree中，并找出最新关键帧位姿
    • kdTree：半径:50m
  • 将找出的位姿 进行降采样，降采样参数 [2m,2m,2m]
• 还提取一些最新的关键帧，以防机器人在一个位置旋转
  • 与当前帧时间相差10s的添加进来 surroundingKeyPosesDS
• 提取点云 extractCloud(surroundingKeyPosesDS)

extractCloud(surroundingKeyPosesDS)

• 遍历所有符合条件的帧，得到各帧世界坐标系下的角点和平面点
  • 取该帧的Id ，若该帧与当前帧距离超过阈值时跳过
  • 将当前帧特征点转换到世界坐标系下，并保存
• 融合地图，即 将上述符合条件的特征点想融合(点云直接相加)
• 将融合后的 角点和平面点 地图进行相应的降采样

scan2MapOptimization

• 要求当前帧特征点数量足够多，且匹配的点数够多，才执行优化
• 将上一步提取的地图放入kdTree中，方便查找最近点，迭代30次优化
  • 当前激光帧角点寻找局部map匹配点 cornerOptimization()
    • 更新当前帧位姿，(与哪个地图匹配需转到哪个地图)
    • 将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线，则认为匹配上了（用距离中心点的协方差矩阵，特征值进行判断）
    • 计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数
  • 当前激光帧平面点寻找局部map匹配点 surfOptimization()
    • 更新当前帧位姿，将当前帧平面点坐标交换到map系下，在局部map中查找5个最近点，距离小于1米，且5个点构成平面（最小二乘拟合平面），则认为匹配上了
    • 计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数
  • 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合 combineOptimizationCoeffs()
  • LM迭代优化 LMOptimization 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped
    • 基于特征值大小评判该方向是否可观，进而评判是否优化该方向
• 优化结果更新 transformUpdate ，用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll pirch 约束z坐标

saveKeyFramesAndFactor

该优化是独立于scan-map的另一个优化

• 评判当前帧是否 保存关键帧 saveFrame()，不保存return
  • 计算当前帧与前一帧关键帧位姿的变化，如果变化太小，不设为关键帧，反之设为关键帧
• 添加激光里程计因子 addodomFactor()
  • 若无关键帧时，协方差较大Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8
  • 有关键帧时，协方差较小，Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4
• 添加GPS因子 addGPSFactor()
  • 等待系统初始化并稳定下来，否则 return
    • 若无新的关键帧时跳过
    • 如果 第一关键帧与最新关键帧距离小于5m时直接跳过
  • 位姿协方差小于阈值时，跳过
  • 遍历 gpsQueue 队列，只添加当前帧 [-0.2,0.2]的数据
    • GPS 未初始化 或者噪声很大时跳过
    • 添加GPS 到因子图
• 添加 闭环因子 addLoopFactor()
  • 添加闭环队列中的所有数据到因子图中，并 清空队列数据
• 执行因子图优化，得到当前帧优化后位姿，位姿协方差
• 添加cloudKeyPoses3D,cloudKeyPoses6D,更新transformTobeMapped,添加当前关键帧的角点、平面点集合

loopClosureThread

• 若不进行闭环检测时直接return
• 循环执行，每50ms执行一次闭环检测
  • performLoopClosureDetection()

performLoopClosureDetection()

• 找最近的关键帧符合阈值(20m)的 关键帧数组
  • 将所有关键帧放入kdTree中，并用最近距离查找
  • 取当前帧的 3自由度位姿和 时间
• 找到 关键帧数组中 与当前帧时间之差 大于阈值的 关键帧
  • 遍历 关键帧数组，若与当前帧时间超出阈值，则 break
  • 最接近且时间符合阈值 historyKeyframeSearchTimeDiff: 30.0
• 执行闭环 performLoopClosure(match_msg);
  • match_msg 存放当前帧和闭环帧的时间戳
  • performLoopClosure 在 loopHandler中已经介绍了。

IMUPreintegration

FeatureExtraction

这两模块跟lio-sam一样