LI-Init是一种鲁棒、实时的激光雷达惯性系统初始化方法。该方法可校准激光雷达与IMU之间的时间偏移量和外部参数，以及重力矢量和IMU偏差。我们的方法不需要任何目标或额外的传感器，特定的结构化环境，先前的环境点图或初始值的外在和时间偏移。
功能包安装
需要环境要求：
Ubuntu >= 18.04.
ROS >= Melodic
PCL >= 1.8
Eigen >= 3.3.4
下载源码
git clone https://github.com/hku-mars/LiDAR_IMU_Init.git
下载完成提示
正克隆到 ‘LiDAR_IMU_Init’…
remote: Enumerating objects: 340, done.
remote: Counting objects: 100% (118/118), done.
remote: Compressing objects: 100% (62/62), done.
remote: Total 340 (delta 79), reused 78 (delta 55), pack-reused 222
接收对象中: 100% (340/340), 46.43 MiB | 2.57 MiB/s, 完成.
处理 delta 中: 100% (161/161), 完成.
拷贝到工作空间中进行编译
报错提示如下：
CMake Error at /opt/ros/noetic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
Could not find a package configuration file provided by “livox_ros_driver”
with any of the following names:
livox_ros_driverConfig.cmake
livox_ros_driver-config.cmake
Add the installation prefix of “livox_ros_driver” to CMAKE_PREFIX_PATH or
set “livox_ros_driver_DIR” to a directory containing one of the above
files. If “livox_ros_driver” provides a separate development package or
SDK, be sure it has been installed.
Call Stack (most recent call first):
LiDAR_IMU_Init/CMakeLists.txt:45 (find_package)
原因是该功能包的CMakeLists.txt文件中的第45行：
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
nav_msgs
sensor_msgs
roscpp
rospy
std_msgs
pcl_ros
tf
livox_ros_driver #需要这个功能包但是没有找到
message_generation
eigen_conversions
)
需要livox_ros_driver这个功能包但是没有找到
安装这个功能包，需要提前下载安装好Livox SDK
git clone https://github.com/Livox-SDK/Livox-SDK.git
cd Livox-SDK
cd build && cmake ..
成功后提示：
— Configuring done
— Generating done
— Build files have been written to: /home/jk-jone/Livox-SDK/build
make
成功后提示：
[100%] Linking CXX executable lidar_utc_sync
[100%] Built target lidar_utc_sync
sudo make install
成功后提示：
— Install configuration: “”
— Installing: /usr/local/lib/liblivox_sdk_static.a
— Installing: /usr/local/include/livox_def.h
— Installing: /usr/local/include/livox_sdk.h
下载livox_ros_driver功能包
git clone https://github.com/Livox-SDK/livox_ros_driver.git
放到工作空间中进行编译
则不再提示
Could not find a package configuration file provided by “livox_ros_driver”
再次编译提示缺少Ceres功能包
Make Error at LiDAR_IMU_Init/CMakeLists.txt:61 (find_package):
By not providing “FindCeres.cmake” in CMAKE_MODULE_PATH this project has
asked CMake to find a package configuration file provided by “Ceres”, but
CMake did not find one.
Could not find a package configuration file provided by “Ceres” with any of
the following names:
CeresConfig.cmake
ceres-config.cmake
Add the installation prefix of “Ceres” to CMAKE_PREFIX_PATH or set
“Ceres_DIR” to a directory containing one of the above files. If “Ceres”
provides a separate development package or SDK, be sure it has been
installed.
安装ceres-solver-2.0.0 （注意安装2.2.0不行，必须要安装2.0.0）
先安装ceres依赖
sudo apt-get install cmake
sudo apt-get install libgoogle-glog-dev libgflags-dev
sudo apt-get install libatlas-base-dev
sudo apt-get install libeigen3-dev
下载ceres-solver-2.0.0
git clone https://github.com/ceres-solver/ceres-solver # 这个版本不行
git clone -b 2.0.0 https://github.com/ceres-solver/ceres-solver.git
下载完成后形成该文件
然后进行编译
cd ceres-solver
mkdir build
cd build
cmake ..
成功后提示
— Build the examples.
— Configuring done
— Generating done
— Build files have been written to: /home/jk-jone/ceres-solver/build
然后再进行make
make -j4
成功后显示：
[ 99%] Built target ba_iterschur_suitesparse_clusttri_user_test
[100%] Linking CXX executable ../../../bin/ba_sparsecholesky_suitesparse_user_threads_test
[100%] Built target ba_sparsecholesky_suitesparse_user_threads_test
最后进行安装
sudo make install
成功后显示 ：
— Installing: /usr/local/include/ceres/internal/config.h
— Installing: /usr/local/include/ceres/internal/export.h
— Installing: /usr/local/lib/cmake/Ceres/CeresTargets.cmake
— Installing: /usr/local/lib/cmake/Ceres/CeresTargets-release.cmake
— Installing: /usr/local/lib/cmake/Ceres/CeresConfig.cmake
— Installing: /usr/local/lib/cmake/Ceres/CeresConfigVersion.cmake
— Installing: /usr/local/lib/cmake/Ceres/FindGlog.cmake
至此Ceres-solver-2.0.0安装完毕
这个功能包对ceres-solver的版本有要求，2.2.0的版本有问题，需要2.0.0的版本
再次对工作空间进行编译
编译成功
重点参数
参数在config文件夹下的xxx.yaml文件里
里面有几种雷达的，有几款livox的固态雷达
- avia
- horizon
- mid360
还有禾赛的雷达
- pandar
还有几种常见的机械式激光雷达 - ouster
- robosense
- velodyne
可以看出 _Lidar_IMU_Init_ 这套标定算法不但可以用于固态激光雷达同样也可以用于机械式激光雷达
下面来看重点参数
1、lid_topic ： 雷达点云的消息名称
2、imu_topic： IMU的消息名称
3、cut_frame_num： 将一帧分割成子帧，提高里程计频率。必须是正整数。
4、orig_odom_freq： 原始雷达帧输入频率，对于大部分雷达，输入频率是10HZ,机械旋转激光雷达建议cut_frame_num _ orig_odom_freq = 30， Livox 激光雷达建议 cut_frame_num _ orig_odom_freq = 50。
5、mean_acc_norm : IMU 静止时的加速度范数。通常，普通 IMU 为 9.805，Livox 内置 IMU 为 1。
6、data_accum_length ： 评估数据是否足以初始化的阈值。太小可能会导致质量差的结果。
7、online_refine_time（秒）：FAST-LIO2 的外在精炼时间。建议约15~30秒的细化。
8 、 filter_size_surf（米）：室内场景建议filter_size_surf=0.05~0.15，室外场景filter_size_surf=0.5。
9 、 filter_size_map（米）：室内场景建议filter_size_map=0.15~0.25，室外场景filter_size_map=0.5。
10、 blind （米）： 激光雷达距离有效最小距离
核心代码
相关配置参数
nh.param<int>("max_iteration", NUM_MAX_ITERATIONS, 4);
nh.param<int>("point_filter_num", p_pre->point_filter_num, 2);
nh.param<string>("map_file_path", map_file_path, "");
nh.param<string>("common/lid_topic", lid_topic, "/livox/lidar");
nh.param<string>("common/imu_topic", imu_topic, "/livox/imu");
nh.param<double>("mapping/filter_size_surf", filter_size_surf_min, 0.5);
nh.param<double>("mapping/filter_size_map", filter_size_map_min, 0.5);
nh.param<double>("cube_side_length", cube_len, 200);
nh.param<float>("mapping/det_range", DET_RANGE, 300.f);
nh.param<double>("mapping/gyr_cov", gyr_cov, 0.1);
nh.param<double>("mapping/acc_cov", acc_cov, 0.1);
nh.param<double>("mapping/grav_cov", grav_cov, 0.001);
nh.param<double>("mapping/b_gyr_cov", b_gyr_cov, 0.0001);
nh.param<double>("mapping/b_acc_cov", b_acc_cov, 0.0001);
nh.param<double>("preprocess/blind", p_pre->blind, 1.0);
nh.param<int>("preprocess/lidar_type", lidar_type, AVIA);
nh.param<int>("preprocess/scan_line", p_pre->N_SCANS, 16);
nh.param<bool>("preprocess/feature_extract_en", p_pre->feature_enabled, 0);
nh.param<bool>("initialization/cut_frame", cut_frame, true);
nh.param<int>("initialization/cut_frame_num", cut_frame_num, 1);
nh.param<int>("initialization/orig_odom_freq", orig_odom_freq, 10);
nh.param<double>("initialization/online_refine_time", online_refine_time, 20.0);
nh.param<double>("initialization/mean_acc_norm", mean_acc_norm, 9.81);
nh.param<double>("initialization/data_accum_length", Init_LI->data_accum_length, 300);
nh.param<vector<double>>("initialization/Rot_LI_cov", Rot_LI_cov, vector<double>());
nh.param<vector<double>>("initialization/Trans_LI_cov", Trans_LI_cov, vector<double>());
nh.param<bool>("publish/path_en", path_en, true);
nh.param<bool>("publish/scan_publish_en", scan_pub_en, 1);
nh.param<bool>("publish/dense_publish_en", dense_pub_en, 1);
nh.param<bool>("publish/scan_bodyframe_pub_en", scan_body_pub_en, 1);
nh.param<bool>("runtime_pos_log_enable", runtime_pos_log, 0);
nh.param<bool>("pcd_save/pcd_save_en", pcd_save_en, false);
nh.param<int>("pcd_save/interval", pcd_save_interval, -1);
订阅雷达消息
ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? \
nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \
nh.subscribe(lid_topic, 200000, standard_pcl_cbk);
订阅imu消息
ros::Subscriber sub_imu = nh.subscribe<sensor_msgs::Imu>
(imu_topic, 200000, boost::bind(&imu_cbk, _1, pubIMU_sync));
发布的消息
ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_registered", 100000);
ros::Publisher pubLaserCloudFullRes_body = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_registered_body", 100000);
ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_effected", 100000);
ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>
("/Laser_map", 100000);
ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>
("/aft_mapped_to_init", 100000);
ros::Publisher pubPath = nh.advertise<nav_msgs::Path>
("/path", 100000);
同步测量值
if (sync_packages(Measures)) {
处理imu数据
p_imu->Process(Measures, state, feats_undistort);
在激光雷达 FOV 中分割地图
lasermap_fov_segment();
对于一帧中的特征点进行下采样
downSizeFilterSurf.setInputCloud(feats_undistort);
downSizeFilterSurf.filter(*feats_down_body);
t1 = omp_get_wtime();
feats_down_size = feats_down_body->points.size();
初始化地图kdtree
if (ikdtree.Root_Node == nullptr) {
if (feats_down_size > 5) {
ikdtree.set_downsample_param(filter_size_map_min);
feats_down_world->resize(feats_down_size);
for (int i = 0; i < feats_down_size; i++) {
pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));
}
ikdtree.Build(feats_down_world->points);
}
continue;
}
int featsFromMapNum = ikdtree.validnum();
kdtree_size_st = ikdtree.size();
icp和迭代卡尔曼滤波更新
normvec->resize(feats_down_size);
feats_down_world->resize(feats_down_size);
euler_cur = RotMtoEuler(state.rot_end);
pointSearchInd_surf.resize(feats_down_size);
Nearest_Points.resize(feats_down_size);
int rematch_num = 0;
bool nearest_search_en = true;
t2 = omp_get_wtime();
迭代状态估计
std::vector<M3D> body_var;
std::vector<M3D> crossmat_list;
body_var.reserve(feats_down_size);
crossmat_list.reserve(feats_down_size);
double t_update_start = omp_get_wtime();
for (iterCount = 0; iterCount < NUM_MAX_ITERATIONS; iterCount++) {
match_start = omp_get_wtime();
laserCloudOri->clear();
corr_normvect->clear();
total_residual = 0.0;
计算测量值的雅克比矩阵和H矩阵和测量向量
MatrixXd Hsub(effect_feat_num, 12);
MatrixXd Hsub_T_R_inv(12, effect_feat_num);
VectorXd R_inv(effect_feat_num);
VectorXd meas_vec(effect_feat_num);
Hsub.setZero();
Hsub_T_R_inv.setZero();
meas_vec.setZero();
for (int i = 0; i < effect_feat_num; i++) {
const PointType &laser_p = laserCloudOri->points[i];
V3D point_this_L(laser_p.x, laser_p.y, laser_p.z);
V3D point_this = state.offset_R_L_I * point_this_L + state.offset_T_L_I;
M3D var;
calcBodyVar(point_this, 0.02, 0.05, var);
var = state.rot_end * var * state.rot_end.transpose();
M3D point_crossmat;
point_crossmat << SKEW_SYM_MATRX(point_this);
/*** get the normal vector of closest surface/corner ***/
const PointType &norm_p = corr_normvect->points[i];
V3D norm_vec(norm_p.x, norm_p.y, norm_p.z);
R_inv(i) = 1000;
laserCloudOri->points[i].intensity = sqrt(R_inv(i));
/*** calculate the Measurement Jacobian matrix H ***/
if (imu_en) {
M3D point_this_L_cross;
point_this_L_cross << SKEW_SYM_MATRX(point_this_L);
V3D H_R_LI = point_this_L_cross * state.offset_R_L_I.transpose() * state.rot_end.transpose() *
norm_vec;
V3D H_T_LI = state.rot_end.transpose() * norm_vec;
V3D A(point_crossmat * state.rot_end.transpose() * norm_vec);
Hsub.row(i) << VEC_FROM_ARRAY(A), norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(
H_R_LI), VEC_FROM_ARRAY(H_T_LI);
} else {
V3D A(point_crossmat * state.rot_end.transpose() * norm_vec);
Hsub.row(i) << VEC_FROM_ARRAY(A), norm_p.x, norm_p.y, norm_p.z, 0, 0, 0, 0, 0, 0;
}
Hsub_T_R_inv.col(i) = Hsub.row(i).transpose() * 1000;
/*** Measurement: distance to the closest surface/corner ***/
meas_vec(i) = -norm_p.intensity;
}
迭代卡尔曼更新
H_T_H.block<12, 12>(0, 0) = Hsub_T_R_inv * Hsub;
MD(DIM_STATE, DIM_STATE) &&K_1 = (H_T_H + state.cov.inverse()).inverse();
K = K_1.block<DIM_STATE, 12>(0, 0) * Hsub_T_R_inv;
auto vec = state_propagat - state;
solution = K * meas_vec + vec - K * Hsub * vec.block<12, 1>(0, 0);
状态更新
state += solution;
rot_add = solution.block<3, 1>(0, 0);
T_add = solution.block<3, 1>(3, 0);
if ((rot_add.norm() * 57.3 < 0.01) && (T_add.norm() * 100 < 0.015))
flg_EKF_converged = true;
deltaR = rot_add.norm() * 57.3;
deltaT = T_add.norm() * 100;
euler_cur = RotMtoEuler(state.rot_end);
收敛判断和协方差更新
if (!EKF_stop_flg && (rematch_num >= 2 || (iterCount == NUM_MAX_ITERATIONS - 1))) {
if (flg_EKF_inited) {
/*** Covariance Update ***/
G.setZero();
G.block<DIM_STATE, 12>(0, 0) = K * Hsub;
state.cov = (I_STATE - G) * state.cov;
total_distance += (state.pos_end - position_last).norm();
position_last = state.pos_end;
if (!imu_en) {
geoQuat = tf::createQuaternionMsgFromRollPitchYaw
(euler_cur(0), euler_cur(1), euler_cur(2));
} else {
//Publish LiDAR's pose, instead of IMU's pose
M3D rot_cur_lidar = state.rot_end * state.offset_R_L_I;
V3D euler_cur_lidar = RotMtoEuler(rot_cur_lidar);
geoQuat = tf::createQuaternionMsgFromRollPitchYaw
(euler_cur_lidar(0), euler_cur_lidar(1), euler_cur_lidar(2));
}
VD(DIM_STATE) K_sum = K.rowwise().sum();
VD(DIM_STATE) P_diag = state.cov.diagonal();
}
EKF_stop_flg = true;
}
solve_time += omp_get_wtime() - solve_start;
if (EKF_stop_flg) break;
发布里程计
publish_odometry(pubOdomAftMapped);
评论（0）
您还未登录，请登录后发表或查看评论