论文题目：FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter，FAST-LIO：一种基于紧耦合IKF算法的快速的，鲁棒性的激光雷达惯性里程计。这里主要记录自己阅读代码的笔记。
相应的数据文件，我放在了百度网盘上，把数据下载下来，放在相应的目录下就可以。
数据百度网盘链接
提取码：ob9k
上面链接给出的数据中，livox avia激光雷达IMU频率为：200Hz，激光雷达的频率为10Hz，也就是一帧数据的时间。
代码的下载，以及相应依赖库的安装参考：
安装参考
程序运行命令：
roslaunch fast_lio mapping_avia.launch
roslaunch fast_lio mapping_velodyne.launch
cd /home/nvidia/ws_fast_lio/src/FAST_LIO/data/
rosbag play YOUR_DOWNLOADED.bag
运行livox avia其中一条数据，效果如下：
// This is an advanced implementation of the algorithm described in the
// following paper:
// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
// Modifier: Livox dev@livoxtech.com
// Copyright 2013, Ji Zhang, Carnegie Mellon University
// Further contributions copyright (c) 2016, Southwest Research Institute
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// 3. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived from this
// software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
#include <omp.h>
#include <mutex>
#include <math.h>
#include <thread>
#include <fstream>
#include <csignal>
#include <unistd.h>
#include <Python.h>
#include <so3_math.h>
#include <ros/ros.h>
#include <Eigen/Core>
#include "IMU_Processing.hpp"
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <visualization_msgs/Marker.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Vector3.h>
#include <livox_ros_driver/CustomMsg.h>
#include "preprocess.h"
#include <ikd-Tree/ikd_Tree.h>
#define INIT_TIME (0.1)
#define LASER_POINT_COV (0.001)
#define MAXN (720000)
#define PUBFRAME_PERIOD (20)
// time_sync_en:IMU和雷达不是同一个时间系统时使用
/*** Time Log Variables ***/
double kdtree_incremental_time = 0.0, kdtree_search_time = 0.0, kdtree_delete_time = 0.0;
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];
double match_time = 0, solve_time = 0, solve_const_H_time = 0;
int kdtree_size_st = 0, kdtree_size_end = 0;
int add_point_size = 0; // ikdtree 新增数目
int kdtree_delete_counter = 0;
bool runtime_pos_log = false, pcd_save_en = false, time_sync_en = false;
bool extrinsic_est_en = true; // 外参估计开关
bool path_en = true;
/**************************/
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;
double res_mean_last = 0.05; // 观测模型中的平均残差
double total_residual = 0.0; // 观测模型中的残差和
double last_timestamp_lidar = 0; // 最新的雷达接收回调时间戳
double 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;
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;
double first_lidar_time = 0.0; // 一帧雷达数据的开始时间戳
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, pcd_save_interval = -1, pcd_index = 0;
bool point_selected_surf[100000] = {0}; // 有效的特征点
bool lidar_pushed = true; //
bool flg_first_scan = true; // 第一帧
bool 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;
vector<PointVector> Nearest_Points; // ？？
vector<double> extrinT(3, 0.0);
vector<double> extrinR(9, 0.0);
deque<double> time_buffer; // 激光雷达数据
deque<PointCloudXYZI::Ptr> lidar_buffer; // 雷达数据队列
deque<sensor_msgs::Imu::ConstPtr> imu_buffer; // IMU数据队列（指针形式）
// PointCloudXYZI:点云坐标 + 信号强度形式
// Ptr:指针形式
PointCloudXYZI::Ptr featsFromMap(new PointCloudXYZI());
PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI());
PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI()); // 雷达坐标系
PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI());// 世界坐标系
PointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1));
PointCloudXYZI::Ptr laserCloudOri(new PointCloudXYZI(100000, 1)); // 雷达滤波后原始数据
PointCloudXYZI::Ptr corr_normvect(new PointCloudXYZI(100000, 1)); // 存放法向量
PointCloudXYZI::Ptr _featsArray;
// VoxelGrid:使用体素化网格的方法实现下采样，并保持点云的形状特征
pcl::VoxelGrid<PointType> downSizeFilterSurf;
pcl::VoxelGrid<PointType> downSizeFilterMap;
KD_TREE<PointType> ikdtree;
V3F XAxisPoint_body(LIDAR_SP_LEN, 0.0, 0.0);
V3F XAxisPoint_world(LIDAR_SP_LEN, 0.0, 0.0);
V3D euler_cur;
V3D position_last(Zero3d);
V3D Lidar_T_wrt_IMU(Zero3d); // 雷达和IMU之间的杆臂
M3D Lidar_R_wrt_IMU(Eye3d); // 雷达和IMU之间的安装角（转换矩阵形式）
/*** EKF inputs and output ***/
MeasureGroup Measures; // 激光雷达，IMU数据
// state_ikfom:22维
// 系统噪声的维数：12
// input_ikfom:6维
esekfom::esekf<state_ikfom, 12, input_ikfom> kf;
state_ikfom state_point; // 状态向量（反馈之后）
vect3 pos_lid; // 雷达位置（导航系）
nav_msgs::Path path;
nav_msgs::Odometry odomAftMapped; // 里程计消息
geometry_msgs::Quaternion geoQuat;
geometry_msgs::PoseStamped msg_body_pose;
shared_ptr<Preprocess> p_pre(new Preprocess());
shared_ptr<ImuProcess> p_imu(new ImuProcess()); // 类指针
void SigHandle(int sig)
{
flg_exit = true;
ROS_WARN("catch sig %d", sig);
sig_buffer.notify_all();
}
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);
}
// 函数功能：激光雷达坐标点转到世界坐标系
void pointBodyToWorld_ikfom(PointType const * const pi, PointType * const po, state_ikfom &s)
{
V3D p_body(pi->x, pi->y, pi->z);
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;
}
// 函数功能：激光雷达坐标点转到世界坐标系
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; // 信号强度
}
// pi:激光雷达坐标系
// 函数功能：激光雷达坐标点转到世界坐标系
// state_point.offset_R_L_I*p_body + state_point.offset_T_L_I:转到IMU坐标系
// state_point.rot：IMU坐标系到世界坐标系
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);
}
// 函数功能：激光雷达坐标点转到世界坐标系
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;
}
// 函数功能：激光雷达坐标点转到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;
}
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]);
}
BoxPointType LocalMap_Points;
bool Localmap_Initialized = false;
void lasermap_fov_segment()
{
cub_needrm.clear();
kdtree_delete_counter = 0;
kdtree_delete_time = 0.0;
pointBodyToWorld(XAxisPoint_body, XAxisPoint_world); // ??
V3D pos_LiD = pos_lid;
if (!Localmap_Initialized){
for (int i = 0; i < 3; i++){
// vertex:顶点
// cube_len:200
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;
}
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]);
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();
if(cub_needrm.size() > 0) kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm);
kdtree_delete_time = omp_get_wtime() - delete_begin;
}
// 标准雷达回调函数
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();
}
// 雷达和IMU之间的时间差（不是同一个时间系统）
double timediff_lidar_wrt_imu = 0.0;
bool timediff_set_flg = false; // 是否已经计算了时间差
// livox激光雷达回调函数
void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
{
mtx_buffer.lock();
// omp_get_wtime：获得绝对时间
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();
// 不是同一个时间系统
if (!time_sync_en && abs(last_timestamp_imu - last_timestamp_lidar) > 10.0 && !imu_buffer.empty() && !lidar_buffer.empty() )
{
printf("IMU and LiDAR not Synced, IMU time: %lf, lidar header time: %lf \n",last_timestamp_imu, last_timestamp_lidar);
}
// 如果是同一个时间系统,正常情况下不会相差大于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; // 0.1??
printf("Self sync IMU and LiDAR, time diff is %.10lf \n", timediff_lidar_wrt_imu);
}
PointCloudXYZI::Ptr ptr(new PointCloudXYZI());
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();
}
// 接收IMU数据回调函数
// ConstPtr：智能指针
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));
// 不是同一个时间系统,需要转换到同一个时间系统
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());
}
double timestamp = msg->header.stamp.toSec();
mtx_buffer.lock(); // 上锁
if (timestamp < last_timestamp_imu)
{
ROS_WARN("imu loop back, clear buffer");
imu_buffer.clear();
}
last_timestamp_imu = timestamp;
imu_buffer.push_back(msg);
mtx_buffer.unlock(); // 解锁
sig_buffer.notify_all(); // ？？
}
double lidar_mean_scantime = 0.0; // 雷达扫描一帧平均时间
int scan_num = 0; // 激光雷达帧数
// 取一帧激光雷达数据，以及对应时间区间的IMU数据
// 输入数据：lidar_buffer，imu_buffer
// 输出数据：MeasureGroup
// 备注：必须同时有IMU数据，以及雷达数据
bool sync_packages(MeasureGroup &meas)
{
if (lidar_buffer.empty() || imu_buffer.empty()) {
return false;
}
/*** push a lidar scan ***/
// 计算lidar_end_time
if(!lidar_pushed)
{
meas.lidar = lidar_buffer.front();
meas.lidar_beg_time = time_buffer.front();
if (meas.lidar->points.size() <= 1) // time too little
{
lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime;
ROS_WARN("Too few input point cloud!\n");
}
// curvature:曲率？时间单位
// 一帧所用的时间
else if (meas.lidar->points.back().curvature / double(1000) < 0.5 * lidar_mean_scantime)
{
lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime;
}
else
{
scan_num ++;
lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000);
// 迭代方式计算平均时间
lidar_mean_scantime += (meas.lidar->points.back().curvature / double(1000) - lidar_mean_scantime) / scan_num;
}
meas.lidar_end_time = lidar_end_time;
lidar_pushed = true;
}
// last_timestamp_imu：最新IMU时间戳
// 必须有IMU数据
if (last_timestamp_imu < lidar_end_time)
{
return false;
}
/*** push imu data, and pop from imu buffer ***/
// 在激光雷达一帧时间区间中取IMU数据
// 同时有IMU数据和雷达数据
double imu_time = imu_buffer.front()->header.stamp.toSec();
meas.imu.clear();
while ((!imu_buffer.empty()) && (imu_time < lidar_end_time))
{
imu_time = imu_buffer.front()->header.stamp.toSec();
if(imu_time > lidar_end_time) break;
meas.imu.push_back(imu_buffer.front());
imu_buffer.pop_front();
}
lidar_buffer.pop_front();
time_buffer.pop_front();
lidar_pushed = false;
return true;
}
int process_increments = 0;
// 更新地图
void map_incremental()
{
PointVector PointToAdd;// 需要在地图中新增的雷达点
PointVector PointNoNeedDownsample; // 不需要在地图中新增的雷达点
PointToAdd.reserve(feats_down_size);
PointNoNeedDownsample.reserve(feats_down_size);
for (int i = 0; i < feats_down_size; i++)
{
/* transform to world frame */
// 转到导航坐标系
pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));
/* decide if need add to map */
// Nearest_Points??
if (!Nearest_Points[i].empty() && flg_EKF_inited)
{
const PointVector &points_near = Nearest_Points[i];
bool need_add = true;
BoxPointType Box_of_Point;
PointType downsample_result, mid_point;
// 体素滤波器长度：filter_size_map_min
mid_point.x = floor(feats_down_world->points[i].x/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min;
mid_point.y = floor(feats_down_world->points[i].y/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min;
mid_point.z = floor(feats_down_world->points[i].z/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min;
// 二范数：dist
float dist = calc_dist(feats_down_world->points[i],mid_point);
if (fabs(points_near[0].x - mid_point.x) > 0.5 * filter_size_map_min && fabs(points_near[0].y - mid_point.y) > 0.5 * filter_size_map_min && fabs(points_near[0].z - mid_point.z) > 0.5 * filter_size_map_min){
PointNoNeedDownsample.push_back(feats_down_world->points[i]);
continue;
}
for (int readd_i = 0; readd_i < NUM_MATCH_POINTS; readd_i ++)
{
// NUM_MATCH_POINTS：5
if (points_near.size() < NUM_MATCH_POINTS) break;
if (calc_dist(points_near[readd_i], mid_point) < dist)
{
need_add = false;
break;
}
}
if (need_add) PointToAdd.push_back(feats_down_world->points[i]);
}
else // 初始点
{
PointToAdd.push_back(feats_down_world->points[i]);
}
}
double st_time = omp_get_wtime();
add_point_size = ikdtree.Add_Points(PointToAdd, true);
ikdtree.Add_Points(PointNoNeedDownsample, false);
add_point_size = PointToAdd.size() + PointNoNeedDownsample.size();
kdtree_incremental_time = omp_get_wtime() - st_time;
}
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;
}
/**************** save map ****************/
/* 1. make sure you have enough memories
/* 2. noted that pcd save will 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;
static int scan_wait_num = 0;
scan_wait_num ++;
if (pcl_wait_save->size() > 0 && pcd_save_interval > 0 && scan_wait_num >= pcd_save_interval)
{
pcd_index ++;
string all_points_dir(string(string(ROOT_DIR) + "PCD/scans_") + to_string(pcd_index) + string(".pcd"));
pcl::PCDWriter pcd_writer;
cout << "current scan saved to /PCD/" << all_points_dir << endl;
pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
pcl_wait_save->clear();
scan_wait_num = 0;
}
}
}
void publish_frame_body(const ros::Publisher & pubLaserCloudFull_body)
{
int size = feats_undistort->points.size();
PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1));
for (int i = 0; i < size; i++)
{
RGBpointBodyLidarToIMU(&feats_undistort->points[i], \
&laserCloudIMUBody->points[i]);
}
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_effect_world(const ros::Publisher & pubLaserCloudEffect)
{
PointCloudXYZI::Ptr laserCloudWorld( \
new PointCloudXYZI(effct_feat_num, 1));
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);
}
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);
}
template<typename T>
void set_posestamp(T & out)
{
out.pose.position.x = state_point.pos(0);
out.pose.position.y = state_point.pos(1);
out.pose.position.z = state_point.pos(2);
out.pose.orientation.x = geoQuat.x;
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 ++)
{
// 0，3
// 1，4
// 2，5
// 3，0
// 4，1
// 5，2
// 0-2：位置
// 3-5：欧拉角
int k = i < 3 ? i + 3 : i - 3;
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" ) );
}
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);
}
}
// 观测模型
void h_share_model(state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_data)
{
double match_start = omp_get_wtime();
laserCloudOri->clear();
corr_normvect->clear();
total_residual = 0.0; // 残差和
// 最邻近面搜索，以及残差计算
/** closest surface search and residual computation **/
#ifdef MP_EN
omp_set_num_threads(MP_PROC_NUM);
#pragma omp parallel for
#endif
// feats_down_size？？
//遍历所有的特征点
for (int i = 0; i < feats_down_size; i++)
{
// feats_down_body：网格滤波器之后的激光点
PointType &point_body = feats_down_body->points[i];
// feats_down_world：世界坐标系下的激光点
PointType &point_world = feats_down_world->points[i];
/* transform to world frame */
V3D p_body(point_body.x, point_body.y, point_body.z);
// 激光雷达坐标系->IMU坐标系->世界坐标系
V3D p_global(s.rot * (s.offset_R_L_I*p_body + s.offset_T_L_I) + s.pos);
point_world.x = p_global(0);
point_world.y = p_global(1);
point_world.z = p_global(2);
point_world.intensity = point_body.intensity; // 信号强度
// NUM_MATCH_POINTS：5
vector<float> pointSearchSqDis(NUM_MATCH_POINTS);
auto &points_near = Nearest_Points[i];
if (ekfom_data.converge)
{
/** Find the closest surfaces in the map **/
// 在地图中找到与之最邻近的平面
ikdtree.Nearest_Search(point_world, NUM_MATCH_POINTS, points_near, pointSearchSqDis);
//判断是否是有效匹配点，与loam系列类似，要求特征点最近邻的地图点数量大于阈值A，距离小于阈值B
point_selected_surf[i] = points_near.size() < NUM_MATCH_POINTS ? false : pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5 ? false : true;
}
if (!point_selected_surf[i]) continue;
VF(4) pabcd; //法向量
// 参考：https://blog.csdn.net/u011483307/article/details/51034169
point_selected_surf[i] = false;
if (esti_plane(pabcd, points_near, 0.1f))//计算平面法向量
{
float pd2 = pabcd(0) * point_world.x + pabcd(1) * point_world.y + pabcd(2) * point_world.z + pabcd(3);//残差（点到平面距离）
// 发射距离越长，测量误差越大，归一化，消除雷达点发射距离的影响
float s = 1 - 0.9 * fabs(pd2) / sqrt(p_body.norm());
if (s > 0.9)
{
point_selected_surf[i] = true;
normvec->points[i].x = pabcd(0); //法向量
normvec->points[i].y = pabcd(1);
normvec->points[i].z = pabcd(2);
normvec->points[i].intensity = pd2; //残差存到intensity里
res_last[i] = abs(pd2);
}
}
}
effct_feat_num = 0;
for (int i = 0; i < feats_down_size; i++)
{
if (point_selected_surf[i])//只保留有效的特征点
{
laserCloudOri->points[effct_feat_num] = feats_down_body->points[i];
corr_normvect->points[effct_feat_num] = normvec->points[i];
total_residual += res_last[i];
effct_feat_num ++;
}
}
if (effct_feat_num < 1)
{
ekfom_data.valid = false;
ROS_WARN("No Effective Points! \n");
return;
}
res_mean_last = total_residual / effct_feat_num;
match_time += omp_get_wtime() - match_start;
double solve_start_ = omp_get_wtime();
/*** Computation of Measuremnt Jacobian matrix H and measurents vector ***/
// 计算雅可比矩阵，以及观测向量
//h_x是观测h相对于状态x的jacobian，见fatliov1的论文公式(14)
//h_x 为观测相对于（姿态、位置、imu和雷达间的变换）
//的雅克比，尺寸为 特征点数x12
ekfom_data.h_x = MatrixXd::Zero(effct_feat_num, 12); //23
ekfom_data.h.resize(effct_feat_num);
for (int i = 0; i < effct_feat_num; i++)
{
const PointType &laser_p = laserCloudOri->points[i];
V3D point_this_be(laser_p.x, laser_p.y, laser_p.z);
M3D point_be_crossmat;
point_be_crossmat << SKEW_SYM_MATRX(point_this_be);
// 激光雷达坐标系->IMU坐标系
V3D point_this = s.offset_R_L_I * point_this_be + s.offset_T_L_I;
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);
/*** calculate the Measuremnt Jacobian matrix H ***/
// conjugate:共轭，相当于矩阵求逆
//
V3D C(s.rot.conjugate() *norm_vec); // 转到IMU坐标系
V3D A(point_crossmat * C);
if (extrinsic_est_en)
{
// 先转到雷达坐标系
// 如何推导？？？
V3D B(point_be_crossmat * s.offset_R_L_I.conjugate() * C); //s.rot.conjugate()*norm_vec);
ekfom_data.h_x.block<1, 12>(i,0) << norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C);
}
else
{
ekfom_data.h_x.block<1, 12>(i,0) << norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
}
/*** Measuremnt: distance to the closest surface/corner ***/
// 观测量：误差形式
ekfom_data.h(i) = -norm_p.intensity;
}
solve_time += omp_get_wtime() - solve_start_;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "laserMapping");
ros::NodeHandle nh;
nh.param<bool>("publish/path_en",path_en, true); // ??
nh.param<bool>("publish/scan_publish_en",scan_pub_en, true);
nh.param<bool>("publish/dense_publish_en",dense_pub_en, true);// 高密度点云
nh.param<bool>("publish/scan_bodyframe_pub_en",scan_body_pub_en, true);
nh.param<int>("max_iteration",NUM_MAX_ITERATIONS,4); // IEKF最大迭代次数？
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<bool>("common/time_sync_en", time_sync_en, false);
nh.param<double>("filter_size_corner",filter_size_corner_min,0.5);
nh.param<double>("filter_size_surf",filter_size_surf_min,0.5);
nh.param<double>("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/fov_degree",fov_deg,180);
nh.param<double>("mapping/gyr_cov",gyr_cov,0.1);
nh.param<double>("mapping/acc_cov",acc_cov,0.1);
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, 0.01);
nh.param<int>("preprocess/lidar_type", p_pre->lidar_type, AVIA); // 激光雷达类型
nh.param<int>("preprocess/scan_line", p_pre->N_SCANS, 16); // 激光雷达线束数
nh.param<int>("preprocess/timestamp_unit", p_pre->time_unit, US);
nh.param<int>("preprocess/scan_rate", p_pre->SCAN_RATE, 10); // 激光雷达扫描频率？
nh.param<int>("point_filter_num", p_pre->point_filter_num, 2);
nh.param<bool>("feature_extract_enable", p_pre->feature_enabled, false); // 是否提取特征
nh.param<bool>("runtime_pos_log_enable", runtime_pos_log, 0);
nh.param<bool>("mapping/extrinsic_est_en", extrinsic_est_en, true); // 打开外参估计
nh.param<bool>("pcd_save/pcd_save_en", pcd_save_en, false);
nh.param<int>("pcd_save/interval", pcd_save_interval, -1);
nh.param<vector<double>>("mapping/extrinsic_T", extrinT, vector<double>()); // IMU和激光雷达之间的杆臂
nh.param<vector<double>>("mapping/extrinsic_R", extrinR, vector<double>()); // IMU和激光雷达之间的安装角（）
cout<<"p_pre->lidar_type "<<p_pre->lidar_type<<endl;
path.header.stamp = ros::Time::now();
path.header.frame_id ="camera_init";
/*** variables definition ***/
int effect_feat_num = 0, frame_num = 0;
double deltaT, deltaR, aver_time_consu = 0, aver_time_icp = 0, aver_time_match = 0, aver_time_incre = 0, aver_time_solve = 0, aver_time_const_H_time = 0;
bool flg_EKF_converged, EKF_stop_flg = 0;
FOV_DEG = (fov_deg + 10.0) > 179.9 ? 179.9 : (fov_deg + 10.0); // ？？
HALF_FOV_COS = cos((FOV_DEG) * 0.5 * PI_M / 180.0);
_featsArray.reset(new PointCloudXYZI());
memset(point_selected_surf, true, sizeof(point_selected_surf));
memset(res_last, -1000.0f, sizeof(res_last));
// setLeafSize：设置体素的长,宽,高
// 这里默认是0.5米
downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min);
downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min);
memset(point_selected_surf, true, sizeof(point_selected_surf));
memset(res_last, -1000.0f, sizeof(res_last));
//设置参数
Lidar_T_wrt_IMU<<VEC_FROM_ARRAY(extrinT);
Lidar_R_wrt_IMU<<MAT_FROM_ARRAY(extrinR);
p_imu->set_extrinsic(Lidar_T_wrt_IMU, Lidar_R_wrt_IMU); // 外参（安装角，杆臂）
p_imu->set_gyr_cov(V3D(gyr_cov, gyr_cov, gyr_cov));
p_imu->set_acc_cov(V3D(acc_cov, acc_cov, acc_cov));
p_imu->set_gyr_bias_cov(V3D(b_gyr_cov, b_gyr_cov, b_gyr_cov));
p_imu->set_acc_bias_cov(V3D(b_acc_cov, b_acc_cov, b_acc_cov));
double epsi[23] = {0.001};
// fill:用相同的值做初始化
fill(epsi, epsi+23, 0.001);
// NUM_MAX_ITERATIONS = 4
//
kf.init_dyn_share(get_f, df_dx, df_dw, h_share_model, NUM_MAX_ITERATIONS, epsi);
/*** debug record ***/
FILE *fp;
string pos_log_dir = root_dir + "/Log/pos_log.txt";
fp = fopen(pos_log_dir.c_str(),"w");
ofstream fout_pre, fout_out, fout_dbg;
fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"),ios::out);
fout_out.open(DEBUG_FILE_DIR("mat_out.txt"),ios::out);
fout_dbg.open(DEBUG_FILE_DIR("dbg.txt"),ios::out);
if (fout_pre && fout_out)
cout << "~~~~"<<ROOT_DIR<<" file opened" << endl;
else
cout << "~~~~"<<ROOT_DIR<<" doesn't exist" << endl;
/*** ROS subscribe initialization ***/
// 订阅话题
// 话题名:lid_topic
// 队列大小:200000
// 回调函数:livox_pcl_cbk
// 通过回调函数接收激光雷达数据,IMU数据
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);
ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk);
// 发布消息类型：sensor_msgs::PointCloud2
// 消息名：/cloud_registered
// 消息队列大小：100000
ros::Publisher pubLaserCloudFull = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_registered", 100000);
ros::Publisher pubLaserCloudFull_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>
("/Odometry", 100000);
ros::Publisher pubPath = nh.advertise<nav_msgs::Path>
("/path", 100000);
//------------------------------------------------------------------------------------------------------
// 信号捕获函数
signal(SIGINT, SigHandle);
// 设置循环频率：5000HZ
ros::Rate rate(5000);
bool status = ros::ok();
while (status)
{
if (flg_exit) break;
// 可以根据自己的需求设置接收频率,更加主动灵活
ros::spinOnce();
// sync_packages: 取一帧激光雷达数据，以及对应时间区间的IMU数据
if(sync_packages(Measures))
{
// 第一帧雷达数据
if (flg_first_scan)
{
first_lidar_time = Measures.lidar_beg_time;
p_imu->first_lidar_time = first_lidar_time;
flg_first_scan = false;
continue;
}
double t0,t1,t2,t3,t4,t5,match_start, solve_start, svd_time;
match_time = 0;
kdtree_search_time = 0.0;
solve_time = 0;
solve_const_H_time = 0;
svd_time = 0;
t0 = omp_get_wtime();
// feats_undistort:运动畸变去除之后的点云数据
// 对IMU数据进行预处理，其中包含了点云畸变处理 前向传播 反向传播
p_imu->Process(Measures, kf, feats_undistort);
state_point = kf.get_x();
// state_point.pos:IMU位置
// pos_lid:雷达位置
pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; // 导航系
if (feats_undistort->empty() || (feats_undistort == NULL))
{
ROS_WARN("No point, skip this scan!\n");
continue;
}
flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? \
false : true;
/*** Segment the map in lidar FOV ***/
lasermap_fov_segment();
/*** downsample the feature points in a scan ***/
downSizeFilterSurf.setInputCloud(feats_undistort);
downSizeFilterSurf.filter(*feats_down_body);
t1 = omp_get_wtime();
feats_down_size = feats_down_body->points.size();
/*** initialize the map 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();
// cout<<"[ mapping ]: In num: "<<feats_undistort->points.size()<<" downsamp "<<feats_down_size<<" Map num: "<<featsFromMapNum<<"effect num:"<<effct_feat_num<<endl;
/*** ICP and iterated Kalman filter update ***/
if (feats_down_size < 5)
{
ROS_WARN("No point, skip this scan!\n");
continue;
}
normvec->resize(feats_down_size);
feats_down_world->resize(feats_down_size);
V3D ext_euler = SO3ToEuler(state_point.offset_R_L_I);
fout_pre<<setw(20)<<Measures.lidar_beg_time - first_lidar_time<<" "<<euler_cur.transpose()<<" "<< state_point.pos.transpose()<<" "<<ext_euler.transpose() << " "<<state_point.offset_T_L_I.transpose()<< " " << state_point.vel.transpose() \
<<" "<<state_point.bg.transpose()<<" "<<state_point.ba.transpose()<<" "<<state_point.grav<< endl;
if(0) // If you need to see map point, change to "if(1)"
{
PointVector ().swap(ikdtree.PCL_Storage);
ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD);
featsFromMap->clear();
featsFromMap->points = ikdtree.PCL_Storage;
}
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();
/*** iterated state estimation ***/
double t_update_start = omp_get_wtime();
double solve_H_time = 0;
// 核心函数
// 观测方程
kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time);
state_point = kf.get_x();
euler_cur = SO3ToEuler(state_point.rot);
// pos_lid:雷达后验位置
// 补偿IMU和雷达之间杆臂，得到雷达的导航系坐标
pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I;
// 转到四元数
geoQuat.x = state_point.rot.coeffs()[0];
geoQuat.y = state_point.rot.coeffs()[1];
geoQuat.z = state_point.rot.coeffs()[2];
geoQuat.w = state_point.rot.coeffs()[3];
double t_update_end = omp_get_wtime();
/******* Publish odometry *******/
publish_odometry(pubOdomAftMapped);
/*** add the feature points to map kdtree ***/
t3 = omp_get_wtime();
map_incremental();
t5 = omp_get_wtime();
/******* Publish points *******/
if (path_en) publish_path(pubPath);
if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFull);
if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFull_body);
// publish_effect_world(pubLaserCloudEffect);
// publish_map(pubLaserCloudMap);
/*** Debug variables ***/
if (runtime_pos_log)
{
frame_num ++;
kdtree_size_end = ikdtree.size();
aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num;
aver_time_icp = aver_time_icp * (frame_num - 1)/frame_num + (t_update_end - t_update_start) / frame_num;
aver_time_match = aver_time_match * (frame_num - 1)/frame_num + (match_time)/frame_num;
aver_time_incre = aver_time_incre * (frame_num - 1)/frame_num + (kdtree_incremental_time)/frame_num;
aver_time_solve = aver_time_solve * (frame_num - 1)/frame_num + (solve_time + solve_H_time)/frame_num;
aver_time_const_H_time = aver_time_const_H_time * (frame_num - 1)/frame_num + solve_time / frame_num;
T1[time_log_counter] = Measures.lidar_beg_time;
s_plot[time_log_counter] = t5 - t0;
s_plot2[time_log_counter] = feats_undistort->points.size();
s_plot3[time_log_counter] = kdtree_incremental_time;
s_plot4[time_log_counter] = kdtree_search_time;
s_plot5[time_log_counter] = kdtree_delete_counter;
s_plot6[time_log_counter] = kdtree_delete_time;
s_plot7[time_log_counter] = kdtree_size_st;
s_plot8[time_log_counter] = kdtree_size_end;
s_plot9[time_log_counter] = aver_time_consu;
s_plot10[time_log_counter] = add_point_size;
time_log_counter ++;
printf("[ mapping ]: time: IMU + Map + Input Downsample: %0.6f ave match: %0.6f ave solve: %0.6f ave ICP: %0.6f map incre: %0.6f ave total: %0.6f icp: %0.6f construct H: %0.6f \n",t1-t0,aver_time_match,aver_time_solve,t3-t1,t5-t3,aver_time_consu,aver_time_icp, aver_time_const_H_time);
ext_euler = SO3ToEuler(state_point.offset_R_L_I);
fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_point.pos.transpose()<< " " << ext_euler.transpose() << " "<<state_point.offset_T_L_I.transpose()<<" "<< state_point.vel.transpose() \
<<" "<<state_point.bg.transpose()<<" "<<state_point.ba.transpose()<<" "<<state_point.grav<<" "<<feats_undistort->points.size()<<endl;
dump_lio_state_to_log(fp);
}
}
status = ros::ok();
rate.sleep(); // 通过睡眠度过循环中剩余的时间
}
/**************** save map ****************/
/* 1. make sure you have enough memories
/* 2. pcd save will largely influence the real-time performences **/
if (pcl_wait_save->size() > 0 && pcd_save_en)
{
string file_name = string("scans.pcd");
string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name);
pcl::PCDWriter pcd_writer;
cout << "current scan saved to /PCD/" << file_name<<endl;
pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
}
fout_out.close();
fout_pre.close();
if (runtime_pos_log)
{
vector<double> t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7;
FILE *fp2;
string log_dir = root_dir + "/Log/fast_lio_time_log.csv";
fp2 = fopen(log_dir.c_str(),"w");
fprintf(fp2,"time_stamp, total time, scan point size, incremental time, search time, delete size, delete time, tree size st, tree size end, add point size, preprocess time\n");
for (int i = 0;i<time_log_counter; i++){
fprintf(fp2,"%0.8f,%0.8f,%d,%0.8f,%0.8f,%d,%0.8f,%d,%d,%d,%0.8f\n",T1[i],s_plot[i],int(s_plot2[i]),s_plot3[i],s_plot4[i],int(s_plot5[i]),s_plot6[i],int(s_plot7[i]),int(s_plot8[i]), int(s_plot10[i]), s_plot11[i]);
t.push_back(T1[i]);
s_vec.push_back(s_plot9[i]);
s_vec2.push_back(s_plot3[i] + s_plot6[i]);
s_vec3.push_back(s_plot4[i]);
s_vec5.push_back(s_plot[i]);
}
fclose(fp2);
}
return 0;
}
#include <cmath>
#include <math.h>
#include <deque>
#include <mutex>
#include <thread>
#include <fstream>
#include <csignal>
#include <ros/ros.h>
#include <so3_math.h>
#include <Eigen/Eigen>
#include <common_lib.h>
#include <pcl/common/io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <condition_variable>
#include <nav_msgs/Odometry.h>
#include <pcl/common/transforms.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <tf/transform_broadcaster.h>
#include <eigen_conversions/eigen_msg.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/Vector3.h>
#include "use-ikfom.hpp"
/// *************Preconfiguration
#define MAX_INI_COUNT (10)
const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);};
/// *************IMU Process and undistortion
class ImuProcess
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ImuProcess();
~ImuProcess();
void Reset();
void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu);
void set_extrinsic(const V3D &transl, const M3D &rot);
void set_extrinsic(const V3D &transl);
void set_extrinsic(const MD(4,4) &T);
void set_gyr_cov(const V3D &scaler);
void set_acc_cov(const V3D &scaler);
void set_gyr_bias_cov(const V3D &b_g);
void set_acc_bias_cov(const V3D &b_a);
Eigen::Matrix<double, 12, 12> Q;
void Process(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI::Ptr pcl_un_);
ofstream fout_imu;
V3D cov_acc; // 加速度方差
V3D cov_gyr; // 陀螺仪方差
V3D cov_acc_scale;
V3D cov_gyr_scale;
V3D cov_bias_gyr;
V3D cov_bias_acc;
double first_lidar_time; //一帧雷达数据的开始时间戳
private:
void IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N);
void UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI &pcl_in_out);
PointCloudXYZI::Ptr cur_pcl_un_; // 没有去除畸变之前的雷达数据
sensor_msgs::ImuConstPtr last_imu_; // 一帧雷达数据对应的最后一个imu数据
deque<sensor_msgs::ImuConstPtr> v_imu_; //IMU数据队列
vector<Pose6D> IMUpose; // 一帧激光雷达时间区间内的imu位姿（前向预测过程）
vector<M3D> v_rot_pcl_;
M3D Lidar_R_wrt_IMU; //雷达和IMU之间的安装角
V3D Lidar_T_wrt_IMU; //雷达和IMU之间的旋转矩阵
V3D mean_acc; // 求平均，加速度零偏
V3D mean_gyr; // 求平均，陀螺仪零偏
V3D angvel_last; // 去除零偏之后的角速度
V3D acc_s_last; // 导航系坐标系的加速度
double start_timestamp_;
double last_lidar_end_time_; // 一帧雷达点的最后时间戳
int init_iter_num = 1; // 计算陀螺仪零偏需要的IMU个数
bool b_first_frame_ = true; // 第一帧IMU数据
bool imu_need_init_ = true; // imu没有完成初始化
};
ImuProcess::ImuProcess()
: b_first_frame_(true), imu_need_init_(true), start_timestamp_(-1)
{
init_iter_num = 1;
Q = process_noise_cov();
cov_acc = V3D(0.1, 0.1, 0.1);
cov_gyr = V3D(0.1, 0.1, 0.1);
cov_bias_gyr = V3D(0.0001, 0.0001, 0.0001);
cov_bias_acc = V3D(0.0001, 0.0001, 0.0001);
mean_acc = V3D(0, 0, -1.0);
mean_gyr = V3D(0, 0, 0);
angvel_last = Zero3d;
Lidar_T_wrt_IMU = Zero3d;
Lidar_R_wrt_IMU = Eye3d;
last_imu_.reset(new sensor_msgs::Imu());
}
ImuProcess::~ImuProcess() {}
void ImuProcess::Reset()
{
// ROS_WARN("Reset ImuProcess");
mean_acc = V3D(0, 0, -1.0); // 平均比力
mean_gyr = V3D(0, 0, 0);
angvel_last = Zero3d;
imu_need_init_ = true; // 还没有初始化，需要初始化
start_timestamp_ = -1;
init_iter_num = 1;
v_imu_.clear();
IMUpose.clear();
last_imu_.reset(new sensor_msgs::Imu());
cur_pcl_un_.reset(new PointCloudXYZI());
}
void ImuProcess::set_extrinsic(const MD(4,4) &T)
{
Lidar_T_wrt_IMU = T.block<3,1>(0,3);
Lidar_R_wrt_IMU = T.block<3,3>(0,0);
}
void ImuProcess::set_extrinsic(const V3D &transl)
{
Lidar_T_wrt_IMU = transl;
Lidar_R_wrt_IMU.setIdentity();
}
// 设置外参
void ImuProcess::set_extrinsic(const V3D &transl, const M3D &rot)
{
Lidar_T_wrt_IMU = transl;
Lidar_R_wrt_IMU = rot;
}
// 设置陀螺仪噪声
void ImuProcess::set_gyr_cov(const V3D &scaler)
{
cov_gyr_scale = scaler;
}
// 设置加表噪声
void ImuProcess::set_acc_cov(const V3D &scaler)
{
cov_acc_scale = scaler;
}
// 设置陀螺仪零偏
void ImuProcess::set_gyr_bias_cov(const V3D &b_g)
{
cov_bias_gyr = b_g;
}
// 设置加表零偏
void ImuProcess::set_acc_bias_cov(const V3D &b_a)
{
cov_bias_acc = b_a;
}
// 初始化滤波器状态变量，以及初始协方差矩阵
// IMU初始化，需要静止
// meas：激光雷达，IMU数据集合
// 计算陀螺仪零偏
void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N)
{
/** 1. initializing the gravity, gyro bias, acc and gyro covariance
** 2. normalize the acceleration measurenments to unit gravity **/
V3D cur_acc, cur_gyr;
if (b_first_frame_)
{
Reset();
N = 1;
b_first_frame_ = false;
const auto &imu_acc = meas.imu.front()->linear_acceleration; // 线加速度
const auto &gyr_acc = meas.imu.front()->angular_velocity; // 角速度
mean_acc << imu_acc.x, imu_acc.y, imu_acc.z;
mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
first_lidar_time = meas.lidar_beg_time; // 雷达开始时间
}
for (const auto &imu : meas.imu)
{
const auto &imu_acc = imu->linear_acceleration;
const auto &gyr_acc = imu->angular_velocity;
cur_acc << imu_acc.x, imu_acc.y, imu_acc.z;
cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
// 迭代方式求平均
mean_acc += (cur_acc - mean_acc) / N;
mean_gyr += (cur_gyr - mean_gyr) / N;
// cwiseProduct??
// 方差
cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N);
cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) * (N - 1.0) / (N * N);
// cout<<"acc norm: "<<cur_acc.norm()<<" "<<mean_acc.norm()<<endl;
N ++;
}
state_ikfom init_state = kf_state.get_x();
// 归一化重力向量： - mean_acc / mean_acc.norm()
// 防止加速度数据标度因子的影响：G_m_s2：9.81
// S2？
init_state.grav = S2(- mean_acc / mean_acc.norm() * G_m_s2); // 重力向量
//state_inout.rot = Eye3d; // Exp(mean_acc.cross(V3D(0, 0, -1 / scale_gravity)));
init_state.bg = mean_gyr; // 陀螺零偏
init_state.offset_T_L_I = Lidar_T_wrt_IMU;
init_state.offset_R_L_I = Lidar_R_wrt_IMU;
kf_state.change_x(init_state);
// 协方差注意单位
esekfom::esekf<state_ikfom, 12, input_ikfom>::cov init_P = kf_state.get_P();
init_P.setIdentity();
init_P(6,6) = init_P(7,7) = init_P(8,8) = 0.00001; // IMU和雷达安装角
init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001; // IMU和雷达杆臂
init_P(15,15) = init_P(16,16) = init_P(17,17) = 0.0001; // 陀螺仪零偏
init_P(18,18) = init_P(19,19) = init_P(20,20) = 0.001; // 加表零偏
init_P(21,21) = init_P(22,22) = 0.00001; // 重力向量
kf_state.change_P(init_P);
last_imu_ = meas.imu.back();
}
// 激光雷达去畸变
// 设置系统噪声
// 滤波预测过程
void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI &pcl_out)
{
/*** add the imu of the last frame-tail to the of current frame-head ***/
auto v_imu = meas.imu;
// push_front：从队列最前面插入
v_imu.push_front(last_imu_);
const double &imu_beg_time = v_imu.front()->header.stamp.toSec();
const double &imu_end_time = v_imu.back()->header.stamp.toSec();
const double &pcl_beg_time = meas.lidar_beg_time;
const double &pcl_end_time = meas.lidar_end_time;
/*** sort point clouds by offset time ***/
pcl_out = *(meas.lidar);
// 按照排序？
sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);
// cout<<"[ IMU Process ]: Process lidar from "<<pcl_beg_time<<" to "<<pcl_end_time<<", " \
// <<meas.imu.size()<<" imu msgs from "<<imu_beg_time<<" to "<<imu_end_time<<endl;
/*** Initialize IMU pose ***/
state_ikfom imu_state = kf_state.get_x();
IMUpose.clear();
IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));
//前向过程
/*** forward propagation at each imu point ***/
V3D angvel_avr, acc_avr, acc_imu, vel_imu, pos_imu;
M3D R_imu;
double dt = 0;
input_ikfom in;
for (auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++)
{
auto &&head = *(it_imu); // &&？
auto &&tail = *(it_imu + 1);
if (tail->header.stamp.toSec() < last_lidar_end_time_) continue;
// 取平均值
// 角速度平均值
angvel_avr<<0.5 * (head->angular_velocity.x + tail->angular_velocity.x),
0.5 * (head->angular_velocity.y + tail->angular_velocity.y),
0.5 * (head->angular_velocity.z + tail->angular_velocity.z);
// 加速度平均值
acc_avr <<0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x),
0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y),
0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z);
// fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl;
// 防止加速度数据标度因子的影响
acc_avr = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba;
if(head->header.stamp.toSec() < last_lidar_end_time_)
{
dt = tail->header.stamp.toSec() - last_lidar_end_time_;
// dt = tail->header.stamp.toSec() - pcl_beg_time;
}
else
{
dt = tail->header.stamp.toSec() - head->header.stamp.toSec();
}
in.acc = acc_avr; // 加速度计平均值
in.gyro = angvel_avr; // 陀螺仪平均值
// 设置系统噪声
// Q:12 * 12
Q.block<3, 3>(0, 0).diagonal() = cov_gyr;
Q.block<3, 3>(3, 3).diagonal() = cov_acc;
Q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr;
Q.block<3, 3>(9, 9).diagonal() = cov_bias_acc;
kf_state.predict(dt, Q, in); // 滤波预测过程
/* save the poses at each IMU measurements */
imu_state = kf_state.get_x();
angvel_last = angvel_avr - imu_state.bg;
acc_s_last = imu_state.rot * (acc_avr - imu_state.ba);// 加速度转到导航系
for(int i=0; i<3; i++)
{
acc_s_last[i] += imu_state.grav[i]; // 计算导航系比力
}
// offs_t:畸变时间
double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time;
IMUpose.push_back(set_pose6d(offs_t, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));
}
/*** calculated the pos and attitude prediction at the frame-end ***/
double note = pcl_end_time > imu_end_time ? 1.0 : -1.0;
dt = note * (pcl_end_time - imu_end_time); // dt > 0
kf_state.predict(dt, Q, in);// 滤波预测过程
imu_state = kf_state.get_x();
last_imu_ = meas.imu.back();
last_lidar_end_time_ = pcl_end_time;
// 反向过程
/*** undistort each lidar point (backward propagation) ***/
if (pcl_out.points.begin() == pcl_out.points.end()) return;
auto it_pcl = pcl_out.points.end() - 1;
// 对于每一个imu位姿，找到一个对应的雷达位姿，该雷达时间戳比imu时间点大，并且满足最邻近，
// 去求该雷达点时间戳所对应的位姿
for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--)
{
auto head = it_kp - 1;
auto tail = it_kp;
R_imu<<MAT_FROM_ARRAY(head->rot);
// cout<<"head imu acc: "<<acc_imu.transpose()<<endl;
vel_imu<<VEC_FROM_ARRAY(head->vel);
pos_imu<<VEC_FROM_ARRAY(head->pos);
acc_imu<<VEC_FROM_ARRAY(tail->acc);
angvel_avr<<VEC_FROM_ARRAY(tail->gyr);
// 遍历一帧中的每一个雷达点
// curvature:相对于一帧中第一个雷达点的时间
// double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time！！！
// curvature =
// 由imu时间戳位置计算雷达时间戳位姿
for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --)
{
dt = it_pcl->curvature / double(1000) - head->offset_time; // dt > 0
/* Transform to the 'end' frame, using only the rotation
* Note: Compensation direction is INVERSE of Frame's moving direction
* So if we want to compensate a point at timestamp-i to the frame-e
* P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */
M3D R_i(R_imu * Exp(angvel_avr, dt)); // 最新的转换矩阵（IMU到导航坐标系）
V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z); // 没有去除畸变的激光雷达坐标（雷达坐标系）
V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos);
// offset_T_L_I：IMU和雷达之间的杆臂
// offset_R_L_I：IMU和雷达之间的安装角
// imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I:转到IMU坐标系
// (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei)：
//
V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate!
// save Undistorted points and their rotation
// 去除畸变之后的激光雷达点云
it_pcl->x = P_compensate(0);
it_pcl->y = P_compensate(1);
it_pcl->z = P_compensate(2);
if (it_pcl == pcl_out.points.begin()) break;
}
}
}
// feats_undistort:运动畸变去除之后的点云数据
// 对IMU数据进行预处理，其中包含了点云畸变处理 前向传播 反向传播
void ImuProcess::Process(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI::Ptr cur_pcl_un_)
{
double t1,t2,t3;
t1 = omp_get_wtime();
if(meas.imu.empty()) {return;};
ROS_ASSERT(meas.lidar != nullptr);
// 初始化，执行一次
if (imu_need_init_)
{
/// The very first lidar frame
// init_iter_num：计算陀螺仪零偏需要的IMU个数
IMU_init(meas, kf_state, init_iter_num);
imu_need_init_ = true;
last_imu_ = meas.imu.back();
state_ikfom imu_state = kf_state.get_x();
// MAX_INI_COUNT：10
if (init_iter_num > MAX_INI_COUNT)
{
// G_m_s2：广东重力加速度
// norm：范数
cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2); // ？
imu_need_init_ = false;
// cov_acc覆盖？？
cov_acc = cov_acc_scale; // 设置加表噪声
cov_gyr = cov_gyr_scale; // 设置陀螺仪噪声
ROS_INFO("IMU Initial Done");
// ROS_INFO("IMU Initial Done: Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\
// imu_state.grav[0], imu_state.grav[1], imu_state.grav[2], mean_acc.norm(), cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]);
fout_imu.open(DEBUG_FILE_DIR("imu.txt"),ios::out);
}
return;
}
UndistortPcl(meas, kf_state, *cur_pcl_un_);
t2 = omp_get_wtime();
t3 = omp_get_wtime();
// cout<<"[ IMU Process ]: Time: "<<t3 - t1<<endl;
}
#ifndef USE_IKFOM_H
#define USE_IKFOM_H
#include <IKFoM_toolkit/esekfom/esekfom.hpp>
typedef MTK::vect<3, double> vect3;
typedef MTK::SO3<double> SO3;
typedef MTK::S2<double, 98090, 10000, 1> S2;
typedef MTK::vect<1, double> vect1;
typedef MTK::vect<2, double> vect2;
// 状态向量（22维）
// pos：位置
// rot：欧拉角
// offset_R_L_I：IMU和激光雷达之间的安装角（旋转矩阵）
// offset_T_L_I：IMU和激光雷达之间的杆臂
// vel：速度
// bg：陀螺仪零偏
// ba：加速度计零偏
// grav：重力向量
MTK_BUILD_MANIFOLD(state_ikfom,
((vect3, pos))
((SO3, rot))
((SO3, offset_R_L_I))
((vect3, offset_T_L_I))
((vect3, vel))
((vect3, bg))
((vect3, ba))
((S2, grav))
);
// 输入数据：加速度，陀螺仪
// 增量形式？
MTK_BUILD_MANIFOLD(input_ikfom,
((vect3, acc))
((vect3, gyro))
);
// IMU噪声
MTK_BUILD_MANIFOLD(process_noise_ikfom,
((vect3, ng))
((vect3, na))
((vect3, nbg))
((vect3, nba))
);
// 设置系统噪声
MTK::get_cov<process_noise_ikfom>::type process_noise_cov()
{
MTK::get_cov<process_noise_ikfom>::type cov = MTK::get_cov<process_noise_ikfom>::type::Zero();
MTK::setDiagonal<process_noise_ikfom, vect3, 0>(cov, &process_noise_ikfom::ng, 0.0001);// 0.03
MTK::setDiagonal<process_noise_ikfom, vect3, 3>(cov, &process_noise_ikfom::na, 0.0001); // *dt 0.01 0.01 * dt * dt 0.05
MTK::setDiagonal<process_noise_ikfom, vect3, 6>(cov, &process_noise_ikfom::nbg, 0.00001); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01
MTK::setDiagonal<process_noise_ikfom, vect3, 9>(cov, &process_noise_ikfom::nba, 0.00001); //0.001 0.05 0.0001/out 0.01
return cov;
}
//double L_offset_to_I[3] = {0.04165, 0.02326, -0.0284}; // Avia
//vect3 Lidar_offset_to_IMU(L_offset_to_I, 3);
// 函数功能：求系统微分方程（系统转换矩阵）
// 备注：没有陀螺仪随机游走，以及加速度随机游走
Eigen::Matrix<double, 24, 1> get_f(state_ikfom &s, const input_ikfom &in)
{
Eigen::Matrix<double, 24, 1> res = Eigen::Matrix<double, 24, 1>::Zero();
vect3 omega;
// in.gyro：输入的陀螺仪数据
in.gyro.boxminus(omega, s.bg); // 陀螺仪数据减去零偏
// in.acc：输入的加速度数据
// 加速度数据转到导航坐标系
vect3 a_inertial = s.rot * (in.acc-s.ba); // 加速度数据减去零偏
for(int i = 0; i < 3; i++ ){
res(i) = s.vel[i]; // 位置微分方程（00-02:位置）
res(i + 3) = omega[i]; // 欧拉角微分方程（03-05:欧拉角）
// 消除重力加速度
res(i + 12) = a_inertial[i] + s.grav[i]; // 速度微分方程（12-15:速度）
}
return res;
}
// 求雅可比矩阵：系统转换矩阵的雅可比矩阵
// 参考论文《FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter》，公式（7）
// 状态变量：
// 00-02:位置
// 03-05:欧拉角
// 06-08:IMU和激光雷达之间的安装角（旋转矩阵）
// 09-11:IMU和激光雷达之间的杆臂
// 12-15:速度
// 15-17:陀螺仪零偏
// 18-20:加速度零偏
// 21-23:重力向量
// 备注：求偏导（分子->行号,分母->列号）
Eigen::Matrix<double, 24, 23> df_dx(state_ikfom &s, const input_ikfom &in)
{
Eigen::Matrix<double, 24, 23> cov = Eigen::Matrix<double, 24, 23>::Zero();
cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); // 位置对速度
vect3 acc_;
in.acc.boxminus(acc_, s.ba); // 加速度计数据减去零偏
vect3 omega;
in.gyro.boxminus(omega, s.bg); // 陀螺仪数据减去零偏
//
cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix()*MTK::hat(acc_); //速度对欧拉角
cov.template block<3, 3>(12, 18) = -s.rot.toRotationMatrix(); // 速度对加速度零偏
Eigen::Matrix<state_ikfom::scalar, 2, 1> vec = Eigen::Matrix<state_ikfom::scalar, 2, 1>::Zero();
Eigen::Matrix<state_ikfom::scalar, 3, 2> grav_matrix;
// grav_matrix:0
s.S2_Mx(grav_matrix, vec, 21);
cov.template block<3, 2>(12, 21) = grav_matrix; // 速度对
// 以下公式和论文不一致
cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity(); // 欧拉角对陀螺仪零偏？？
return cov;
}
// 求雅可比矩阵：噪声转换矩阵的雅可比矩阵
// 参考论文《FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter》，公式（7）
// 设定固定不变的值
// 状态变量：
// 00-02:位置
// 03-05:欧拉角
// 06-08:IMU和激光雷达之间的安装角（旋转矩阵）
// 09-11:IMU和激光雷达之间的杆臂
// 12-15:速度
// 15-17:陀螺仪零偏
// 18-20:加速度零偏
// 21-23:重力向量
Eigen::Matrix<double, 24, 12> df_dw(state_ikfom &s, const input_ikfom &in)
{
Eigen::Matrix<double, 24, 12> cov = Eigen::Matrix<double, 24, 12>::Zero();
cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix(); // 速度对加速度零偏的偏导
cov.template block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity(); // 欧拉角对陀螺仪零偏的偏导
cov.template block<3, 3>(15, 6) = Eigen::Matrix3d::Identity(); // 陀螺零偏对陀螺仪随机游走噪声
cov.template block<3, 3>(18, 9) = Eigen::Matrix3d::Identity(); // 加速度零偏对加速度计随机游走噪声
return cov;
}
// 旋转矩阵转欧拉角：噪声转换矩阵的雅可比矩阵
vect3 SO3ToEuler(const SO3 &orient)
{
Eigen::Matrix<double, 3, 1> _ang;
Eigen::Vector4d q_data = orient.coeffs().transpose();
//scalar w=orient.coeffs[3], x=orient.coeffs[0], y=orient.coeffs[1], z=orient.coeffs[2];
double sqw = q_data[3]*q_data[3];
double sqx = q_data[0]*q_data[0];
double sqy = q_data[1]*q_data[1];
double sqz = q_data[2]*q_data[2];
double unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise is correction factor
double test = q_data[3]*q_data[1] - q_data[2]*q_data[0];
if (test > 0.49999*unit) { // singularity at north pole
_ang << 2 * std::atan2(q_data[0], q_data[3]), M_PI/2, 0;
double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3};
vect3 euler_ang(temp, 3);
return euler_ang;
}
if (test < -0.49999*unit) { // singularity at south pole
_ang << -2 * std::atan2(q_data[0], q_data[3]), -M_PI/2, 0;
double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3};
vect3 euler_ang(temp, 3);
return euler_ang;
}
_ang <<
std::atan2(2*q_data[0]*q_data[3]+2*q_data[1]*q_data[2] , -sqx - sqy + sqz + sqw),
std::asin (2*test/unit),
std::atan2(2*q_data[2]*q_data[3]+2*q_data[1]*q_data[0] , sqx - sqy - sqz + sqw);
double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3};
vect3 euler_ang(temp, 3);
// euler_ang[0] = roll, euler_ang[1] = pitch, euler_ang[2] = yaw
return euler_ang;
}
#endif