目录
1 r3live_lio.cpp文件简介
在r3live.cpp文件中创建LIO线程后，R3LIVE中的LIO线程本质上整体流程和FAST-LIO2基本一致。
2 r3live_lio.cpp源码解析
函数最开始会进行一系列的声明和定义，发布的lidar路径,协方差矩阵,kalman增益矩阵和4个重要的点云容器,IMU前向/后向传播处理器等
/**
* ===================================================================================================
* @note (1) 定义用到的变量 : 发布的lidar路径,协方差矩阵,kalman增益矩阵和4个重要的点云容器,IMU前向/后向传播处理器等.
* ===================================================================================================
*/
nav_msgs::Path path; // Lidar的路径 : 主要有两个成员变量: header和pose
path.header.stamp = ros::Time::now(); // header的时间
path.header.frame_id = "/world"; // header的id
/*** variables definition ***/
// DIM_OF_STATES=29. G:, H_T_H, I_STATE:
// G : 用于传递协方差用的矩阵, H_T_H : 用于计算kalman增益用的, I_STATE : 单位阵
Eigen::Matrix< double, DIM_OF_STATES, DIM_OF_STATES > G, H_T_H, I_STATE;
G.setZero();
H_T_H.setZero();
I_STATE.setIdentity();
cv::Mat matA1( 3, 3, CV_32F, cv::Scalar::all( 0 ) ); // 后面没用到
cv::Mat matD1( 1, 3, CV_32F, cv::Scalar::all( 0 ) ); // 后面没用到
cv::Mat matV1( 3, 3, CV_32F, cv::Scalar::all( 0 ) ); // 后面没用到
cv::Mat matP( 6, 6, CV_32F, cv::Scalar::all( 0 ) ); // 后面没用到
PointCloudXYZINormal::Ptr feats_undistort( new PointCloudXYZINormal() );//去除畸变后的点云
PointCloudXYZINormal::Ptr feats_down( new PointCloudXYZINormal() ); // 保存下采样后的点云
// 有M个特征点找到了M个对应的最近平面Si,则coeffSel存储Si的平面方程系数,和点到平面的残差
PointCloudXYZINormal::Ptr coeffSel( new PointCloudXYZINormal() );// 存放M个最近平面信息的容器: 平面方程,点-面残差
PointCloudXYZINormal::Ptr laserCloudOri( new PointCloudXYZINormal() );// 存放找到了最近平面的M个点的容器
/*** variables initialize ***/
FOV_DEG = fov_deg + 10; // fov_deg=360
HALF_FOV_COS = std::cos( ( fov_deg + 10.0 ) * 0.5 * PI_M / 180.0 );// cos(185)
for ( int i = 0; i < laserCloudNum; i++ ) // laserCloudNum = 48x48x48
{
// featsArray[48x48x48]
featsArray[ i ].reset( new PointCloudXYZINormal() );
}
std::shared_ptr< ImuProcess > p_imu( new ImuProcess() ); // 定义用于前向/后向传播的IMU处理器
m_imu_process = p_imu;
//------------------------------------------------------------------------------------------------------
ros::Rate rate( 5000 );
bool status = ros::ok();
g_camera_lidar_queue.m_liar_frame_buf = &lidar_buffer;//取出lidar数据
set_initial_state_cov( g_lio_state ); //初始化g_lio_state的状态协方差矩阵然后开始LIO线程的主循环。首先会对比相机和lidar队列头的时间戳，如果camera的时间戳更早则等待vio线程把更早的图像处理完。
- td::this_thread::yield()函数的作用是让出当前线程的执行权，让其他线程有机会执行。它可以用来提高多线程程序的效率，避免某个线程长时间占用CPU资源。
- std::this_thread::sleep_for()函数的作用是让当前线程暂停执行一段时间。它接受一个时间间隔作为参数，参数类型为std::chrono::milliseconds，表示以毫秒为单位的时间间隔。在代码中，THREAD_SLEEP_TIM是一个时间间隔的变量，它的值可以根据需要进行设置，代码中设置为1s。
while ( g_camera_lidar_queue.if_lidar_can_process() == false )
{ // 考虑camera和lidar在频率上的时间对齐关系
// 判断当前时间是否可以处理lidar数据, 不可以则sleep一会
ros::spinOnce();
std::this_thread::yield();
std::this_thread::sleep_for( std::chrono::milliseconds( THREAD_SLEEP_TIM ) );
}从lidar_buffer和imu_buffer_lio中分别获取雷达和imu数据，存放至Measures中，如果没有读取到数据则退出。
/**
* @note (2-2) 从lidar_buffer和imu_buffer_lio中分别获取雷达和imu数据,存放至Measures中
* MeasureGroup Measures; // 存放雷达数据和IMU数据的结构体全局变量
*/
if ( sync_packages( Measures ) == 0 )
{
continue; // 提取数据失败
}然后下一步就是调用Process 函数补偿点云畸变，并用imu数据进行系统状态预测。
- 如果没有成功去除点云运动畸变，则重置当前处理lidar帧的起始时间。
- 接着进行时间判断， 当前帧中lidar的开始时间,必须<=记录的帧开始时间，如果时间满足关系，开始EKF过程
/**
* @note (2-3) 通过测量数据Measures(IMU数据和Lidar数据)计算LIO的状态和去畸变后的Lidar点云数据
* 前向传播 + 后向传播
* LIO状态结果保存在g_lio_state,去运动畸变后的点保存在feats_undistort中
*/
p_imu->Process( Measures, g_lio_state, feats_undistort );
g_camera_lidar_queue.g_noise_cov_acc = p_imu->cov_acc; // 获取加速度误差状态传递的协方差
g_camera_lidar_queue.g_noise_cov_gyro = p_imu->cov_gyr; // 获取角速度误差状态传递的协方差
StatesGroup state_propagate( g_lio_state ); // 状态传播值(先验):通过计算得到的状态实例化一个StatesGroup变量
// 输出lio上一帧更新的时间 : 上一帧更新记录时间 - lidar开始时间
// cout << "G_lio_state.last_update_time = " << std::setprecision(10) << g_lio_state.last_update_time -g_lidar_star_tim << endl;
if ( feats_undistort->empty() || ( feats_undistort == NULL ) ) // 没有成功去除点云运动畸变
{
// 重置当前处理lidar帧的起始时间 : 比如当前处理t1-t2间的lidar但失败了
// 因此后面处理时间为t1-t3,所以需要把t1保存进frame_first_pt_time
frame_first_pt_time = Measures.lidar_beg_time;
std::cout << "not ready for odometry" << std::endl;
continue;
}
// 当前帧中lidar的开始时间,必须<=记录的帧开始时间
// 按理应该相等,这里的判断估计是实际使用中发生的问题(从理解的角度-不一定正确:如果lidar中损失了初始点,那么
// 当前最开始的lidar点的时间就<记录下来的帧开始时间)
if ( ( Measures.lidar_beg_time - frame_first_pt_time ) < INIT_TIME ) // INIT_TIME=0
{
flg_EKF_inited = false;
std::cout << "||||||||||Initiallizing LiDAR||||||||||" << std::endl;
}
else // 时间满足关系,开始EKF过程
{
flg_EKF_inited = true;
}p_imu->Process() 函数的主要作用是通过测量数据meas(IMU数据和Lidar数据)计算LIO的状态和去畸变后的Lidar点云数据。
/**
* @note 通过测量数据meas(IMU数据和Lidar数据)计算LIO的状态和去畸变后的Lidar点云数据
* @param meas : 输入的Lidar和IMU数据
* @param stat : Lidar状态
* @param cur_pcl_un_ : 去畸变后的点云
*/
void ImuProcess::Process( const MeasureGroup &meas, StatesGroup &stat, PointCloudXYZINormal::Ptr cur_pcl_un_ )
{
// double t1, t2, t3;
// t1 = omp_get_wtime();
if ( meas.imu.empty() ) // 判断IMU数据
{
// std::cout << "no imu data" << std::endl;
return;
};
ROS_ASSERT( meas.lidar != nullptr ); // 判断Lidar数据
if ( imu_need_init_ ) // 初始化IMU数据
{
/// The very first lidar frame 初始化第一帧雷达数据帧
// 初始化重力,陀螺仪bias,加速度和陀螺仪协方差
// 归一化加速度测量值到单位重力下
IMU_Initial( meas, stat, init_iter_num );
imu_need_init_ = true;
last_imu_ = meas.imu.back(); // 获取当前处理的最后一个imu数据
if ( init_iter_num > MAX_INI_COUNT ) // (1 > 20) : 默认不进入if
{
imu_need_init_ = false;
// std::cout<<"mean acc: "<<mean_acc<<" acc measures in word frame:"<<state.rot_end.transpose()*mean_acc<<std::endl;
ROS_INFO(
"IMU Initials: Gravity: %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",
stat.gravity[ 0 ], stat.gravity[ 1 ], stat.gravity[ 2 ], stat.bias_g[ 0 ], stat.bias_g[ 1 ], stat.bias_g[ 2 ], cov_acc[ 0 ],
cov_acc[ 1 ], cov_acc[ 2 ], cov_gyr[ 
最低0.47元/天 解锁文章
298
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
