Lvi-sam

lidar-visual-inertial odometry and mapping system

• 总体框架示意图：
  
• 各个节点 数据传输示意图
  

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

lio-sam

ImageProjection

Construct

• 订阅 Topic：
  • 订阅imu原始数据 imuHandler
  • 订阅由vins提供的ros 里程计， odometryHandler
  • 订阅雷达