R3live笔记:从代码看lio线程
r3live的LIO部分主要继承r2live、fast-lio部分,ros中主要体现在前端r3live_LiDAR_front_end和后端r3live_mapping节点中,对应代码
- src/loam/LiDAR_front_end.cpp
- src/r3live.cpp
/************************************************************************************************************************************************/
r3live_LiDAR_front_end
特征提取总体概述:
原始点云输入->对其进行初步筛选
- 1.按线分,只取线内点(如avia是6线),其他点判定为异常点
- 2.x、y、z的值超1e8的点判定为异常点
- 3.x<0.7,nearing点,异常,x>2.0, 异常
- 4.tag,tag是livox雷达custom数据类型下的一个标签,是一个二进制,表征的是回波信息和造点信息
官网显示avia好像这个量没有实值,所以代码中这块意义不大 - 5.| 与上一点的距离 | > 1e7, 异常
筛选完之后得到的全是有效点,然后将其按线存储起来
接下来,按线提取特征
{
间隔3个点采样
判断点是不是在盲区,是则为异常点(只针对第一个点)
面特征直接得出,给到lio线程
}
1.获取参数, 定义相关变量,定义了几个全局变量角度值,下面会用到
jump_up_limit = cos( jump_up_limit / 180 * M_PI ); // cos(170度)jump_down_limit = cos( jump_down_limit / 180 * M_PI ); // cos(8度)cos160 = cos( cos160 / 180 * M_PI ); // cos(160度)smallp_intersect = cos( smallp_intersect / 180 * M_PI );// cos(172.5度)
2. 接收原始激光点云并处理, 提取角点特征和面特征,以MID为例:
订阅/livox/lidar 原始数据,在回调函数中转换数据, 并 提 取 角点特征和面特征
sub_points = n.subscribe( "/livox/lidar", 1000, mid_handler, ros::TransportHints().tcpNoDelay() );
mid_handler:
点云格式转换(ros下sensor_msgs::PointCloud2格式 -> pcl::PointCloud< PointType >)
pcl::PointCloud< PointType > pl; // 存放Lidar点的一维数组pcl::fromROSMsg( *msg, pl ); // 通过PointCloud2格式的msg初始化PointXYZINormal的pl// 其中PointType为pcl::PointXYZINormal
定义提取的角点和平面点变量
pcl::PointCloud< PointType > pl_corn, pl_surf; // 保存提取的角点和平面点uint plsize = pl.size() - 1; // Lidar点数量pl_corn.reserve( plsize ); // 预留sizepl_surf.reserve( plsize ); // 预留sizetypes.resize( plsize + 1 );
遍历msg中的前n-1个点,保存在types中
for ( uint i = 0; i < plsize; i++ ){types[ i ].range = pl[ i ].x; // vx = pl[ i ].x - pl[ i + 1 ].x; // 相邻两点做差vy = pl[ i ].y - pl[ i + 1 ].y; // 得到两点构成vz = pl[ i ].z - pl[ i + 1 ].z; // 的向量types[ i ].dista = vx * vx + vy * vy + vz * vz;// 向量模长(少开根号)}types[ plsize ].range = sqrt( pl[ plsize ].x * pl[ plsize ].x + pl[ plsize ].y * pl[ plsize ].y );
提取角点特征(pl_corn)和面特征(pl_surf)
give_feature( pl, types, pl_corn, pl_surf );
give_feature函数:
pl : Lidar帧的原始点容器
types : 对Lidar原始点计算了相邻两点距离后的点容器
pl_corn : 存放提取到的点特征
pl_surf : 存放提取到的面特征
(1)预先判断点数量和点的距离
uint plsize = pl.size();if ( plsize == 0 ) // 判断点数量{printf( "something wrong\n" );return;}uint head = 0;while ( types[ head ].range < blind ) // blind = 0.1 : default{ // types[i].range = pl[i].xhead++; //这里有点没看懂} plsize2 = ( plsize > group_size ) ? ( plsize - group_size ) : 0;//group_size = 8 点数大于8,则后续处理范围为 head ~ 点数-8,
(2)对head到plsize2进行计算
for ( uint i = head; i < plsize2; i += g_LiDAR_sampling_point_step ) {// g_LiDAR_sampling_point_step = 3这里以间隔3个点的形式赋值了面特征,证明了后面并没有进行特征提取PointType ap;// 充当中间交换量的临时容器if ( types[ i ].range > blind ){ap.x = pl[ i ].x;ap.y = pl[ i ].y;ap.z = pl[ i ].z;ap.curvature = pl[ i ].curvature;pl_surf.push_back( ap ); //pl_surf : 存放提取到的面特征if ( g_if_using_raw_point ) //g_if_using_raw_point=1{continue;}
本来还要执行plane_judge,然后根据plane_type进行状态更新 现在无了plane_type = plane_judge( pl, types, i, i_nex, curr_direct );if ( g_if_using_raw_point ) //1{return;}}}
所以只有pl_surf有值,还是间隔3个点的形式提取的点,pl_corn无值
3. 通过pub_full,pub_surf,pub_corn 将消息发布出去
定义发布话题
pub_full = n.advertise< sensor_msgs::PointCloud2 >( "/laser_cloud", 100 ); //发布转换后的消息pub_surf = n.advertise< sensor_msgs::PointCloud2 >( "/laser_cloud_flat", 100 ); //发布面特征消息pub_corn = n.advertise< sensor_msgs::PointCloud2 >( "/laser_cloud_sharp", 100 );//发布角点特征消息
将转换后的pl,提取到的pl_surf,pl_corn发布出去.
ros::Time ct( ros::Time::now() );pub_func( pl, pub_full, msg->header.stamp );pub_func( pl_surf, pub_surf, msg->header.stamp );pub_func( pl_corn, pub_corn, msg->header.stamp );
其中,pl_surf有值、pl_corn无值、pub_full为原始点云
/************************************************************************************************************************************************/
LIO线程主循环:
lio概述:
对输入的点云和camera帧首先考虑时间上对齐
通过measures计算lio状态并去畸变
构建立方体、体素管理地图点
体素栅格下采样滤波
下采样后的点->构造ikd树
(这里第一帧直接构建ikdtree,后面的点不直接加到树上而是找树上的最近5个点构建平面,然后计算点面残差,EKF更新后再添加到树上)
之后进行迭代kalman
{
找最近点,PCA拟合,点面loss
IKF更新,得到kalman增益后的状态结果=先验+kalman增益
收敛判断,协方差更新
} kalman结束
R3LIVE::service_LIO_update()
在r3live.cpp
main 函数中实例化 R3LIVE 对象,在构造函数开启LIO 子线程,具体参考R3live笔记:r3live的几个线程分析
其本质是 LOAM 面特征+fast-lio 的 ESIKF
m_thread_pool_ptr->commit_task(&R3LIVE::service_LIO_update, this);//开启lio线程
定义变量:
nav_msgs::Path path; // Lidar的路径 Eigen::Matrix< double, DIM_OF_STATES, DIM_OF_STATES > G, H_T_H, I_STATE;G : 用于传递协方差用的矩阵, H_T_H : 用于计算kalman增益用的, I_STATE : 单位阵PointCloudXYZINormal::Ptr feats_undistort( new PointCloudXYZINormal() );//去除畸变后的点云PointCloudXYZINormal::Ptr feats_down( new PointCloudXYZINormal() ); // 保存下采样后的点云PointCloudXYZINormal::Ptr coeffSel( new PointCloudXYZINormal() );// 存放M个最近平面信息的容器: 平面方程,点-面残差PointCloudXYZINormal::Ptr laserCloudOri( new PointCloudXYZINormal() );// 存放找到了最近平面的M个点的容器std::shared_ptr< ImuProcess > p_imu( new ImuProcess() ); // 定义用于前向/后向传播的IMU处理器
1.camera和lidar在频率上的时间对齐,判断当前时间是否可以处理lidar数据, 不可以则sleep一会.
while ( g_camera_lidar_queue.if_lidar_can_process() == false )
{ ros::spinOnce();std::this_thread::yield();std::this_thread::sleep_for( std::chrono::milliseconds( THREAD_SLEEP_TIM ) );
}
2.从lidar_buffer和imu_buffer_lio中分别获取雷达和imu数据,存放至Measures中
if ( sync_packages( Measures ) == 0 ){continue; // 提取数据失败}
3.通过测量数据Measures(IMU数据和Lidar数据)计算LIO的状态和去畸变后的Lidar点云数据
LIO状态结果保存在g_lio_state,去运动畸变后的点保存在feats_undistort中
p_imu->Process( Measures, g_lio_state, feats_undistort );
Process函数:
判断IMU数据、Lidar数据是否为空,然后初始化IMU数据
/** 1. initializing the gravity, gyro bias, acc and gyro covariance
* 初始化重力,陀螺仪bias,加速度和陀螺仪协方差
* 2. normalize the acceleration measurenments to unit gravity
* 归一化加速度测量值到单位重力下
**/
IMU_Initial( meas, stat, init_iter_num );
去畸变: 第一个点看作为基坐标,使用IMU旋转矩阵补偿Lidar点(仅使用旋转)
// ***IMU前向传播计算,Lidar点后向传播去畸变***if ( 1 ) {lic_point_cloud_undistort( meas, stat, *cur_pcl_un_ );}
// ***状态传播***lic_state_propagate( meas, stat );
如果没有成功去除点云运动畸变
if ( feats_undistort->empty() || ( feats_undistort == NULL ) ) // 没有成功去除点云运 动畸变{// 重置当前处理lidar帧的起始时间 : 比如当前处理t1-t2间的lidar但失败了// 因此后面处理时间为t1-t3,所以需要把t1保存进frame_first_pt_timeframe_first_pt_time = Measures.lidar_beg_time;std::cout << "not ready for odometry" << std::endl;continue;}
4.以lidar帧最后一个状态位置构建cube,cube结果放置在featsArray[484848]中,表明cube长宽高为48,一共有48^3个体素
lasermap_fov_segment();
5.根据给定去畸变后的点云构造三维体素栅格,对体素栅格进行下采样达到滤波的目的(一个体素用重心点来近似,减少点的数量,保持点云形状),结果保存在feats_down中
downSizeFilterSurf.setInputCloud( feats_undistort );//构建三维体素栅格 downSizeFilterSurf.filter( *feats_down ); //下采样滤波
6.使用下采样后得到的特征点云构造ikd树
(下采样后特征点数量大于1) && (ikd树根节点为空) : if ( ( feats_down->points.size() > 1 ) && ( ikdtree.Root_Node == nullptr ) ){// std::vector<PointType> points_init = feats_down->points;ikdtree.set_downsample_param( filter_size_map_min ); // filter_size_map_min默认=0.4ikdtree.Build( feats_down->points ); // 构造idk树flg_map_initialized = true;continue; // 进入下一次循环}if ( ikdtree.Root_Node == nullptr ) // 构造ikd树失败{flg_map_initialized = false;std::cout << "~~~~~~~ Initialize Map iKD-Tree Failed! ~~~~~~~" << std::endl;continue;}int featsFromMapNum = ikdtree.size(); // ikd树的节点数int feats_down_size = feats_down->points.size(); // 下采样过滤后的点数
7.ICP and iterated Kalman filter update : ICP迭代 + Kalman更新
PointCloudXYZINormal::Ptr coeffSel_tmpt( new PointCloudXYZINormal( *feats_down ) );PointCloudXYZINormal::Ptr feats_down_updated( new PointCloudXYZINormal( *feats_down ) );std::vector< double > res_last( feats_down_size, 1000.0 ); // initial : 存放每个特征点的残差值if ( featsFromMapNum >= 5 ) // ***重点*** : 正式开始ICP和迭代Kalman : ikd树上至少有5个点才进行操作{在ros上发布ikd特征点云数据,默认不发布遍历所有(下采样后)特征点,搜索每个点在树上的最近点集(5个点),由最近点集通过PCA方法拟合最近平面,再计算点-面残差,残差定义为点到平面的距离,对应FAST-LIO公式(12)从找到平面的点中筛选满足条件的点,用新的变量保存该点和对应平面(下标对齐)计算测量Jacobian矩阵和测量向量. Iterative Kalman Filter Update 收敛判断和协方差更新 更新维持的固定大小的map立方体将Kalman更新后的新lidar帧特征点先转世界坐标系,再加入ikd树ICP迭代+Kalman更新完成}
8.发布当前帧的点云数据
将laserCloudFullRes2的点转到世界坐标系下,再存入laserCloudFullResColor
*laserCloudFullRes2 = dense_map_en ? ( *feats_undistort ) : ( *feats_down );//去畸变or下采样点int laserCloudFullResNum = laserCloudFullRes2->points.size();// 发布点数量for ( int i = 0; i < laserCloudFullResNum; i++ ){ RGBpointBodyToWorld( &laserCloudFullRes2->points[ i ], &temp_point );laserCloudFullResColor->push_back( temp_point );}sensor_msgs::PointCloud2 laserCloudFullRes3;// 将laserCloudFullResColor转为发布形式pcl::toROSMsg( *laserCloudFullResColor, laserCloudFullRes3 );laserCloudFullRes3.header.stamp.fromSec( Measures.lidar_end_time );laserCloudFullRes3.header.frame_id = "world"; // world; camera_initpubLaserCloudFullRes.publish( laserCloudFullRes3 );
9.将点云添加至世界地图中
10.Publish Maps: 发布地图
11.Publish Odometry 发布里程计
12.Publish Path 发布路径
13.save debug variables 保存debug变量
R3live笔记:从代码看lio线程相关推荐
- 连傻瓜都能看懂的基于代码注入的线程守护技术
连傻瓜都能看懂的基于代码注入的线程守护技术 2010年08月31日 连傻瓜都能看懂的基于代码注入的线程守护技术 Author: 叶紫孤(CPP肥兔) (感谢冷风大哥提供技术支持) E-mail: ye ...
- C#笔记20:多线程之线程同步中的信号量AutoResetEvent和ManualResetEvent
C#笔记20:多线程之线程同步中的信号量AutoResetEvent和ManualResetEvent 本章概要: 1:终止状态和非终止状态 2:AutoResetEvent和ManualResetE ...
- 一个简单的例子看java线程机制
一个简单的例子看java线程机制 作者: zyf0808 发表日期: 2006-03-26 11:20 文章属性: 原创 复制链接 import java.util.*; public class T ...
- python structure_GitHub - CYZYZG/Data_Structure_with_Python: 这是我在学习《基于Python的数据结构》的时候的笔记与代码...
Data_Structure_with_Python 这是我在学习<基于Python的数据结构>的时候的笔记与代码 主要参考:数据结构与算法(Python) 对于算法的时间效率,我们可以用 ...
- Windows进程与线程学习笔记(五)—— 模拟线程切换
Windows进程与线程学习笔记(五)-- 模拟线程切换 ThreadSwitch代码分析 ThreadSwitch.cpp ThreadCore.h ThreadCore.cpp 总结 Thread ...
- 开源!《模式识别与机器学习(PRML)》笔记、代码、NoteBooks 发布
微软剑桥研究院实验室主任 Christopher Bishop 的经典著作<Pattern Recognition and Machine Learning>,中文译名<模式识别与机 ...
- 对于原生代码使用Java线程的优缺点
对于原生的代码使用Java线程与原生的线程相比有一下的优点: 它是很容易建立的. 在原生代码不需要任何改变 它不需要明确的附加到这虚拟机,Java线程已经是Java平台的一部分.原生代码和Java代码 ...
- (实验38)单片机,STM32F4学习笔记,代码讲解【SD卡实验】【正点原子】【原创】
文章目录 其它文章链接,独家吐血整理 实验现象 主程序 SD卡驱动程序 代码讲解 其它文章链接,独家吐血整理 (实验3)单片机,STM32F4学习笔记,代码讲解[按键输入实验][正点原子][原创] ( ...
- (实验39)单片机,STM32F4学习笔记,代码讲解【FATFS实验】【正点原子】【原创】
文章目录 其它文章链接,独家吐血整理 实验现象 主程序 FATFS初始化程序 代码讲解 其它文章链接,独家吐血整理 (实验3)单片机,STM32F4学习笔记,代码讲解[按键输入实验][正点原子][原创 ...
最新文章
- 从60多场技术面试中,我总结了这份面试经验
- linux shell mkpasswd 生成随机密码
- Java中如何生成jar(框架)
- json - 如何在 flutter 中的List String中加入2 json值?
- python求矩阵维度必须一致_python数据分析(二)--Numpy
- Android开发笔记(一百二十)两种侧滑布局
- 甲骨文超 IBM 成全球第二大软件公司
- 苹果4s怎么越狱_越狱源和插件大全2020.4.4
- html swf转mp4,swf怎么转换成mp4 swf视频转换成mp4格式没有声音|转换成MP4格式没有画面...
- 【整理】童鞋尺码大全--方便查找对照
- 全文检索语句中的AND和OR的用法
- 向量的加减(输出重载)
- 信息安全技术 关键信息基础设施安全保护要求
- 【观察】亚信科技:中期业绩逆势上扬,让5G新价值挺进纵深
- 【关于Citespace和JRE(JAVA运行环境)的详细安装教程】
- k8s各版本离线安装部署教程
- 谁是三鹿奶粉事件的真凶
- AV终结者病毒愈演愈烈
- MySQL 数据库的提速器-写缓存(Change Buffer)
- nuxtjs+express+vue2+vuex搭建的服务端渲染(SSR)个人网站项目 1
热门文章
- 固态硬盘、机械硬盘、手机的“内存”有三种
- 扇贝python骗局_北斗揭獐子岛扇贝骗局:27条采捕船数万航行数据还原轨迹
- thinkphp3.2乐观锁源码解读与优化
- 去中心化社交网络协议除了Nostr还有哪些?
- SuperPoint学习训练纪录 无训练版与带训练版本(一)
- ❤女朋友生日❤ HTML+css3+js 实现抖音炫酷樱花3D相册 (含背景音乐)程序员表白必备
- AI绘图实战(一):制作购物车图标icon | Stable Diffusion成为设计师生产力工具
- JavaScript实现在线Markdown编辑器、转换HTML工具-toolfk程序员工具网
- H3C路由器交换机配置sflow
- HUD1873看病要排队