#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"
#include "preprocess.h"
/// *************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;
int lidar_type;
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_;
deque<sensor_msgs::ImuConstPtr> v_imu_;
vector<Pose6D> IMUpose;
vector<M3D> v_rot_pcl_;
M3D Lidar_R_wrt_IMU;
V3D Lidar_T_wrt_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;
bool b_first_frame_ = true;
bool imu_need_init_ = true;
};
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());
}
ImuProcess 类解析
1. 文件功能
imu.preprocess.cpp 定义了 ImuProcess 类及其功能。该类负责：
1. IMU 数据处理：
• 初始化、标定和噪声建模。
2. 点云去畸变：
• 基于 IMU 数据对点云进行运动补偿和时间同步。
3. 协助 EKF 初始化：
• 提供初始的加速度和角速度均值，辅助状态估计器初始化。
2. 代码逐段解析
（1）引入头文件
#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"
#include "preprocess.h"
• 主要头文件：
• PCL：点云处理库。
• Eigen：矩阵运算库，用于姿态、旋转矩阵和向量处理。
• ROS：用于点云和 IMU 消息的发布与订阅。
• so3_math.h：用于处理旋转矩阵的数学操作。
• use-ikfom.hpp：用于接入 IKFOM 扩展卡尔曼滤波器。
（2）宏定义
#define MAX_INI_COUNT (10)
const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);};
• 作用：
• MAX_INI_COUNT：定义 EKF 初始化所需的最大迭代次数。
• time_list：比较两个点的曲率值，用于排序操作。
3. ImuProcess 类定义
（1）类成员变量
公有成员
Eigen::Matrix<double, 12, 12> Q;
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;
int lidar_type;
• 作用：
• Q：IMU 的过程噪声协方差矩阵，用于建模传感器误差。
• cov_*：各种噪声参数，包括加速度噪声、角速度噪声、零偏噪声。
• first_lidar_time：记录第一个激光雷达数据的时间戳，用于时间同步。
• lidar_type：记录激光雷达的类型，用于数据格式适配。
私有成员
PointCloudXYZI::Ptr cur_pcl_un_;
sensor_msgs::ImuConstPtr last_imu_;
deque<sensor_msgs::ImuConstPtr> v_imu_;
vector<Pose6D> IMUpose;
vector<M3D> v_rot_pcl_;
M3D Lidar_R_wrt_IMU;
V3D Lidar_T_wrt_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;
bool b_first_frame_;
bool imu_need_init_;
• 说明：
• cur_pcl_un_：当前去畸变后的点云。
• last_imu_：上一帧 IMU 数据。
• v_imu_：IMU 数据队列，用于去畸变时的插值。
• IMUpose：IMU 的姿态信息序列。
• Lidar_R_wrt_IMU / Lidar_T_wrt_IMU：雷达到 IMU 的外参（旋转和平移）。
• mean_acc / mean_gyr：初始化阶段计算的加速度和角速度均值。
• start_timestamp_：IMU 数据处理的起始时间戳。
• imu_need_init_：标记是否需要重新初始化 IMU。
（2）类构造与析构函数
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() {}
• 作用：
• 构造函数中对所有成员变量进行初始化：
• cov_acc / cov_gyr：初始化加速度和角速度噪声。
• mean_acc：设置默认重力方向。
• Lidar_T_wrt_IMU / Lidar_R_wrt_IMU：初始化雷达与 IMU 的外参为单位矩阵。
• last_imu_：初始化空的 IMU 数据指针。
• 析构函数无特定操作。
（3）Reset 函数
void ImuProcess::Reset()
{
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());
}
• 作用：
• 重置 IMU 处理器的状态，清空所有缓存。
• 主要用于重新初始化或处理数据异常时。
• 详细说明：
• 重置均值和偏差：
• 重置加速度和角速度的均值。
• 清除之前的偏差信息。
• 重置队列和缓存：
• 清空 IMU 数据队列和点云缓存。
• 初始化标志：
• 标记需要重新初始化 imu_need_init_。
4. 小结
核心功能
• ImuProcess 类：
• 提供 IMU 数据的初始化、处理和去畸变功能。
• 为点云的运动补偿提供所需的姿态信息。
• 关键变量：
• 外参（Lidar_R_wrt_IMU / Lidar_T_wrt_IMU）实现雷达和 IMU 数据的对齐。
• 噪声协方差（cov_*）用于优化传感器模型。
代码整体逻辑
• 初始化阶段：
• 通过构造函数设置默认参数。
• 重置功能：
• 通过 Reset 函数清空状态，重新开始数据处理。
• 外参设定：
• 通过成员变量存储雷达和 IMU 的空间关系。
该部分代码为 IMU 与点云数据的深度融合（如去畸变、同步）提供了基础支持，是 FastLIO 系统的重要组成部分。
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;
}
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;
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();
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;
init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001;
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;
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();
double pcl_beg_time = meas.lidar_beg_time;
double pcl_end_time = meas.lidar_end_time;
if (lidar_type == MARSIM) {
pcl_beg_time = last_lidar_end_time_;
pcl_end_time = meas.lidar_beg_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.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];
}
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);
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;
if(lidar_type != MARSIM){
auto it_pcl = pcl_out.points.end() - 1;
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);
for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --)
{
dt = it_pcl->curvature / double(1000) - head->offset_time;
/* 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));
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);
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;
}
}
}
}
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
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();
if (init_iter_num > MAX_INI_COUNT)
{
cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2);
imu_need_init_ = false;
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;
}
ImuProcess 类后半部分解析
1. 文件功能
这一部分实现了 ImuProcess 类的核心功能，包括：
1. IMU外参设置：
• 通过不同接口设置雷达与IMU之间的外参（旋转和平移关系）。
2. 噪声参数设置：
• 配置IMU的噪声模型，包括加速度、角速度及零偏噪声。
3. IMU初始化：
• 计算IMU的初始状态，包括重力方向、零偏和噪声协方差。
4. 点云去畸变：
• 基于IMU数据对点云进行运动补偿。
5. IMU处理主流程：
• 整合IMU数据与点云，实现初始状态估计和点云畸变校正。
2. 代码逐段解析
（1）IMU 外参设置
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;
}
• 作用：
• 通过不同接口设置雷达到IMU的外参。
• 外参格式：
• 旋转矩阵（Lidar_R_wrt_IMU）。
• 平移向量（Lidar_T_wrt_IMU）。
• 参数说明：
• MD(4,4) &T：4×4齐次变换矩阵，包含旋转和平移。
• V3D &transl：仅平移向量。
• V3D &transl, M3D &rot：平移向量和旋转矩阵。
（2）IMU 噪声设置
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的噪声参数。
• 包括：
• cov_gyr_scale：角速度噪声。
• cov_acc_scale：加速度噪声。
• cov_bias_gyr：角速度零偏噪声。
• cov_bias_acc：加速度零偏噪声。
• 参数说明：
• V3D：三维向量，表示噪声的各轴分量。
（3）IMU 初始化
void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N)
{
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;
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);
N++;
}
state_ikfom init_state = kf_state.get_x();
init_state.grav = S2(- mean_acc / mean_acc.norm() * G_m_s2);
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;
init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001;
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();
}
• 作用：
• 计算 IMU 的初始状态（重力方向、零偏、协方差）。
• 更新 EKF 初始状态。
• 主要步骤：
1. 第一帧初始化：
• 清空缓存，获取初始的加速度和角速度。
2. 均值计算：
• 逐帧累加加速度和角速度，计算加权均值。
3. 协方差计算：
• 根据当前值和均值计算噪声协方差。
4. 设置 EKF 初值：
• 设置重力方向、偏移和外参。
• 初始化状态协方差矩阵。
（4）点云去畸变
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;
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();
double pcl_beg_time = meas.lidar_beg_time;
double pcl_end_time = meas.lidar_end_time;
if (lidar_type == MARSIM) {
pcl_beg_time = last_lidar_end_time_;
pcl_end_time = meas.lidar_beg_time;
}
/*** sort point clouds by offset time ***/
pcl_out = *(meas.lidar);
sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);
/*** 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 ***/
...
// Propagate IMU pose using EKF
...
/*** undistort each lidar point (backward propagation) ***/
if(lidar_type != MARSIM) {
...
// Transform point cloud to the end frame using IMU pose
...
}
}
• 作用：
• 使用 IMU 数据对点云进行去畸变，补偿雷达运动。
• 主要步骤：
1. 时间对齐：
• 将上一帧尾部的 IMU 数据加入当前帧头部。
2. 初始化 IMU 姿态：
• 获取当前时刻的 IMU 状态。
3. IMU 前向传播：
• 根据 IMU 数据更新姿态和速度。
4. 点云去畸变：
• 逐点补偿点云位置，使其对齐到帧尾。
（5）IMU 数据处理主流程
void ImuProcess::Process(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI::Ptr cur_pcl_un_)
{
if(meas.imu.empty()) { return; }
ROS_ASSERT(meas.lidar != nullptr);
if (imu_need_init_) {
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();
if (init_iter_num > MAX_INI_COUNT) {
cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2);
imu_need_init_ = false;
cov_acc = cov_acc_scale;
cov_gyr = cov_gyr_scale;
ROS_INFO("IMU Initial Done");
fout_imu.open(DEBUG_FILE_DIR("imu.txt"), ios::out);
}
return;
}
UndistortPcl(meas, kf_state, *cur_pcl_un_);
}
• 作用：
• 实现 IMU 初始化与点云去畸变的主流程。
• 如果需要初始化：
• 调用 IMU_init()。
• 如果已经初始化：
• 调用 UndistortPcl()。
3. 总结
主要功能
• 设置 IMU 的外参和噪声模型。
• 初始化 IMU 的状态，包括重力、零偏和协方差。
• 使用 IMU 数据对点云进行去畸变。
• 与 EKF 紧密结合，通过预测和更新实现高精度的姿态估计。
关键点
• IMU 初始化是整个流程的关键，通过统计加速度均值确定重力方向。
• 点云去畸变使用 IMU 数据进行双向补偿，确保点云的时间一致性。
• 与 EKF 的深度融合保证了状态估计的准确性和鲁棒性。
这一部分为 FastLIO 的 IMU 模块提供了完整的功能支持，是激光 SLAM 系统中点云预处理和状态估计的重要环节。
扫一扫
1635
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
