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-dev
 sudo apt-get install -y libatlas-base-dev
 wget -O ~/Downloads/ceres.zip https://github.com/ceres-solver/ceres-solver/archive/1.14.0.zip
 cd ~/Downloads/ && unzip ceres.zip -d ~/Downloads/
 cd ~/Downloads/ceres-solver-1.14.0
 mkdir ceres-bin && cd ceres-bin
 cmake ..
 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 VV（归一化坐标）
计算ABC确定的法向量 N NN
计算原点到 平面的距离 (N(0) * A(0) + N(1) * A(1) + N(2) * A(2))
计算原点到 归一化特征点与法向量N NN确定的平面的距离 (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

​
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一样