R3LIVE源码解析(9) — R3LIVE中r3live_lio.cpp文件
目录
1 r3live_lio.cpp文件简介
2 r3live_lio.cpp源码解析
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: "<然后就是完成localmap边界更新，便于完成降采样当前帧点云。
lasermap_fov_segment();
// 降采样
downSizeFilterSurf.setInputCloud( feats_undistort );
downSizeFilterSurf.filter( *feats_down );然后就是使用下采样后得到的特征点云构造ikd树。
ikdtree.set_downsample_param(filter_size_map_min)是设置IKD树的下采样参数。filter_size_map_min用于指定下采样的大小。ikdtree.Build(feats_down->points)是构建IKD树的操作。feats_down->points表示输入的特征点。
/**
* @note (2-6)initialize the map kdtree 使用下采样后得到的特征点云构造ikd树**
* @note 判断条件 : if(下采样后特征点数量大于1) && (ikd树根节点为空) :
* 重点：判断条件表明这里只进来一次,且第一帧lidar数据用来构造了ikd树后,就进入下一次循环了
* 所以可知,第一帧Lidar数据直接用来构造ikd树,第>=2帧Lidar数据不再直接添加到树上,而
* 是找与树上最近点,由最近点构成平面,然后计算点到平面的残差,并运用EKF迭代更新后,才加
* 到树上
*/
if ( ( feats_down->points.size() > 1 ) && ( ikdtree.Root_Node == nullptr ) )
{
// std::vector points_init = feats_down->points;
ikdtree.set_downsample_param( filter_size_map_min ); // filter_size_map_min默认=0.4
ikdtree.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(); // 下采样过滤后的点数 在拿到ikd-tree构建的树后就是使用激光信息进行ESIKF状态更新。
- 在ros上发布ikd特征点云数据，默认不发布
- 遍历所有(下采样后)特征点,搜索每个点在树上的最近点集(5个点), 由最近点集通过PCA方法拟合最近平面,再计算点-面残差，残差定义为点到平面的距离。
- 从找到平面的点中筛选满足条件的点,用新的变量保存该点和对应平面(下标对齐)
- 计算测量Jacobian矩阵和测量向量.
- Iterative Kalman Filter Update
- 收敛判断和协方差更新
- 更新维持的固定大小的map立方体
- 将Kalman更新后的新lidar帧特征点先转世界坐标系,再加入ikd树
- ICP迭代+Kalman更新完成
if ( featsFromMapNum >= 5 ) // ***重点*** : 正式开始ICP和迭代Kalman : ikd树上至少有5个点才进行操作
{
t1 = omp_get_wtime();
/**
* @note (2-7-1) : 在ros上发布特征点云数据 - 默认不发布
*/
if ( m_if_publish_feature_map )
{
PointVector().swap( ikdtree.PCL_Storage );
// flatten会将需要删除的点放入Points_deleted或Multithread_Points_deleted中
ikdtree.flatten( ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD );
featsFromMap->clear();
featsFromMap->points = ikdtree.PCL_Storage;
sensor_msgs::PointCloud2 laserCloudMap;
pcl::toROSMsg( *featsFromMap, laserCloudMap ); // 将点云数据格式转换为发布的消息格式
laserCloudMap.header.stamp = ros::Time::now(); // ros::Time().fromSec(last_timestamp_lidar);
// laserCloudMap.header.stamp.fromSec(Measures.lidar_end_time); // ros::Time().fromSec(last_timestamp_lidar);
laserCloudMap.header.frame_id = "world";
pubLaserCloudMap.publish( laserCloudMap );
}
/**
* @note (2-7-2) : 定义后面点-面计算时需要用到的变量
* 变量的理解举例: E为所有特征点,P点属于E,现在为P点找平面S:
* 当成功为P点找到平面S,则point_selected_surf[P] = true, 否则=false
* 平面S由m个点构成:pointSearchInd_surf[0]到pointSearchInd_surf[m]
* E中离P点按距离排序后的点放在Nearest_Points[]中:
* Nearest_Points是二维数组(PointVector是一维),存储每个点的最近点集合
*/
std::vector< bool > point_selected_surf( feats_down_size, true ); // 记录有那些点成功找到了平面
std::vector< std::vector< int > > pointSearchInd_surf( feats_down_size ); // 构成平面的点的index
std::vector< PointVector > Nearest_Points( feats_down_size ); // 二维数组,存点i的最近点排序后的集合
int rematch_num = 0;
bool rematch_en = 0;
flg_EKF_converged = 0;
deltaR = 0.0;
deltaT = 0.0;
t2 = omp_get_wtime();
double maximum_pt_range = 0.0;
// cout <<"Preprocess 2 cost time: " << tim.toc("Preprocess") << endl;
/**
* @note (2-7-3) 进行误差Kalman迭代
* //TODO
*/
for ( iterCount = 0; iterCount < NUM_MAX_ITERATIONS; iterCount++ ) // NUM_MAX_ITERATIONS默认为4
{
tim.tic( "Iter" ); // 本次迭代起始时间
match_start = omp_get_wtime();
laserCloudOri->clear(); // 清空存放找到了最近平面的点的容器
coeffSel->clear(); // 清空存放最近平面信息的容器
/**
* @note (2-7-3-1) : closest surface search and residual computation *
* 遍历所有(下采样后)特征点,搜索每个点在树上的最近点集(5个点),
* 由最近点集通过PCA方法拟合最近平面,再计算点-面残差
*/
for ( int i = 0; i < feats_down_size; i += m_lio_update_point_step ) // m_lio_update_point_step默认为1
{
double search_start = omp_get_wtime();
PointType &pointOri_tmpt = feats_down->points[ i ]; // 获取当前下标的特征点 - 原始点
// 计算特征点与原点的距离
double ori_pt_dis =
sqrt( pointOri_tmpt.x * pointOri_tmpt.x + pointOri_tmpt.y * pointOri_tmpt.y + pointOri_tmpt.z * pointOri_tmpt.z );
maximum_pt_range = std::max( ori_pt_dis, maximum_pt_range );// 保存离原点最远的点产生的最远距离
PointType &pointSel_tmpt = feats_down_updated->points[ i ]; // 获取当前下标的特征点 - 更新后的点
/* transform to world frame */
pointBodyToWorld( &pointOri_tmpt, &pointSel_tmpt );// 将特征点转到世界坐标下,并保存至可变点pointSel_tmpt中
std::vector< float > pointSearchSqDis_surf; // 搜索点-平面时产生的距离序列
auto &points_near = Nearest_Points[ i ]; // 下标i的特征点的最近点集合(一维数组)
/**
* @note (2-7-3-1-1) : 搜索与点最近平面上的5个点
* ***注意*** 只有第一次迭代时才进入,后面都用迭代的方法,就不找平面了
* (猜测应该是寻找平面耗时,后面直接用第一次找到的平面迭代优化就好了)
*/
if ( iterCount == 0 || rematch_en ) // 第一次迭代或者重匹配使能时才在ikd树上搜索最近平面
{
point_selected_surf[ i ] = true;
/** Find the closest surfaces in the map 在地图中找到最近的平面**/
// NUM_MATCH_POINTS=5
ikdtree.Nearest_Search( pointSel_tmpt, NUM_MATCH_POINTS, points_near, pointSearchSqDis_surf );
float max_distance = pointSearchSqDis_surf[ NUM_MATCH_POINTS - 1 ];//最近点集的最后一个元素自然最远
// max_distance to add residuals
// ANCHOR - Long range pt stragetry
if ( max_distance > m_maximum_pt_kdtree_dis ) // 超出限定距离,放弃为这个点寻找平面
{
point_selected_surf[ i ] = false; // 当前点寻找平面失败
}
}
kdtree_search_time += omp_get_wtime() - search_start;
if ( point_selected_surf[ i ] == false ) // 当前点寻找平面失败,进入下一个点的寻找流程
continue;
// match_time += omp_get_wtime() - match_start;
double pca_start = omp_get_wtime();
/**
* @note (2-7-3-2) : 使用PCA方法拟合平面 (using minimum square method)
* ***注意*** : 具体PCA解释待补充
*/
cv::Mat matA0( NUM_MATCH_POINTS, 3, CV_32F, cv::Scalar::all( 0 ) );
cv::Mat matB0( NUM_MATCH_POINTS, 1, CV_32F, cv::Scalar::all( -1 ) );
cv::Mat matX0( NUM_MATCH_POINTS, 1, CV_32F, cv::Scalar::all( 0 ) );
for ( int j = 0; j < NUM_MATCH_POINTS; j++ )
{
matA0.at< float >( j, 0 ) = points_near[ j ].x;
matA0.at< float >( j, 1 ) = points_near[ j ].y;
matA0.at< float >( j, 2 ) = points_near[ j ].z;
}
cv::solve( matA0, matB0, matX0, cv::DECOMP_QR ); // TODO
float pa = matX0.at< float >( 0, 0 );
float pb = matX0.at< float >( 1, 0 );
float pc = matX0.at< float >( 2, 0 );
float pd = 1;
float ps = sqrt( pa * pa + pb * pb + pc * pc );
pa /= ps;
pb /= ps;
pc /= ps;
pd /= ps;
/**
* @note (2-7-3-1-3) : 检测计算平面的有效性 - 将最近点集合带入平面方程中求残差做判断
*/
bool planeValid = true;
for ( int j = 0; j < NUM_MATCH_POINTS; j++ ) // NUM_MATCH_POINTS=5
{
// ANCHOR - Planar check : 将特征点在平面上的最近5个点带入平面公式中,得到的结果>0.05 (表明当前点不在拟合平面上)
if ( fabs( pa * points_near[ j ].x + pb * points_near[ j ].y + pc * points_near[ j ].z + pd ) >
m_planar_check_dis ) // Raw 0.05
{
// ANCHOR - Far distance pt processing
// ori_pt_dis:当前点到原点的距离, maximum_pt_range:特征点云中的最远点离原点的距离
// m_long_rang_pt_dis:lidar点最远有效距离(默认500)
if ( ori_pt_dis < maximum_pt_range * 0.90 || ( ori_pt_dis < m_long_rang_pt_dis ) )
// if(1)
{
planeValid = false;
point_selected_surf[ i ] = false; // 当前特征点寻找平面失败
break;
}
}
}
/**
* @note (2-7-3-1-4) : 前面计算平面有效时,计算残差res_last[] : 残差定义为点到平面的距离
* 对应FAST-LIO公式(12)
*/
if ( planeValid )
{
// 将特征点(世界坐标系下) 代入平面方程,计算残差 (在平面的点代入=0,不在平面的点代入会产生残差)
// 按照点到平面的距离公式,这里省略了除以分母的内容 : 对应FAST-LIO公式(12)
float pd2 = pa * pointSel_tmpt.x + pb * pointSel_tmpt.y + pc * pointSel_tmpt.z + pd;
// s计算了后面没用 : s考虑了除以分母的步骤.
float s = 1 - 0.9 * fabs( pd2 ) /
sqrt( sqrt( pointSel_tmpt.x * pointSel_tmpt.x + pointSel_tmpt.y * pointSel_tmpt.y +
pointSel_tmpt.z * pointSel_tmpt.z ) );
// ANCHOR - Point to plane distance
// 点到原点的距离<500 ? 0.3 : 1.0
double acc_distance = ( ori_pt_dis < m_long_rang_pt_dis ) ? m_maximum_res_dis : 1.0;
if ( pd2 < acc_distance ) // 残差小于0.3或者1.0
{
{// if(std::abs(pd2) > 5 * res_mean_last)
// {
// point_selected_surf[i] = false;
// res_last[i] = 0.0;
// continue;
// }
}
point_selected_surf[ i ] = true; // 当前点寻找平面成功
coeffSel_tmpt->points[ i ].x = pa; // 记录当前特征点对应最近平面的平面方程
coeffSel_tmpt->points[ i ].y = pb;
coeffSel_tmpt->points[ i ].z = pc;
coeffSel_tmpt->points[ i ].intensity = pd2;
res_last[ i ] = std::abs( pd2 ); // 当前特征点代入平面方程产生的残差
}
else
{
point_selected_surf[ i ] = false;// 当前点寻找平面失败
}
}
pca_time += omp_get_wtime() - pca_start;
} // 遍历所有特征点寻找平面,计算点-面残差完成
tim.tic( "Stack" );
double total_residual = 0.0;
laserCloudSelNum = 0;
/**
* @note (2-7-3-2) : 从找到平面的点中筛选满足条件的点,用新的变量保存该点和对应平面(下标对齐)
* 后面进行Kalman等计算时,都使用这里得到的"laserCloudOri"和"coeffSel"
*/
for ( int i = 0; i < coeffSel_tmpt->points.size(); i++ ) // 遍历找到了对应平面的特征点
{
// 平面有效 && 点到面的残差小于2 (这里的残差条件按理都满足,因为前面存入的残差值小于1.0)
if ( point_selected_surf[ i ] && ( res_last[ i ] <= 2.0 ) )
{ // 下面重新放入容器是为了对齐点-面的索引
laserCloudOri->push_back( feats_down->points[ i ] ); // 将找到最近平面的点放入laserCloudOri中
coeffSel->push_back( coeffSel_tmpt->points[ i ] ); // 将最近平面容器放入coeffSel中
total_residual += res_last[ i ]; // 总残差 - 从最小二乘的角度,优化的就是让这个总残差最小
laserCloudSelNum++; // 找到平面的点的数量
}
}
res_mean_last = total_residual / laserCloudSelNum; // 均值-期望, 后面没用此变量
match_time += omp_get_wtime() - match_start;
solve_start = omp_get_wtime();
/**
* @note (2-7-3-3) Computation of Measuremnt Jacobian matrix H and measurents vector
* 计算测量Jacobian矩阵和测量向量. 猜测对应FAST-LIO的公式(14)(15)(16)(17)
*/
Eigen::MatrixXd Hsub( laserCloudSelNum, 6 ); // Hsub(n x 6)
Eigen::VectorXd meas_vec( laserCloudSelNum ); // meas_vec(n x 1)
Hsub.setZero();
for ( int i = 0; i < laserCloudSelNum; i++ )
{
const PointType &laser_p = laserCloudOri->points[ i ];// 获取当前点
Eigen::Vector3d point_this( laser_p.x, laser_p.y, laser_p.z );// 点坐标
point_this += Lidar_offset_to_IMU; // Lidar和IMU的偏移
Eigen::Matrix3d point_crossmat;
point_crossmat << SKEW_SYM_MATRIX( point_this ); // 将点转为反对称矩阵用于叉乘
/*** get the normal vector of closest surface/corner 获取最近平面的法向量***/
const PointType &norm_p = coeffSel->points[ i ]; // 当前点的最近平面方程系数
Eigen::Vector3d norm_vec( norm_p.x, norm_p.y, norm_p.z );// 平面法向量
/*** calculate the Measuremnt Jacobian matrix H ***/
// A = 当前点的反对称矩阵 * (lidar帧最后时刻时的旋转)转置 * 最近平面法向量
// ?TODO : 这里可能又是一个近似,猜测是因为直接计算H矩阵太耗时
Eigen::Vector3d A( point_crossmat * g_lio_state.rot_end.transpose() * norm_vec );
Hsub.row( i ) << VEC_FROM_ARRAY( A ), norm_p.x, norm_p.y, norm_p.z;// row(i)=A[0],A[1],A[2],norm_p.x, norm_p.y, norm_p.z
/*** Measuremnt: distance to the closest surface/corner ***/
meas_vec( i ) = -norm_p.intensity;
}
Eigen::Vector3d rot_add, t_add, v_add, bg_add, ba_add, g_add; //更新量:旋转,平移,速度,偏置等
Eigen::Matrix< double, DIM_OF_STATES, 1 > solution; // 最终解 : 29维
Eigen::MatrixXd K( DIM_OF_STATES, laserCloudSelNum );// kalman增益
/**
* @note (2-7-3-4) : Iterative Kalman Filter Update
* 对应(有出入)FAST-LIO中公式(18)(19)(20)
*/
if ( !flg_EKF_inited ) // 未初始化时初始化 - 前面已经初始化了
{
cout << ANSI_COLOR_RED_BOLD << "Run EKF init" << ANSI_COLOR_RESET << endl;
/*** only run in initialization period ***/
set_initial_state_cov( g_lio_state );
}
else{
// cout << ANSI_COLOR_RED_BOLD << "Run EKF uph" << ANSI_COLOR_RESET << endl;
//1>:求公式中需要用到的 H 和 H^T*T
auto &&Hsub_T = Hsub.transpose(); // H转置 : 6xn = (nx6)^T
H_T_H.block< 6, 6 >( 0, 0 ) = Hsub_T * Hsub;//(0,0)处6x6块.H^T*T
//2>:求公式(20)中的Kalman增益的前面部分(省略R) : (H^T * R^-1 * H + P^-1)^-1
Eigen::Matrix< double, DIM_OF_STATES, DIM_OF_STATES > &&K_1 =
( H_T_H + ( g_lio_state.cov / LASER_POINT_COV ).inverse() ).inverse();
//3>:结合2>求的前面部分,求公式(20)Kalman增益(省略R)
K = K_1.block< DIM_OF_STATES, 6 >( 0, 0 ) * Hsub_T;// K = (29x6) * (6xn) = (29xn)
//4>:求公式(18)中的最右边部分 : x^kk-x^k : Kalman迭代时的传播状态-预估(也是更新)状态
auto vec = state_propagate - g_lio_state;//state_propagate初始=g_lio_state
//5>:求公式(18)的中间和右边部分(有出入:I什么的都省略了)
solution = K * ( meas_vec - Hsub * vec.block< 6, 1 >( 0, 0 ) ); // kalman增益
// double speed_delta = solution.block( 0, 6, 3, 1 ).norm();
// if(solution.block( 0, 6, 3, 1 ).norm() > 0.05 )
// {
// solution.block( 0, 6, 3, 1 ) = solution.block( 0, 6, 3, 1 ) / speed_delta * 0.05;
// }
//6>:结合5>中结果,求公式18计算结果,得到k+1次kalman的迭代更新值
g_lio_state = state_propagate + solution; // kalman增益后的状态结果
print_dash_board();
// cout << ANSI_COLOR_RED_BOLD << "Run EKF uph, vec = " << vec.head<9>().transpose() << ANSI_COLOR_RESET << endl;
rot_add = solution.block< 3, 1 >( 0, 0 ); // 旋转增量
t_add = solution.block< 3, 1 >( 3, 0 ); // 平移增量
flg_EKF_converged = false; // 收敛标识
//7>:判断是否收敛
if ( ( ( rot_add.norm() * 57.3 - deltaR ) < 0.01 ) && ( ( t_add.norm() * 100 - deltaT ) < 0.015 ) )
{
flg_EKF_converged = true; // 通过旋转和平移增量与上一次迭代的差值,判断是否收敛
// ? : 收敛了为啥不加break,而是继续进行迭代
}
//8>:旋转和平移增量转换单位
deltaR = rot_add.norm() * 57.3; // 角度单位
deltaT = t_add.norm() * 100; // 厘米单位
}
// printf_line;
g_lio_state.last_update_time = Measures.lidar_end_time;
euler_cur = RotMtoEuler( g_lio_state.rot_end ); // 获得当前lidar的里程计信息,最后这个需要发布到ros中去
dump_lio_state_to_log( m_lio_state_fp );
/*** Rematch Judgement 重匹配判断 ***/
rematch_en = false;
if ( flg_EKF_converged || ( ( rematch_num == 0 ) && ( iterCount == ( NUM_MAX_ITERATIONS - 2 ) ) ) )
{
rematch_en = true;
rematch_num++;
}
/**
* @note (2-7-3-5) Convergence Judgements and Covariance Update
* 收敛判断和协方差更新 : 对应FAST-LIO公式(19)
*/
// if (rematch_num >= 10 || (iterCount == NUM_MAX_ITERATIONS - 1))
if ( rematch_num >= 2 || ( iterCount == NUM_MAX_ITERATIONS - 1 ) ) // Fast lio ori version.
{
if ( flg_EKF_inited )
{
/*** Covariance Update ***/
G.block< DIM_OF_STATES, 6 >( 0, 0 ) = K * Hsub; // 对应公式(19)中 : K * H
g_lio_state.cov = ( I_STATE - G ) * g_lio_state.cov;//公式(19): (单位阵-K*H)*Cur_协方差
total_distance += ( g_lio_state.pos_end - position_last ).norm();// 两次state间的距离
position_last = g_lio_state.pos_end;
// std::cout << "position: " << g_lio_state.pos_end.transpose() << " total distance: " << total_distance << std::endl;
}
solve_time += omp_get_wtime() - solve_start;
break;
}
solve_time += omp_get_wtime() - solve_start;
// cout << "Match cost time: " << match_time * 1000.0
// << ", search cost time: " << kdtree_search_time*1000.0
// << ", PCA cost time: " << pca_time*1000.0
// << ", solver_cost: " << solve_time * 1000.0 << endl;
// cout <<"Iter cost time: " << tim.toc("Iter") << endl;
} // (2-7-3) : Kalman滤波迭代完成
t3 = omp_get_wtime();
/**
* @note (2-7-4):add new frame points to map ikdtree
* 1> : 更新维持的固定大小的map立方体 (参考FAST-LIO2:V.A地图管理)
* 2> : 将Kalman更新后的新lidar帧特征点先转世界坐标系,再加入ikd树
*/
PointVector points_history; // 将ikd树中需要移除的点放入points_history中
ikdtree.acquire_removed_points( points_history );// 从Points_deleted和Multithread_Points_deleted获取点
memset( cube_updated, 0, sizeof( cube_updated ) );
//1> : 更新维持的固定大小的map立方体 (参考FAST-LIO2:V.A地图管理)
for ( int i = 0; i < points_history.size(); i++ )
{
PointType &pointSel = points_history[ i ];
int cubeI = int( ( pointSel.x + 0.5 * cube_len ) / cube_len ) + laserCloudCenWidth;
int cubeJ = int( ( pointSel.y + 0.5 * cube_len ) / cube_len ) + laserCloudCenHeight;
int cubeK = int( ( pointSel.z + 0.5 * cube_len ) / cube_len ) + laserCloudCenDepth;
if ( pointSel.x + 0.5 * cube_len < 0 )
cubeI--;
if ( pointSel.y + 0.5 * cube_len < 0 )
cubeJ--;
if ( pointSel.z + 0.5 * cube_len < 0 )
cubeK--;
if ( cubeI >= 0 && cubeI < laserCloudWidth && cubeJ >= 0 && cubeJ < laserCloudHeight && cubeK >= 0 && cubeK < laserCloudDepth )
{
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
featsArray[ cubeInd ]->push_back( pointSel );
}
}
//2-1> : 将Kalman更新后的新lidar帧特征点先转世界坐标系
for ( int i = 0; i < feats_down_size; i++ )
{
/* transform to world frame */
pointBodyToWorld( &( feats_down->points[ i ] ), &( feats_down_updated->points[ i ] ) );
}
t4 = omp_get_wtime();
//2-2> : 将特征点加入世界坐标中
ikdtree.Add_Points( feats_down_updated->points, true ); // 存入ikd树中
kdtree_incremental_time = omp_get_wtime() - t4 + readd_time + readd_box_time + delete_box_time;
t5 = omp_get_wtime();
} // (2-7) ICP迭代+Kalman更新完成最后就是向rgb点云地图中加入新点，并发布laserCloudFullResColor、Maps、Odometry、Path等信息。
/**
* @note (2-9) : append point cloud to global map.
* 将点云添加至世界地图中
*/
if ( 1 )
{
static std::vector< double > stastic_cost_time;
Common_tools::Timer tim;
// tim.tic();
// ANCHOR - RGB maps update
wait_render_thread_finish();
if ( m_if_record_mvs )
{
std::vector< std::shared_ptr< RGB_pts > > pts_last_hitted;
pts_last_hitted.reserve( 1e6 );
m_number_of_new_visited_voxel = m_map_rgb_pts.append_points_to_global_map(
*laserCloudFullResColor, Measures.lidar_end_time - g_camera_lidar_queue.m_first_imu_time, &pts_last_hitted,
m_append_global_map_point_step );
m_map_rgb_pts.m_mutex_pts_last_visited->lock();
m_map_rgb_pts.m_pts_last_hitted = pts_last_hitted;
m_map_rgb_pts.m_mutex_pts_last_visited->unlock();
}
else
{
m_number_of_new_visited_voxel = m_map_rgb_pts.append_points_to_global_map(
*laserCloudFullResColor, Measures.lidar_end_time - g_camera_lidar_queue.m_first_imu_time, nullptr,
m_append_global_map_point_step );
}
stastic_cost_time.push_back( tim.toc( " ", 0 ) );
}
/**
* @note (2-10) : 仅发布找到了最近平面的有效点
*/
if(0) // Uncomment this code scope to enable the publish of effective points.
{
/******* Publish effective points *******/
laserCloudFullResColor->clear();
pcl::PointXYZI temp_point;
for ( int i = 0; i < laserCloudSelNum; i++ )
{
RGBpointBodyToWorld( &laserCloudOri->points[ i ], &temp_point );
laserCloudFullResColor->push_back( temp_point );
}
sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg( *laserCloudFullResColor, laserCloudFullRes3 );
// laserCloudFullRes3.header.stamp = ros::Time::now(); //.fromSec(last_timestamp_lidar);
laserCloudFullRes3.header.stamp.fromSec( Measures.lidar_end_time ); //.fromSec(last_timestamp_lidar);
laserCloudFullRes3.header.frame_id = "world";
pubLaserCloudEffect.publish( laserCloudFullRes3 );
}
/**
* @note (2-11)***** Publish Maps: 发布地图******
*/
sensor_msgs::PointCloud2 laserCloudMap;
pcl::toROSMsg( *featsFromMap, laserCloudMap );
laserCloudMap.header.stamp.fromSec( Measures.lidar_end_time ); // ros::Time().fromSec(last_timestamp_lidar);
laserCloudMap.header.frame_id = "world";
pubLaserCloudMap.publish( laserCloudMap );
/**
* @note (2-12)***** Publish Odometry 发布里程计*****
*/
geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw( euler_cur( 0 ), euler_cur( 1 ), euler_cur( 2 ) );
odomAftMapped.header.frame_id = "world";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = ros::Time::now(); // ros::Time().fromSec(last_timestamp_lidar);
odomAftMapped.pose.pose.orientation.x = geoQuat.x;
odomAftMapped.pose.pose.orientation.y = geoQuat.y;
odomAftMapped.pose.pose.orientation.z = geoQuat.z;
odomAftMapped.pose.pose.orientation.w = geoQuat.w;
odomAftMapped.pose.pose.position.x = g_lio_state.pos_end( 0 );
odomAftMapped.pose.pose.position.y = g_lio_state.pos_end( 1 );
odomAftMapped.pose.pose.position.z = g_lio_state.pos_end( 2 );
pubOdomAftMapped.publish( odomAftMapped );
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::Quaternion q;
transform.setOrigin(
tf::Vector3( odomAftMapped.pose.pose.position.x, odomAftMapped.pose.pose.position.y, odomAftMapped.pose.pose.position.z ) );
q.setW( odomAftMapped.pose.pose.orientation.w );
q.setX( odomAftMapped.pose.pose.orientation.x );
q.setY( odomAftMapped.pose.pose.orientation.y );
q.setZ( odomAftMapped.pose.pose.orientation.z );
transform.setRotation( q );
br.sendTransform( tf::StampedTransform( transform, ros::Time().fromSec( Measures.lidar_end_time ), "world", "/aft_mapped" ) );
msg_body_pose.header.stamp = ros::Time::now();
msg_body_pose.header.frame_id = "/camera_odom_frame";
msg_body_pose.pose.position.x = g_lio_state.pos_end( 0 );
msg_body_pose.pose.position.y = g_lio_state.pos_end( 1 );
msg_body_pose.pose.position.z = g_lio_state.pos_end( 2 );
msg_body_pose.pose.orientation.x = geoQuat.x;
msg_body_pose.pose.orientation.y = geoQuat.y;
msg_body_pose.pose.orientation.z = geoQuat.z;
msg_body_pose.pose.orientation.w = geoQuat.w;
/**
* @note (2-13)***** Publish Path 发布路径*******
*/
msg_body_pose.header.frame_id = "world";
if ( frame_num > 10 )
{
path.poses.push_back( msg_body_pose );
}
pubPath.publish( path );
/**
* @note (2-14)* save debug variables 保存debug变量**
*/
frame_num++;
aver_time_consu = aver_time_consu * ( frame_num - 1 ) / frame_num + ( t5 - t0 ) / frame_num;
// aver_time_consu = aver_time_consu * 0.8 + (t5 - t0) * 0.2;
T1[ time_log_counter ] = Measures.lidar_beg_time;
s_plot[ time_log_counter ] = aver_time_consu;
s_plot2[ time_log_counter ] = kdtree_incremental_time;
s_plot3[ time_log_counter ] = kdtree_search_time;
s_plot4[ time_log_counter ] = fov_check_time;
s_plot5[ time_log_counter ] = t5 - t0;
s_plot6[ time_log_counter ] = readd_box_time;
time_log_counter++;
fprintf( m_lio_costtime_fp, "%.5f %.5f\r\n", g_lio_state.last_update_time - g_camera_lidar_queue.m_first_imu_time, t5 - t0 );
fflush( m_lio_costtime_fp );