0. 简介
在讲述完一，二后IMU和雷达的数据输入算是讲明白了，下面几节我们将结合主程序来介绍本文最关键的迭代卡尔曼滤波器和iKD-Tree的算法。
1. 主程序
一开始在laserMapping.cpp主程序中定义了一系列待使用的变量定义，包含了ESKF和IKF-Tree的定义
#define INIT_TIME (0.1)
#define LASER_POINT_COV (0.001)
#define MAXN (720000)
#define PUBFRAME_PERIOD (20)
/*** Time Log Variables ***/
// kdtree_incremental_time为kdtree建立时间，kdtree_search_time为kdtree搜索时间，kdtree_delete_time为kdtree删除时间;
double kdtree_incremental_time = 0.0, kdtree_search_time = 0.0, kdtree_delete_time = 0.0;
// T1为雷达初始时间戳，s_plot为整个流程耗时，s_plot2特征点数量,s_plot3为kdtree增量时间，s_plot4为kdtree搜索耗时，s_plot5为kdtree删除点数量
//，s_plot6为kdtree删除耗时，s_plot7为kdtree初始大小，s_plot8为kdtree结束大小,s_plot9为平均消耗时间，s_plot10为添加点数量，s_plot11为点云预处理的总时间
double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot4[MAXN], s_plot5[MAXN], s_plot6[MAXN], s_plot7[MAXN], s_plot8[MAXN], s_plot9[MAXN], s_plot10[MAXN], s_plot11[MAXN];
// 定义全局变量，用于记录时间,match_time为匹配时间，solve_time为求解时间，solve_const_H_time为求解H矩阵时间
double match_time = 0, solve_time = 0, solve_const_H_time = 0;
// kdtree_size_st为ikd-tree获得的节点数，kdtree_size_end为ikd-tree结束时的节点数，add_point_size为添加点的数量，kdtree_delete_counter为删除点的数量
int kdtree_size_st = 0, kdtree_size_end = 0, add_point_size = 0, kdtree_delete_counter = 0;
// runtime_pos_log运行时的log是否开启，pcd_save_en是否保存pcd文件，time_sync_en是否同步时间
bool runtime_pos_log = false, pcd_save_en = false, time_sync_en = false;
/**************************/
float res_last[100000] = {0.0}; //残差，点到面距离平方和
float DET_RANGE = 300.0f; //设置的当前雷达系中心到各个地图边缘的距离
const float MOV_THRESHOLD = 1.5f; //设置的当前雷达系中心到各个地图边缘的权重
mutex mtx_buffer; // 互斥锁
condition_variable sig_buffer; // 条件变量
string root_dir = ROOT_DIR; //设置根目录
string map_file_path, lid_topic, imu_topic; //设置地图文件路径，雷达topic，imu topic
double res_mean_last = 0.05, total_residual = 0.0; //设置残差平均值，残差总和
double last_timestamp_lidar = 0, last_timestamp_imu = -1.0; //设置雷达时间戳，imu时间戳
double gyr_cov = 0.1, acc_cov = 0.1, b_gyr_cov = 0.0001, b_acc_cov = 0.0001; //设置imu的角速度协方差，加速度协方差，角速度协方差偏置，加速度协方差偏置
double filter_size_corner_min = 0, filter_size_surf_min = 0, filter_size_map_min = 0, fov_deg = 0; //设置滤波器的最小尺寸，地图的最小尺寸，视野角度
//设置立方体长度，视野一半的角度，视野总角度，总距离，雷达结束时间，雷达初始时间
double cube_len = 0, HALF_FOV_COS = 0, FOV_DEG = 0, total_distance = 0, lidar_end_time = 0, first_lidar_time = 0.0;
//设置有效特征点数，时间log计数器, scan_count：接收到的激光雷达Msg的总数，publish_count：接收到的IMU的Msg的总数
int effct_feat_num = 0, time_log_counter = 0, scan_count = 0, publish_count = 0;
//设置迭代次数，下采样的点数，最大迭代次数，有效点数
int iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudValidNum = 0;
bool point_selected_surf[100000] = {0}; // 是否为平面特征点
// lidar_pushed：用于判断激光雷达数据是否从缓存队列中拿到meas中的数据, flg_EKF_inited用于判断EKF是否初始化完成
bool lidar_pushed, flg_reset, flg_exit = false, flg_EKF_inited;
//设置是否发布激光雷达数据，是否发布稠密数据，是否发布激光雷达数据的身体数据
bool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false;
vector<vector<int>> pointSearchInd_surf; //每个点的索引,暂时没用到
vector<BoxPointType> cub_needrm; // ikd-tree中，地图需要移除的包围盒序列
vector<PointVector> Nearest_Points; //每个点的最近点序列
vector<double> extrinT(3, 0.0); //雷达相对于IMU的外参T
vector<double> extrinR(9, 0.0); //雷达相对于IMU的外参R
deque<double> time_buffer; // 激光雷达数据时间戳缓存队列
deque<PointCloudXYZI::Ptr> lidar_buffer; //记录特征提取或间隔采样后的lidar（特征）数据
deque<sensor_msgs::Imu::ConstPtr> imu_buffer; // IMU数据缓存队列
//一些点云变量
PointCloudXYZI::Ptr featsFromMap(new PointCloudXYZI()); //提取地图中的特征点，IKD-tree获得
PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI()); //去畸变的特征
PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI()); //畸变纠正后降采样的单帧点云，lidar系
PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI()); //畸变纠正后降采样的单帧点云，w系
PointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1)); //特征点在地图中对应点的，局部平面参数,w系
PointCloudXYZI::Ptr laserCloudOri(new PointCloudXYZI(100000, 1)); // laserCloudOri是畸变纠正后降采样的单帧点云，body系
PointCloudXYZI::Ptr corr_normvect(new PointCloudXYZI(100000, 1)); //对应点法相量
PointCloudXYZI::Ptr _featsArray; // ikd-tree中，map需要移除的点云序列
//下采样的体素点云
pcl::VoxelGrid<PointType> downSizeFilterSurf; //单帧内降采样使用voxel grid
pcl::VoxelGrid<PointType> downSizeFilterMap; //未使用
KD_TREE ikdtree; // ikd-tree类
V3F XAxisPoint_body(LIDAR_SP_LEN, 0.0, 0.0); //雷达相对于body系的X轴方向的点
V3F XAxisPoint_world(LIDAR_SP_LEN, 0.0, 0.0); //雷达相对于world系的X轴方向的点
V3D euler_cur; //当前的欧拉角
V3D position_last(Zero3d); //上一帧的位置
V3D Lidar_T_wrt_IMU(Zero3d); // T lidar to imu (imu = r * lidar + t)
M3D Lidar_R_wrt_IMU(Eye3d); // R lidar to imu (imu = r * lidar + t)
/*** EKF inputs and output ***/
// ESEKF操作
MeasureGroup Measures;
esekfom::esekf<state_ikfom, 12, input_ikfom> kf; // 状态，噪声维度，输入
state_ikfom state_point; // 状态
vect3 pos_lid; // world系下lidar坐标
//输出的路径参数
nav_msgs::Path path; //包含了一系列位姿
nav_msgs::Odometry odomAftMapped; //只包含了一个位姿
geometry_msgs::Quaternion geoQuat; //四元数
geometry_msgs::PoseStamped msg_body_pose; //位姿
//激光和imu处理操作
shared_ptr<Preprocess> p_pre(new Preprocess()); // 定义指向激光雷达数据的预处理类Preprocess的智能指针
shared_ptr<ImuProcess> p_imu(new ImuProcess()); // 定义指向IMU数据预处理类ImuProcess的智能指针
下面的两个函数就比较简单，分别是接受到中断信号会触发以及将详细的信息打印到log中以便回溯
//按下ctrl+c后唤醒所有线程
void SigHandle(int sig)
{
flg_exit = true;
ROS_WARN("catch sig %d", sig);
sig_buffer.notify_all(); // 会唤醒所有等待队列中阻塞的线程 线程被唤醒后，会通过轮询方式获得锁，获得锁前也一直处理运行状态，不会被再次阻塞。
}
//将fast lio2信息打印到log中
inline void dump_lio_state_to_log(FILE *fp)
{
V3D rot_ang(Log(state_point.rot.toRotationMatrix()));
fprintf(fp, "%lf ", Measures.lidar_beg_time - first_lidar_time);
fprintf(fp, "%lf %lf %lf ", rot_ang(0), rot_ang(1), rot_ang(2)); // Angle
fprintf(fp, "%lf %lf %lf ", state_point.pos(0), state_point.pos(1), state_point.pos(2)); // Pos
fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // omega
fprintf(fp, "%lf %lf %lf ", state_point.vel(0), state_point.vel(1), state_point.vel(2)); // Vel
fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // Acc
fprintf(fp, "%lf %lf %lf ", state_point.bg(0), state_point.bg(1), state_point.bg(2)); // Bias_g
fprintf(fp, "%lf %lf %lf ", state_point.ba(0), state_point.ba(1), state_point.ba(2)); // Bias_a
fprintf(fp, "%lf %lf %lf ", state_point.grav[0], state_point.grav[1], state_point.grav[2]); // Bias_a
fprintf(fp, "\r\n");
fflush(fp);
}
然后就是一系列的点云转换公式，基本上就是将雷达的坐标转到世界坐标系中，或者将雷达坐标系转化到IMU坐标系中
//把点从body系转到world系，通过ikfom的位置和姿态
void pointBodyToWorld_ikfom(PointType const *const pi, PointType *const po, state_ikfom &s)
{
V3D p_body(pi->x, pi->y, pi->z);
//下面式子最里面的括号是从雷达到IMU坐标系 然后从IMU转到世界坐标系
V3D p_global(s.rot * (s.offset_R_L_I * p_body + s.offset_T_L_I) + s.pos);
po->x = p_global(0);
po->y = p_global(1);
po->z = p_global(2);
po->intensity = pi->intensity;
}
//把点从body系转到world系
void pointBodyToWorld(PointType const *const pi, PointType *const po)
{
V3D p_body(pi->x, pi->y, pi->z);
V3D p_global(state_point.rot * (state_point.offset_R_L_I * p_body + state_point.offset_T_L_I) + state_point.pos);
po->x = p_global(0);
po->y = p_global(1);
po->z = p_global(2);
po->intensity = pi->intensity;
}
//把点从body系转到world系
template <typename T>
void pointBodyToWorld(const Matrix<T, 3, 1> &pi, Matrix<T, 3, 1> &po)
{
V3D p_body(pi[0], pi[1], pi[2]);
V3D p_global(state_point.rot * (state_point.offset_R_L_I * p_body + state_point.offset_T_L_I) + state_point.pos);
po[0] = p_global(0);
po[1] = p_global(1);
po[2] = p_global(2);
}
// 含有RGB的点云从body系转到world系
void RGBpointBodyToWorld(PointType const *const pi, PointType *const po)
{
V3D p_body(pi->x, pi->y, pi->z);
V3D p_global(state_point.rot * (state_point.offset_R_L_I * p_body + state_point.offset_T_L_I) + state_point.pos);
po->x = p_global(0);
po->y = p_global(1);
po->z = p_global(2);
po->intensity = pi->intensity;
}
// 含有RGB的点云从Lidar系转到IMU系
void RGBpointBodyLidarToIMU(PointType const *const pi, PointType *const po)
{
V3D p_body_lidar(pi->x, pi->y, pi->z);
V3D p_body_imu(state_point.offset_R_L_I * p_body_lidar + state_point.offset_T_L_I);
po->x = p_body_imu(0);
po->y = p_body_imu(1);
po->z = p_body_imu(2);
po->intensity = pi->intensity;
}
然后在下面就是一个重构IKD-Tree过滤后剔除的点集
//得到被剔除的点
void points_cache_collect()
{
PointVector points_history;
ikdtree.acquire_removed_points(points_history); //返回被剔除的点
for (int i = 0; i < points_history.size(); i++)
_featsArray->push_back(points_history[i]); //存入到缓存中，后面没有用到该数据
}
2. 动态调整地图
地图点被组织成一个ikd-Tree，该树以里程计速率合并新的点云扫描而动态增长。为了防止地图的大小不受约束，ikid - tree中只保留LiDAR当前位置周围长为L的大型局部区域中的地图点。2D 演示图如下所示。
(a)中，蓝色矩形为长度为l的初始映射区域，红色圆形为以初始LiDAR位置$p_0$为中心的初始探测区域。在(b)中，检测区域(虚线红色圆圈)移动到一个新的位置$p^{‘}$(带实红线的圆圈)，该位置触碰了地图边界。根据距离$d$，地图区域被移动到一个新的位置(绿色矩形)。减法区域(橙色区域)中的点从地图(即 ikd-Tree)中移除。
将映射区域初始化为一个长为L的立方体，该立方体以初始LiDAR位置$p_0$为中心。假设LiDAR的探测区域为以(15)更新得到位置的LiDAR当前位置为中心的探测球。
假设探测球的半径$r = γR$，其中R为LiDAR视场(FOV)范围，$γ$为大于1的松弛参数。当LiDAR移动到检测球接触到地图边界的新位置p0时，地图区域会向增加LiDAR检测区域到接触边界的距离的方向移动。地图区域移动的距离设置为常数$d = (γ−1)R$。通过后中详细介绍的逐框删除操作，新映射区域和旧映射区域之间的减法区域中的所有点将从ikd-Tree中删除。
// 在拿到eskf前馈结果后。动态调整地图区域，防止地图过大而内存溢出，类似LOAM中提取局部地图的方法
BoxPointType LocalMap_Points; // ikd-tree中,局部地图的包围盒角点
bool Localmap_Initialized = false; // 局部地图是否初始化
void lasermap_fov_segment()
{
cub_needrm.clear(); // 清空需要移除的区域
kdtree_delete_counter = 0;
kdtree_delete_time = 0.0;
// X轴分界点转换到w系下，好像没有用到
pointBodyToWorld(XAxisPoint_body, XAxisPoint_world);
// global系下lidar位置
V3D pos_LiD = pos_lid;
//初始化局部地图包围盒角点，以为w系下lidar位置为中心,得到长宽高200*200*200的局部地图
if (!Localmap_Initialized)
{ // 系统起始需要初始化局部地图的大小和位置
for (int i = 0; i < 3; i++)
{
LocalMap_Points.vertex_min[i] = pos_LiD(i) - cube_len / 2.0;
LocalMap_Points.vertex_max[i] = pos_LiD(i) + cube_len / 2.0;
}
Localmap_Initialized = true;
return;
}
// 各个方向上Lidar与局部地图边界的距离，或者说是lidar与立方体盒子六个面的距离
float dist_to_map_edge[3][2];
bool need_move = false;
// 当前雷达系中心到各个地图边缘的距离
for (int i = 0; i < 3; i++)
{
dist_to_map_edge[i][0] = fabs(pos_LiD(i) - LocalMap_Points.vertex_min[i]);
dist_to_map_edge[i][1] = fabs(pos_LiD(i) - LocalMap_Points.vertex_max[i]);
// 与某个方向上的边界距离（例如1.5*300m）太小，标记需要移除need_move，参考论文Fig3
if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE || dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE)
need_move = true;
}
// 不需要挪动就直接退回了
if (!need_move)
return;
// 否则需要计算移动的距离
BoxPointType New_LocalMap_Points, tmp_boxpoints;
// 新的局部地图盒子边界点
New_LocalMap_Points = LocalMap_Points;
float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9, double(DET_RANGE * (MOV_THRESHOLD - 1)));
for (int i = 0; i < 3; i++)
{
tmp_boxpoints = LocalMap_Points;
//与包围盒最小值边界点距离
if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE)
{
New_LocalMap_Points.vertex_max[i] -= mov_dist;
New_LocalMap_Points.vertex_min[i] -= mov_dist;
tmp_boxpoints.vertex_min[i] = LocalMap_Points.vertex_max[i] - mov_dist;
cub_needrm.push_back(tmp_boxpoints); // 移除较远包围盒
}
else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE)
{
New_LocalMap_Points.vertex_max[i] += mov_dist;
New_LocalMap_Points.vertex_min[i] += mov_dist;
tmp_boxpoints.vertex_max[i] = LocalMap_Points.vertex_min[i] + mov_dist;
cub_needrm.push_back(tmp_boxpoints);
}
}
LocalMap_Points = New_LocalMap_Points;
points_cache_collect();
double delete_begin = omp_get_wtime();
// 使用Boxs删除指定盒内的点
if (cub_needrm.size() > 0)
kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm);
kdtree_delete_time = omp_get_wtime() - delete_begin;
}
3. 传感器信息获取
这部分内容就是使用topic从各个传感器中读取数据并传入预处理的过程。同时timediff_lidar_wrt_imu会完成将imu的时间戳对齐到激光雷达时间戳的功能，通过两个最新的时间戳完成校验以及对齐。
// 除了AVIA类型之外的雷达点云回调函数，将数据引入到buffer当中
void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
mtx_buffer.lock(); //加锁
scan_count++;
double preprocess_start_time = omp_get_wtime(); //记录时间
if (msg->header.stamp.toSec() < last_timestamp_lidar)
{
ROS_ERROR("lidar loop back, clear buffer");
lidar_buffer.clear();
}
PointCloudXYZI::Ptr ptr(new PointCloudXYZI());
p_pre->process(msg, ptr); //点云预处理
lidar_buffer.push_back(ptr); //将点云放入缓冲区
time_buffer.push_back(msg->header.stamp.toSec()); //将时间放入缓冲区
last_timestamp_lidar = msg->header.stamp.toSec(); //记录最后一个时间
s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; //预处理时间
mtx_buffer.unlock();
sig_buffer.notify_all(); // 唤醒所有线程
}
double timediff_lidar_wrt_imu = 0.0;
bool timediff_set_flg = false; // 时间同步flag，false表示未进行时间同步，true表示已经进行过时间同步
// 订阅器sub_pcl的回调函数：接收Livox激光雷达的点云数据，对点云数据进行预处理（特征提取、降采样、滤波），并将处理后的数据保存到激光雷达数据队列中
void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
{
// 互斥锁
mtx_buffer.lock();
// 点云预处理的开始时间
double preprocess_start_time = omp_get_wtime();
// 激光雷达扫描的总次数
scan_count++;
// 如果当前扫描的激光雷达数据的时间戳比上一次扫描的激光雷达数据的时间戳早，需要将激光雷达数据缓存队列清空
if (msg->header.stamp.toSec() < last_timestamp_lidar)
{
ROS_ERROR("lidar loop back, clear buffer");
lidar_buffer.clear();
}
last_timestamp_lidar = msg->header.stamp.toSec();
// 如果不需要进行时间同步，而imu时间戳和雷达时间戳相差大于10s，则输出错误信息
if (!time_sync_en && abs(last_timestamp_imu - lidar_end_time) > 10.0)
{
printf("IMU and LiDAR not Synced, IMU time: %lf, lidar scan end time: %lf", last_timestamp_imu, lidar_end_time);
}
// time_sync_en为true时，当imu时间戳和雷达时间戳相差大于1s时，进行时间同步
if (time_sync_en && !timediff_set_flg && abs(last_timestamp_lidar - last_timestamp_imu) > 1 && !imu_buffer.empty())
{
timediff_set_flg = true; // 标记已经进行时间同步
timediff_lidar_wrt_imu = last_timestamp_lidar + 0.1 - last_timestamp_imu;
printf("Self sync IMU and LiDAR, time diff is %.10lf \n", timediff_lidar_wrt_imu);
}
// 用pcl点云格式保存接收到的激光雷达数据
PointCloudXYZI::Ptr ptr(new PointCloudXYZI());
// 对激光雷达数据进行预处理（特征提取或者降采样），其中p_pre是Preprocess类的智能指针
p_pre->process(msg, ptr);
lidar_buffer.push_back(ptr);
time_buffer.push_back(last_timestamp_lidar);
// 点云预处理的总时间
s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time;
mtx_buffer.unlock();
sig_buffer.notify_all(); // 唤醒所有线程
}
// 订阅器sub_imu的回调函数：接收IMU数据，将IMU数据保存到IMU数据缓存队列中
void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in)
{
publish_count++;
// cout<<"IMU got at: "<<msg_in->header.stamp.toSec()<<endl;
sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in));
// 将IMU和激光雷达点云的时间戳对齐
if (abs(timediff_lidar_wrt_imu) > 0.1 && time_sync_en) //时间同步校准
{
msg->header.stamp =
ros::Time().fromSec(timediff_lidar_wrt_imu + msg_in->header.stamp.toSec()); //将IMU时间戳对齐到激光雷达时间戳
}
double timestamp = msg->header.stamp.toSec(); // IMU时间戳
mtx_buffer.lock();
// 如果当前IMU的时间戳小于上一个时刻IMU的时间戳，则IMU数据有误，将IMU数据缓存队列清空
if (timestamp < last_timestamp_imu)
{
ROS_WARN("imu loop back, clear buffer");
imu_buffer.clear();
}
// 将当前的IMU数据保存到IMU数据缓存队列中
last_timestamp_imu = timestamp;
imu_buffer.push_back(msg);
mtx_buffer.unlock(); //解锁
sig_buffer.notify_all(); // 唤醒阻塞的线程，当持有锁的线程释放锁时，这些线程中的一个会获得锁。而其余的会接着尝试获得锁
}
4. 时间戳对齐
这部分主要处理了buffer中的数据，将两帧激光雷达点云数据时间内的IMU数据从缓存队列中取出，进行时间对齐，并保存到meas中
bool sync_packages(MeasureGroup &meas)
{
if (lidar_buffer.empty() || imu_buffer.empty()) //如果缓存队列中没有数据，则返回false
{
return false;
}
/*** push a lidar scan ***/
//如果还没有把雷达数据放到meas中的话，就执行一下操作
if (!lidar_pushed)
{
// 从激光雷达点云缓存队列中取出点云数据，放到meas中
meas.lidar = lidar_buffer.front();
// 如果该lidar没有点云，则返回false
if (meas.lidar->points.size() <= 1)
{
lidar_buffer.pop_front();
return false;
}
// 该lidar测量起始的时间戳
meas.lidar_beg_time = time_buffer.front();
lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000); //此次雷达点云结束时刻
// 成功提取到lidar测量的标志
lidar_pushed = true;
}
// 最新的IMU时间戳(也就是队尾的)不能早于雷达的end时间戳，因为last_timestamp_imu比较时是加了0.1的，所以要比较大于雷达的end时间戳
if (last_timestamp_imu < lidar_end_time)
{
return false;
}
// 压入imu数据，并从imu缓冲区弹出
double imu_time = imu_buffer.front()->header.stamp.toSec();
meas.imu.clear();
// 拿出lidar_beg_time到lidar_end_time之间的所有IMU数据
while ((!imu_buffer.empty()) && (imu_time < lidar_end_time)) //如果imu缓存队列中的数据时间戳小于雷达结束时间戳，则将该数据放到meas中,代表了这一帧中的imu数据
{
imu_time = imu_buffer.front()->header.stamp.toSec(); //获取imu数据的时间戳
if (imu_time > lidar_end_time)
break;
meas.imu.push_back(imu_buffer.front()); //将imu数据放到meas中
imu_buffer.pop_front();
}
lidar_buffer.pop_front(); //将lidar数据弹出
time_buffer.pop_front(); //将时间戳弹出
lidar_pushed = false; //将lidar_pushed置为false，代表lidar数据已经被放到meas中了
return true;
}
5. 发布信息
这里在此之前还存在有一个map_incremental函数，这里面主要做的是将点插入与降采样插入的操作，这里我们放在ikd-tree里面讲，我们先看一下比较简单的消息发布
PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1)); //创建一个点云用于存储等待发布的点云
PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI()); //创建一个点云用于存储等待保存的点云
void publish_frame_world(const ros::Publisher &pubLaserCloudFull)
{
if (scan_pub_en) // 设置是否发布激光雷达数据，是否发布稠密数据，是否发布激光雷达数据的身体数据
{
PointCloudXYZI::Ptr laserCloudFullRes(dense_pub_en ? feats_undistort : feats_down_body); //判断是否需要降采样
int size = laserCloudFullRes->points.size(); //获取待转换点云的大小
PointCloudXYZI::Ptr laserCloudWorld(
new PointCloudXYZI(size, 1)); //创建一个点云用于存储转换到世界坐标系的点云
for (int i = 0; i < size; i++)
{
RGBpointBodyToWorld(&laserCloudFullRes->points[i],
&laserCloudWorld->points[i]);
}
sensor_msgs::PointCloud2 laserCloudmsg;
pcl::toROSMsg(*laserCloudWorld, laserCloudmsg);
laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
laserCloudmsg.header.frame_id = "camera_init";
pubLaserCloudFull.publish(laserCloudmsg);
publish_count -= PUBFRAME_PERIOD;
}
//把结果压入到pcd中
/**************** save map ****************/
/* 1. make sure you have enough memories
/* 2. pcd save will largely influence the real-time performences **/
if (pcd_save_en)
{
int size = feats_undistort->points.size();
PointCloudXYZI::Ptr laserCloudWorld(
new PointCloudXYZI(size, 1));
for (int i = 0; i < size; i++)
{
RGBpointBodyToWorld(&feats_undistort->points[i],
&laserCloudWorld->points[i]); //转换到世界坐标系
}
*pcl_wait_save += *laserCloudWorld; //把结果压入到pcd中
}
}
//把去畸变的雷达系下的点云转到IMU系
void publish_frame_body(const ros::Publisher &pubLaserCloudFull_body)
{
int size = feats_undistort->points.size();
PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1)); //创建一个点云用于存储转换到IMU系的点云
for (int i = 0; i < size; i++)
{
RGBpointBodyLidarToIMU(&feats_undistort->points[i],
&laserCloudIMUBody->points[i]); //转换到IMU坐标系
}
sensor_msgs::PointCloud2 laserCloudmsg;
pcl::toROSMsg(*laserCloudIMUBody, laserCloudmsg);
laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
laserCloudmsg.header.frame_id = "body";
pubLaserCloudFull_body.publish(laserCloudmsg);
publish_count -= PUBFRAME_PERIOD;
}
//发布雷达信息
void publish_frame_lidar(const ros::Publisher &pubLaserCloudFull_lidar)
{
sensor_msgs::PointCloud2 laserCloudmsg;
pcl::toROSMsg(*feats_undistort, laserCloudmsg); //转换到ROS消息格式,并直接发布信息
laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time);
laserCloudmsg.header.frame_id = "lidar";
pubLaserCloudFull_lidar.publish(laserCloudmsg);
publish_count -= PUBFRAME_PERIOD;
}
//把起作用的特征点转到地图中
void publish_effect_world(const ros::Publisher &pubLaserCloudEffect)
{
PointCloudXYZI::Ptr laserCloudWorld(
new PointCloudXYZI(effct_feat_num, 1)); //创建一个点云用于存储转换到世界坐标系的点云,从h_share_model获得
for (int i = 0; i < effct_feat_num; i++)
{
RGBpointBodyToWorld(&laserCloudOri->points[i],
&laserCloudWorld->points[i]);
}
sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = ros::Time().fromSec(lidar_end_time);
laserCloudFullRes3.header.frame_id = "camera_init";
pubLaserCloudEffect.publish(laserCloudFullRes3);
}
// 发布ikd-tree地图
void publish_map(const ros::Publisher &pubLaserCloudMap)
{
sensor_msgs::PointCloud2 laserCloudMap;
pcl::toROSMsg(*featsFromMap, laserCloudMap);
laserCloudMap.header.stamp = ros::Time().fromSec(lidar_end_time);
laserCloudMap.header.frame_id = "camera_init";
pubLaserCloudMap.publish(laserCloudMap);
}
// 设置输出的t,q，在publish_odometry，publish_path调用
template <typename T>
void set_posestamp(T &out)
{
out.pose.position.x = state_point.pos(0); //将eskf求得的位置传入
out.pose.position.y = state_point.pos(1);
out.pose.position.z = state_point.pos(2);
out.pose.orientation.x = geoQuat.x; //将eskf求得的姿态传入
out.pose.orientation.y = geoQuat.y;
out.pose.orientation.z = geoQuat.z;
out.pose.orientation.w = geoQuat.w;
}
//发布里程计
void publish_odometry(const ros::Publisher &pubOdomAftMapped)
{
odomAftMapped.header.frame_id = "camera_init";
odomAftMapped.child_frame_id = "body";
odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time); // ros::Time().fromSec(lidar_end_time);
set_posestamp(odomAftMapped.pose);
pubOdomAftMapped.publish(odomAftMapped);
auto P = kf.get_P();
for (int i = 0; i < 6; i++)
{
int k = i < 3 ? i + 3 : i - 3;
//设置协方差 P里面先是旋转后是位置 这个POSE里面先是位置后是旋转 所以对应的协方差要对调一下
odomAftMapped.pose.covariance[i * 6 + 0] = P(k, 3);
odomAftMapped.pose.covariance[i * 6 + 1] = P(k, 4);
odomAftMapped.pose.covariance[i * 6 + 2] = P(k, 5);
odomAftMapped.pose.covariance[i * 6 + 3] = P(k, 0);
odomAftMapped.pose.covariance[i * 6 + 4] = P(k, 1);
odomAftMapped.pose.covariance[i * 6 + 5] = P(k, 2);
}
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, odomAftMapped.header.stamp, "camera_init", "body")); //发布tf变换
}
//每隔10个发布一下位姿
void publish_path(const ros::Publisher pubPath)
{
set_posestamp(msg_body_pose);
msg_body_pose.header.stamp = ros::Time().fromSec(lidar_end_time);
msg_body_pose.header.frame_id = "camera_init";
/*** if path is too large, the rvis will crash ***/
static int jjj = 0;
jjj++;
if (jjj % 10 == 0)
{
path.poses.push_back(msg_body_pose);
pubPath.publish(path);
}
}
6. 参考链接
https://blog.csdn.net/MWY123_/article/details/124110021
https://zhuanlan.zhihu.com/p/471876531
评论（9）
您还未登录，请登录后发表或查看评论