正规网页设计培训怎么样,河南网站seo费用,做电商网站微信号是多少,鞍山贴吧目录
1 r3live_lio.cpp文件简介
2 r3live_lio.cpp源码解析 1 r3live_lio.cpp文件简介
在r3live.cpp文件中创建LIO线程后#xff0c;R3LIVE中的LIO线程本质上整体流程和FAST-LIO2基本一致。 2 r3live_lio.cpp源码解析
函数最开始会进行一系列的声明和定义#xff0c;发布的…目录
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和posepath.header.stamp ros::Time::now(); // header的时间path.header.frame_id /world; // header的id/*** variables definition ***/// DIM_OF_STATES29. 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_deg360HALF_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_timeframe_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_TIME0
{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::coutmean acc: mean_acc acc measures in word frame:state.rot_end.transpose()*mean_accstd::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 ], cov_gyr[ 1 ], cov_gyr[ 2 ] );}return;}// 去畸变: 第一个点看作为基坐标,使用IMU旋转矩阵补偿Lidar点(仅使用旋转)// if ( 0 || (stat.last_update_time 0.1))if ( 0 ){// UndistortPcl(meas, stat, *cur_pcl_un_);}else{// ***IMU前向传播计算,Lidar点后向传播去畸变***if ( 1 ) {lic_point_cloud_undistort( meas, stat, *cur_pcl_un_ );}else{*cur_pcl_un_ *meas.lidar;}// ***状态传播***lic_state_propagate( meas, stat );}// t2 omp_get_wtime();last_imu_ meas.imu.back();// t3 omp_get_wtime();// std::cout[ IMU Process ]: Time: t3 - t1std::endl;
}
然后就是完成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::vectorPointType 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(); // 下采样过滤后的点数
在拿到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 ); // 构成平面的点的indexstd::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_POINTS5ikdtree.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 stragetryif ( 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 ); // TODOfloat 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_POINTS5{// 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.0double 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*Tauto Hsub_T Hsub.transpose(); // H转置 : 6xn (nx6)^TH_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)^-1Eigen::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计算结果,得到k1次kalman的迭代更新值g_lio_state state_propagate solution; // kalman增益后的状态结果print_dash_board();// cout ANSI_COLOR_RED_BOLD Run EKF uph, vec vec.head9().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 * Hg_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 updatewait_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 );
