1. Toy模板网首页
  2. > Toy博客

Direct LiDAR-Inertial Odometry

10月前 作者：xiaoma_bk 分类：Toy博客 阅读(69) 违法举报

这篇具有很好参考价值的文章主要介绍了Direct LiDAR-Inertial Odometry。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

DLIO

Runing

运行效果：

<video id=“video” controls=""src=“data/dlio_ss.mp4” height=“500”
preload=“none”>

论文

摘要

  • 难点：快速运动 or 穿越不规则地形时降低精度，通常过于简单的方法而过高的计算量。
  • 本方案提出：Direct LiDAR-Inertial Odometry：一种轻量级激光雷达惯性里程计算法，采用新的从粗到精的方法来构建连续时间轨迹以进行精确的运动校正。
    • 去畸变：方程仅通过时间快速且可并行的逐点偏移量进行参数化。
    • 收敛性：IMU预积分 提供正确的状态估计，进行运动畸变和先验于一个步骤，scan-map。

步骤：

  • 首先，我们提出了一种新的从粗到精的技术来构造连续时间轨迹，其中导出了一组具有恒定加加速度和角加速度运动模型的解析方程，用于快速且可并行的逐点运动校正。
  • 其次，提出了一种新颖的压缩架构，它将运动校正和先验构造结合到一个步骤中，并直接执行扫描到地图配准，显着减少了算法的整体计算开销。
  • 第三，我们利用一种新的非线性几何观察器[10]，它具有强大的性能保证（对于实现前两个贡献至关重要），以最小的计算复杂度稳健地生成机器人完整状态的准确估计。B. T. Lopez, “A contracting hierarchical observer for pose-inertial fusion,” arXiv:2303.02777, 2023.
  • 最后，我们的方法的有效性通过使用多个数据集与最新技术进行的广泛实验结果得到验证。

总体框图

Direct LiDAR-Inertial Odometry,自动驾驶,人工智能

  • DLIO 的轻量级架构将运动校正和先前的构造结合到一个步骤中，此外还删除了以前基于 LiDAR 的里程计所需的scan-scan。 W 中的逐点连续时间积分确保了校正点云的最大保真度，并通过基于 GICP 的定制扫描匹配器注册到机器人的地图上。 系统的状态随后由非线性更新具有强收敛特性的几何观察器[10]，并且这些位姿、速度和偏差的估计然后初始化下一次迭代。

相关工作

  • 连续时间方程来查询每个独立激光点的矫正偏移。

  • 我们使用高阶运动模型来表示底层系统动力学，该模型可以捕获高频运动，否则这些运动可能会在尝试将平滑轨迹拟合到一组控制点的方法中丢失。

  • 该方法内置于简化的 LIO 架构中，除了直接执行扫描到地图对齐而无需中间的扫描到扫描之外，还可以一次性执行运动校正和 GICP 优先构建； 这一切都是可能的，因为我们的新型几何观测器具有可证明正确的状态估计的强大收敛保证。

方法：

  • 激光点：在 t k t_k tk​时间，定义 P k P_k Pk​，下标为 k k k，点云 P k P_k Pk​中的点 P k n ∈ R 3 P_k^n \in \mathbb R^3 Pkn​∈R3 与起始时间 Δ t k n ， n = 1 , ⋯   , N \Delta t_k^n，n=1,\cdots,N Δtkn​，n=1,⋯,N，
  • 坐标系：
    • 世界坐标系 W W W
    • 机器人坐标系 R R R ，位于其重心，x向前，y指向左侧，z指向上方。
    • IMU坐标系 B B B
    • 激光坐标系 L L L
  • 机器人状态向量：其中 x ^ {\hat {x}} x^指使用值， x x x 指测量值
    • X k = [ p k W , q k W , v k W , b k a , b k w ] X_k = [p_k^W,q_k^W,v_k^W,b_k^a,b_k^w] Xk​=[pkW​,qkW​,vkW​,bka​,bkw​]
    • a ^ i = ( a i − g ) + b i a + n i a \hat a_i = (a_i -g) + b_i^a + n_i^a a^i​=(ai​−g)+bia​+nia​
    • w ^ i = w i + b i w + n i w \hat w_i = w_i + b_i^w + n_i^w w^i​=wi​+biw​+niw​

预处理

  • 激光(velodyne 10-20hz)，IMU(100-500HZ) ，同步线加速度和角速度。
  • 所有传感器转换到 机器人坐标系 R R R ，外部标定。
  • 对于IMU，如果传感器与重心不重合，则必须考虑位移线性加速度测量对刚体的影响； 这是通过角速度和角速度之间的叉积考虑 R 处线性加速度的所有贡献来完成的。
  • 防止信息丢失不会预处理点云，只会降采样：尝试 角点、边、平面点，或体素滤波。

算法：

  • 算法1 激光里程计

    Direct LiDAR-Inertial Odometry,自动驾驶,人工智能

    使用联合先验的连续时间运动校正

    • 由于旋转激光阵列在扫描过程中在不同时刻收集点，旋转激光雷达传感器的点云在运动过程中会出现运动失真。
    • 我们没有假设扫描过程中的简单运动（即恒定速度）可能无法准确地捕获精细运动，而是使用更精确的持续的急加速和角加速度模型，通过两步粗略到计算每个点的唯一变换 精细传播方案。 该策略旨在最大限度地减少由于 IMU 采样率以及 IMU 和 LiDAR 点测量之间的时间偏移而产生的误差。 整个扫描的轨迹首先通过 IMU 数值积分 [29] 粗略构建，随后通过求解 W 中的一组解析连续时间方程进行细化。
    • 每个点云在局部系 W W W，首先对IMU进行测量进行积分：
      • p ^ i = p ^ i − 1 + v ^ i − 1 Δ t i + 1 2 R ( q ^ i − 1 ) a ^ i − 1 Δ t i 2 + 1 6 j ^ i Δ t i 3 \hat p_i =\hat p_{i-1} + \hat v_{i-1} \Delta t_i + \frac 1 2 R(\hat{q}_{i-1})\hat{a}_{i-1} \Delta t_i^2 + \frac 1 6 \hat j_i \Delta t_i^3 p^​i​=p^​i−1​+v^i−1​Δti​+21​R(q^​i−1​)a^i−1​Δti2​+61​j^​i​Δti3​
      • v ^ i = v ^ i − 1 + R ( q ^ i − 1 ) a ^ i − 1 Δ t i \hat v_i = \hat v_{i-1} + R(\hat{q}_{i-1})\hat{a}_{i-1} \Delta t_i v^i​=v^i−1​+R(q^​i−1​)a^i−1​Δti​
      • q ^ i = q ^ i − 1 + 1 2 ( q ^ i − 1 ⊗ w ^ i − 1 ) Δ t i + 1 4 ( q ^ i − 1 ⊗ α ^ i ) Δ t i 2 \hat q_i = \hat q_{i-1} +\frac 1 2 (\hat q_{i-1} \otimes \hat w_{i-1}) \Delta t_i+\frac 1 4 (\hat q_{i-1} \otimes \hat \alpha_i) \Delta t_i^2 q^​i​=q^​i−1​+21​(q^​i−1​⊗w^i−1​)Δti​+41​(q^​i−1​⊗α^i​)Δti2​
    • 这儿 j ^ i = 1 Δ t i ( R ( q ^ i ) a ^ i − R ( q ^ i − 1 ) a ^ i − 1 ) \hat j_i = \frac 1 {\Delta t_i}(R(\hat{q}_{i})\hat{a}_{i}-R(\hat{q}_{i-1})\hat{a}_{i-1}) j^​i​=Δti​1​(R(q^​i​)a^i​−R(q^​i−1​)a^i−1​) 和 α ^ i = 1 Δ t i ( w ^ i − w ^ i − 1 ) \hat \alpha_i=\frac 1 {\Delta t_i}(\hat w_i - \hat w_{i-1}) α^i​=Δti​1​(w^i​−w^i−1​) 是线加速度和角加速度
    • 连续时间运动校正。 对于云中的每个点，通过求解在最接近的先前 IMU 测量值处初始化的一组封闭式运动方程来计算唯一的变换。 这提供了精确且可并行的连续时间运动或校正。

scan-to-map 匹配

  • GICP 找： $\Delta \hat T_k = \underset {\Delta T_k} {\mathbf {argmin}} \ \varepsilon (\Delta T_k \hat P_k^W, \hat S_k^W) $
    • 这儿 ε \varepsilon ε 被定义 ε ( Δ T k P ^ k W , S ^ k W ) = ∑ c ∈ C d c T ( C k , c S + Δ T k C k , c P Δ T k T ) − 1 d c \varepsilon (\Delta T_k \hat P_k^W, \hat S_k^W) = \underset {c \in C} \sum d_c^T(C_{k,c}^S+\Delta T_k C_{k,c}^P\Delta T_k^T)^{-1}d_c ε(ΔTk​P^kW​,S^kW​)=c∈C∑​dcT​(Ck,cS​+ΔTk​Ck,cP​ΔTkT​)−1dc​
    • 一系列点 C C C 的误差 介于点 P ^ k W \hat P_k^W P^kW​ 和 s ^ k W \hat s_k^W s^kW​： s ^ k W − Δ T k p ^ k c ， p ^ k c ∈ P ^ k W ， s ^ k c ∈ S ^ k W ， ∀ c ∈ C \hat s_k^W-\Delta T_k \hat p_k^c，\hat p_k^c \in \hat P_k^W，\hat s_k^c \in \hat S_k^W，\forall c \in C s^kW​−ΔTk​p^​kc​，p^​kc​∈P^kW​，s^kc​∈S^kW​，∀c∈C
    • C k , c P C_{k,c}^P Ck,cP​ 和 C k , c S C_{k,c}^S Ck,cS​ 是 P ^ k W \hat P_k^W P^kW​ 和 s ^ k W \hat s_k^W s^kW​ 的 协方差矩阵
    • 点到平面的公式 通过正则化协方差 P P P进行面到面的优化矩阵 C k , c P C_{k,c}^P Ck,cP​ 和 C k , c S C_{k,c}^S Ck,cS​ 的 ( 1 , 1 , ε ) (1,1,\varepsilon) (1,1,ε)特征值，其中 ε \varepsilon ε 代表表面法线的不确定性。
    • 所得结果： Δ T ^ k \Delta \hat T_k ΔT^k​ 是最佳矫正变化，该变换使得 激光与子图很好地对其， 因此：
    • Δ T ^ k W = Δ T ^ k T ^ M W \Delta \hat T_k^W = \Delta \hat T_k \hat T_M^W ΔT^kW​=ΔT^k​T^MW​ 是机器人的全局位姿，( T ^ M W \hat T_M^W T^MW​是IMU迭代的最后一个点)。

物理观测

  • T ^ k W \hat T_k^W T^kW​ 计算 scan-to-map 的对齐，融合了IMU测量生成整个状态 X ^ k \hat X_k X^k​，通过一种新怡的分层非线性几何观测。
  • 令 γ e ∈ 1 , ⋯   , 5 \gamma _{e \in {1,\cdots,5}} γe∈1,⋯,5​ 是正常数， Δ t k + \Delta t_k^+ Δtk+​ 是GICP位姿的时间，如果 q e : = ( q e o , q ⃗ e ) = q ^ i ∗ ⊗ q ^ k q_e:=(q_e^o,\vec q_e)=\hat q_i^* \otimes \hat q_k qe​:=(qeo​,q ​e​)=q^​i∗​⊗q^​k​ 和 p e = p ^ k − p ^ i p_e = \hat p_k - \hat p_i pe​=p^​k​−p^​i​ 传播姿态和测量位姿之间的误差。
  • 状态修正：
    • q ^ i = q ^ i + Δ t k + γ 1 q ^ i ⊗ [ 1 − ∣ q e o ∣ s g n ( q e o ) q ⃗ e ] \hat q_i = \hat q_i + \Delta t_k^+ \gamma_1 \hat q_i \otimes \begin{bmatrix}1-|q_e^o| \\ \mathbf{sgn}(q_e^o) \vec q_e\end{bmatrix} q^​i​=q^​i​+Δtk+​γ1​q^​i​⊗[1−∣qeo​∣sgn(qeo​)q ​e​​]
    • b ^ i w = b ^ i w − Δ t k + γ 2 q e o q ⃗ e \hat b_i^w = \hat b_i^w - \Delta t_k^+ \gamma_2 q_e^o \vec q_e b^iw​=b^iw​−Δtk+​γ2​qeo​q ​e​
    • p ^ i = p ^ i + Δ t k + γ 3 P e \hat p_i = \hat p_i + \Delta t_k^+ \gamma_3 P_e p^​i​=p^​i​+Δtk+​γ3​Pe​
    • v ^ i = v ^ i + Δ t k + γ 4 P e \hat v_i = \hat v_i + \Delta t_k^+ \gamma_4 P_e v^i​=v^i​+Δtk+​γ4​Pe​
    • b ^ i α = b ^ i α − Δ t k + γ 5 R ^ ( q ^ i ) T P e \hat b_i^{\alpha} = \hat b_i^{\alpha} - \Delta t_k^+ \gamma_5 \hat R(\hat q_i)^T P_e b^iα​=b^iα​−Δtk+​γ5​R^(q^​i​)TPe​
  • 注意 状态修正 是分层的，因为角度更新（前两个方程）与平移更新（最后三个方程)完全解耦。
    • Direct LiDAR-Inertial Odometry,自动驾驶,人工智能

Code-Odom

成员变量

  • 状态量： State state;
  • 位姿：Pose lidarPose,imuPose
  • 资源消耗相关：cpu类型，所占百分百，上一时刻cpu

类型相关的定义

// 外参 相关
struct Extrinsics {
    struct SE3 {
      Eigen::Vector3f t;
      Eigen::Matrix3f R;
    };
    SE3 baselink2imu;
    SE3 baselink2lidar;
    Eigen::Matrix4f baselink2imu_T;
    Eigen::Matrix4f baselink2lidar_T;
  }; Extrinsics extrinsics;

// Geometric Observer  
struct Geo {
    bool first_opt_done;
    std::mutex mtx;
    double dp;
    double dq_deg;
    Eigen::Vector3f prev_p;
    Eigen::Quaternionf prev_q;
    Eigen::Vector3f prev_vel;
  }; Geo geo;

// imu 的 bias
struct ImuBias {
	Eigen::Vector3f gyro;
	Eigen::Vector3f accel;
};

// 速度->线速度+角速度-> body系+world系
struct Frames {
	Eigen::Vector3f b;
	Eigen::Vector3f w;
};
struct Velocity {
	Frames lin;
	Frames ang;
};

/// 状态量
struct State {
	Eigen::Vector3f p; // position in world frame
	Eigen::Quaternionf q; // orientation in world frame
	Velocity v;
	ImuBias b; // imu biases in body frame
}; State state;

/// 位姿
struct Pose {
	Eigen::Vector3f p; // position in world frame
	Eigen::Quaternionf q; // orientation in world frame
};
Pose lidarPose;
Pose imuPose;

// Metrics
struct Metrics {
	std::vector<float> spaciousness;
	std::vector<float> density;
}; Metrics metrics;

  std::string cpu_type;
  std::vector<double> cpu_percents;
  clock_t lastCPU, lastSysCPU, lastUserCPU;
  int numProcessors;

构造

构造函数

步骤：

  • 读取参数

      • 版本，Frames，ns(获取节点 NS 并删除前导字符)，连接 Frames 名字(ns+Frame)
      • 点云去畸变？、重力大小(9.80665)、关键帧阈值(D：0.1，R：1.0)
      • 子图(knn：10，cv：10，cc：10)
      • 稠密地图降采样？等到运动后发布地图？等
      • 外惨：重力中心到imu、重力中心到lidar
      • IMU参数：标定acc/gyro? 、imu_calib_time(3)、imu_buff(2000)、gravity_align?、imu_calib?、尺度失准矩阵
      • IMU不标定时，标定参数赋值默认值
      • GICP 参数、几何观测参数

  • 参数Reset

    • dlio_initialized ：false、第一帧激光有效帧：false、

  • 订阅：激光+imu、发布：odo+pose+path+kf_pose+kf_cloud+不失真点云

  • 数据初始化

    • T、T_prior、T_corr

    • state

    • imu_means

  • CPU Specs -> /proc/cpuinfo

构造代码：

dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) {
	
  // 读取参数，
  this->getParams();

  this->num_threads_ = omp_get_max_threads();

  this->dlio_initialized = false;
  this->first_valid_scan = false;
  this->first_imu_received = false;
  if (this->imu_calibrate_) {this->imu_calibrated = false;}
  else {this->imu_calibrated = true;}
  this->deskew_status = false;
  this->deskew_size = 0;

  // 订阅激光 和 imu
  this->lidar_sub = this->nh.subscribe("pointcloud", 1,
      &dlio::OdomNode::callbackPointCloud, this, ros::TransportHints().tcpNoDelay());
  this->imu_sub = this->nh.subscribe("imu", 1000,
      &dlio::OdomNode::callbackImu, this, ros::TransportHints().tcpNoDelay());

  this->odom_pub     = this->nh.advertise<nav_msgs::Odometry>("odom", 1, true);
  this->pose_pub     = this->nh.advertise<geometry_msgs::PoseStamped>("pose", 1, true);
  this->path_pub     = this->nh.advertise<nav_msgs::Path>("path", 1, true);
  this->kf_pose_pub  = this->nh.advertise<geometry_msgs::PoseArray>("kf_pose", 1, true);
  this->kf_cloud_pub = this->nh.advertise<sensor_msgs::PointCloud2>("kf_cloud", 1, true);
  this->deskewed_pub = this->nh.advertise<sensor_msgs::PointCloud2>("deskewed", 1, true);

  this->publish_timer = this->nh.createTimer(ros::Duration(0.01), &dlio::OdomNode::publishPose, this);

  this->T = Eigen::Matrix4f::Identity();
  this->T_prior = Eigen::Matrix4f::Identity();
  this->T_corr = Eigen::Matrix4f::Identity();

  this->origin = Eigen::Vector3f(0., 0., 0.);
  this->state.p = Eigen::Vector3f(0., 0., 0.);
  this->state.q = Eigen::Quaternionf(1., 0., 0., 0.);
  this->state.v.lin.b = Eigen::Vector3f(0., 0., 0.);
  this->state.v.lin.w = Eigen::Vector3f(0., 0., 0.);
  this->state.v.ang.b = Eigen::Vector3f(0., 0., 0.);
  this->state.v.ang.w = Eigen::Vector3f(0., 0., 0.);

  this->lidarPose.p = Eigen::Vector3f(0., 0., 0.);
  this->lidarPose.q = Eigen::Quaternionf(1., 0., 0., 0.);

  // imu 数据 均值
  this->imu_meas.stamp = 0.;
  this->imu_meas.ang_vel[0] = 0.;
  this->imu_meas.ang_vel[1] = 0.;
  this->imu_meas.ang_vel[2] = 0.;
  this->imu_meas.lin_accel[0] = 0.;
  this->imu_meas.lin_accel[1] = 0.;
  this->imu_meas.lin_accel[2] = 0.;

  this->imu_buffer.set_capacity(this->imu_buffer_size_);
  this->first_imu_stamp = 0.;
  this->prev_imu_stamp = 0.;

  this->original_scan = pcl::PointCloud<PointType>::ConstPtr (boost::make_shared<const pcl::PointCloud<PointType>>());
  this->deskewed_scan = pcl::PointCloud<PointType>::ConstPtr (boost::make_shared<const pcl::PointCloud<PointType>>());
  this->current_scan = pcl::PointCloud<PointType>::ConstPtr (boost::make_shared<const pcl::PointCloud<PointType>>());
  this->submap_cloud = pcl::PointCloud<PointType>::ConstPtr (boost::make_shared<const pcl::PointCloud<PointType>>());

  this->num_processed_keyframes = 0;

  this->submap_hasChanged = true;
  this->submap_kf_idx_prev.clear();

  this->first_scan_stamp = 0.;
  this->elapsed_time = 0.;
  this->length_traversed;

  this->convex_hull.setDimension(3);
  this->concave_hull.setDimension(3);
  this->concave_hull.setAlpha(this->keyframe_thresh_dist_);
  this->concave_hull.setKeepInformation(true);

  this->gicp.setCorrespondenceRandomness(this->gicp_k_correspondences_);
  this->gicp.setMaxCorrespondenceDistance(this->gicp_max_corr_dist_);
  this->gicp.setMaximumIterations(this->gicp_max_iter_);
  this->gicp.setTransformationEpsilon(this->gicp_transformation_ep_);
  this->gicp.setRotationEpsilon(this->gicp_rotation_ep_);
  this->gicp.setInitialLambdaFactor(this->gicp_init_lambda_factor_);

  this->gicp_temp.setCorrespondenceRandomness(this->gicp_k_correspondences_);
  this->gicp_temp.setMaxCorrespondenceDistance(this->gicp_max_corr_dist_);
  this->gicp_temp.setMaximumIterations(this->gicp_max_iter_);
  this->gicp_temp.setTransformationEpsilon(this->gicp_transformation_ep_);
  this->gicp_temp.setRotationEpsilon(this->gicp_rotation_ep_);
  this->gicp_temp.setInitialLambdaFactor(this->gicp_init_lambda_factor_);

  pcl::Registration<PointType, PointType>::KdTreeReciprocalPtr temp;
  this->gicp.setSearchMethodSource(temp, true);
  this->gicp.setSearchMethodTarget(temp, true);
  this->gicp_temp.setSearchMethodSource(temp, true);
  this->gicp_temp.setSearchMethodTarget(temp, true);

  this->geo.first_opt_done = false;
  this->geo.prev_vel = Eigen::Vector3f(0., 0., 0.);

  pcl::console::setVerbosityLevel(pcl::console::L_ERROR);

  this->crop.setNegative(true);
  this->crop.setMin(Eigen::Vector4f(-this->crop_size_, -this->crop_size_, -this->crop_size_, 1.0));
  this->crop.setMax(Eigen::Vector4f(this->crop_size_, this->crop_size_, this->crop_size_, 1.0));

  this->voxel.setLeafSize(this->vf_res_, this->vf_res_, this->vf_res_);

  this->metrics.spaciousness.push_back(0.);
  this->metrics.density.push_back(this->gicp_max_corr_dist_);

  // CPU Specs
  char CPUBrandString[0x40];
  memset(CPUBrandString, 0, sizeof(CPUBrandString));

  this->cpu_type = "";

  #ifdef HAS_CPUID
  unsigned int CPUInfo[4] = {0,0,0,0};
  __cpuid(0x80000000, CPUInfo[0], CPUInfo[1], CPUInfo[2], CPUInfo[3]);
  unsigned int nExIds = CPUInfo[0];
  for (unsigned int i = 0x80000000; i <= nExIds; ++i) {
    __cpuid(i, CPUInfo[0], CPUInfo[1], CPUInfo[2], CPUInfo[3]);
    if (i == 0x80000002)
      memcpy(CPUBrandString, CPUInfo, sizeof(CPUInfo));
    else if (i == 0x80000003)
      memcpy(CPUBrandString + 16, CPUInfo, sizeof(CPUInfo));
    else if (i == 0x80000004)
      memcpy(CPUBrandString + 32, CPUInfo, sizeof(CPUInfo));
  }
  this->cpu_type = CPUBrandString;
  boost::trim(this->cpu_type);
  #endif

  FILE* file;
  struct tms timeSample;
  char line[128];

  this->lastCPU = times(&timeSample);
  this->lastSysCPU = timeSample.tms_stime;
  this->lastUserCPU = timeSample.tms_utime;

  file = fopen("/proc/cpuinfo", "r");
  this->numProcessors = 0;
  while(fgets(line, 128, file) != nullptr) {
      if (strncmp(line, "processor", 9) == 0) this->numProcessors++;
  }
  fclose(file);

}


void dlio::OdomNode::getParams() {

  // Version
  ros::param::param<std::string>("~dlio/version", this->version_, "0.0.0");

  // Frames
  ros::param::param<std::string>("~dlio/frames/odom", this->odom_frame, "odom");
  ros::param::param<std::string>("~dlio/frames/baselink", this->baselink_frame, "base_link");
  ros::param::param<std::string>("~dlio/frames/lidar", this->lidar_frame, "lidar");
  ros::param::param<std::string>("~dlio/frames/imu", this->imu_frame, "imu");

  // Get Node NS and Remove Leading Character
  std::string ns = ros::this_node::getNamespace();
  ns.erase(0,1);

  // Concatenate Frame Name Strings
  this->odom_frame = ns + "/" + this->odom_frame;
  this->baselink_frame = ns + "/" + this->baselink_frame;
  this->lidar_frame = ns + "/" + this->lidar_frame;
  this->imu_frame = ns + "/" + this->imu_frame;

  // Deskew FLag
  ros::param::param<bool>("~dlio/pointcloud/deskew", this->deskew_, true);

  // Gravity
  ros::param::param<double>("~dlio/odom/gravity", this->gravity_, 9.80665);

  // Keyframe Threshold
  ros::param::param<double>("~dlio/odom/keyframe/threshD", this->keyframe_thresh_dist_, 0.1);
  ros::param::param<double>("~dlio/odom/keyframe/threshR", this->keyframe_thresh_rot_, 1.0);

  // Submap
  ros::param::param<int>("~dlio/odom/submap/keyframe/knn", this->submap_knn_, 10);
  ros::param::param<int>("~dlio/odom/submap/keyframe/kcv", this->submap_kcv_, 10);
  ros::param::param<int>("~dlio/odom/submap/keyframe/kcc", this->submap_kcc_, 10);

  // Dense map resolution
  ros::param::param<bool>("~dlio/map/dense/filtered", this->densemap_filtered_, true);

  // Wait until movement to publish map
  ros::param::param<bool>("~dlio/map/waitUntilMove", this->wait_until_move_, false);

  // Crop Box Filter
  ros::param::param<double>("~dlio/odom/preprocessing/cropBoxFilter/size", this->crop_size_, 1.0);

  // Voxel Grid Filter
  ros::param::param<bool>("~dlio/pointcloud/voxelize", this->vf_use_, true);
  ros::param::param<double>("~dlio/odom/preprocessing/voxelFilter/res", this->vf_res_, 0.05);

  // Adaptive Parameters
  ros::param::param<bool>("~dlio/adaptive", this->adaptive_params_, true);

  // Extrinsics
  std::vector<float> t_default{0., 0., 0.};
  std::vector<float> R_default{1., 0., 0., 0., 1., 0., 0., 0., 1.};

  // center of gravity to imu
  std::vector<float> baselink2imu_t, baselink2imu_R;
  ros::param::param<std::vector<float>>("~dlio/extrinsics/baselink2imu/t", baselink2imu_t, t_default);
  ros::param::param<std::vector<float>>("~dlio/extrinsics/baselink2imu/R", baselink2imu_R, R_default);
  this->extrinsics.baselink2imu.t =
    Eigen::Vector3f(baselink2imu_t[0], baselink2imu_t[1], baselink2imu_t[2]);
  this->extrinsics.baselink2imu.R =
    Eigen::Map<const Eigen::Matrix<float, -1, -1, Eigen::RowMajor>>(baselink2imu_R.data(), 3, 3);

  this->extrinsics.baselink2imu_T = Eigen::Matrix4f::Identity();
  this->extrinsics.baselink2imu_T.block(0, 3, 3, 1) = this->extrinsics.baselink2imu.t;
  this->extrinsics.baselink2imu_T.block(0, 0, 3, 3) = this->extrinsics.baselink2imu.R;

  // center of gravity to lidar
  std::vector<float> baselink2lidar_t, baselink2lidar_R;
  ros::param::param<std::vector<float>>("~dlio/extrinsics/baselink2lidar/t", baselink2lidar_t, t_default);
  ros::param::param<std::vector<float>>("~dlio/extrinsics/baselink2lidar/R", baselink2lidar_R, R_default);

  this->extrinsics.baselink2lidar.t =
    Eigen::Vector3f(baselink2lidar_t[0], baselink2lidar_t[1], baselink2lidar_t[2]);
  this->extrinsics.baselink2lidar.R =
    Eigen::Map<const Eigen::Matrix<float, -1, -1, Eigen::RowMajor>>(baselink2lidar_R.data(), 3, 3);

  this->extrinsics.baselink2lidar_T = Eigen::Matrix4f::Identity();
  this->extrinsics.baselink2lidar_T.block(0, 3, 3, 1) = this->extrinsics.baselink2lidar.t;
  this->extrinsics.baselink2lidar_T.block(0, 0, 3, 3) = this->extrinsics.baselink2lidar.R;

  // IMU
  ros::param::param<bool>("~dlio/odom/imu/calibration/accel", this->calibrate_accel_, true);
  ros::param::param<bool>("~dlio/odom/imu/calibration/gyro", this->calibrate_gyro_, true);
  ros::param::param<double>("~dlio/odom/imu/calibration/time", this->imu_calib_time_, 3.0);
  ros::param::param<int>("~dlio/odom/imu/bufferSize", this->imu_buffer_size_, 2000);

  std::vector<float> accel_default{0., 0., 0.}; std::vector<float> prior_accel_bias;
  std::vector<float> gyro_default{0., 0., 0.}; std::vector<float> prior_gyro_bias;

  ros::param::param<bool>("~dlio/odom/imu/approximateGravity", this->gravity_align_, true);
  ros::param::param<bool>("~dlio/imu/calibration", this->imu_calibrate_, true);
  ros::param::param<std::vector<float>>("~dlio/imu/intrinsics/accel/bias", prior_accel_bias, accel_default);
  ros::param::param<std::vector<float>>("~dlio/imu/intrinsics/gyro/bias", prior_gyro_bias, gyro_default);

  // scale-misalignment matrix
  std::vector<float> imu_sm_default{1., 0., 0., 0., 1., 0., 0., 0., 1.};
  std::vector<float> imu_sm;

  ros::param::param<std::vector<float>>("~dlio/imu/intrinsics/accel/sm", imu_sm, imu_sm_default);

  if (!this->imu_calibrate_) {
    this->state.b.accel[0] = prior_accel_bias[0];
    this->state.b.accel[1] = prior_accel_bias[1];
    this->state.b.accel[2] = prior_accel_bias[2];
    this->state.b.gyro[0] = prior_gyro_bias[0];
    this->state.b.gyro[1] = prior_gyro_bias[1];
    this->state.b.gyro[2] = prior_gyro_bias[2];
    this->imu_accel_sm_ = Eigen::Map<const Eigen::Matrix<float, -1, -1, Eigen::RowMajor>>(imu_sm.data(), 3, 3);
  } else {
    this->state.b.accel = Eigen::Vector3f(0., 0., 0.);
    this->state.b.gyro = Eigen::Vector3f(0., 0., 0.);
    this->imu_accel_sm_ = Eigen::Matrix3f::Identity();
  }

  // GICP
  ros::param::param<int>("~dlio/odom/gicp/minNumPoints", this->gicp_min_num_points_, 100);
  ros::param::param<int>("~dlio/odom/gicp/kCorrespondences", this->gicp_k_correspondences_, 20);
  ros::param::param<double>("~dlio/odom/gicp/maxCorrespondenceDistance", this->gicp_max_corr_dist_,
      std::sqrt(std::numeric_limits<double>::max()));
  ros::param::param<int>("~dlio/odom/gicp/maxIterations", this->gicp_max_iter_, 64);
  ros::param::param<double>("~dlio/odom/gicp/transformationEpsilon", this->gicp_transformation_ep_, 0.0005);
  ros::param::param<double>("~dlio/odom/gicp/rotationEpsilon", this->gicp_rotation_ep_, 0.0005);
  ros::param::param<double>("~dlio/odom/gicp/initLambdaFactor", this->gicp_init_lambda_factor_, 1e-9);

  // Geometric Observer
  ros::param::param<double>("~dlio/odom/geo/Kp", this->geo_Kp_, 1.0);
  ros::param::param<double>("~dlio/odom/geo/Kv", this->geo_Kv_, 1.0);
  ros::param::param<double>("~dlio/odom/geo/Kq", this->geo_Kq_, 1.0);
  ros::param::param<double>("~dlio/odom/geo/Kab", this->geo_Kab_, 1.0);
  ros::param::param<double>("~dlio/odom/geo/Kgb", this->geo_Kgb_, 1.0);
  ros::param::param<double>("~dlio/odom/geo/abias_max", this->geo_abias_max_, 1.0);
  ros::param::param<double>("~dlio/odom/geo/gbias_max", this->geo_gbias_max_, 1.0);


}
debug

Debug主要打印的一个工整。

+-------------------------------------------------------------------+
|               Direct LiDAR-Inertial Odometry v1.1.1               |
+-------------------------------------------------------------------+
| Wed Mar 15 03:33:45 2023              Elapsed Time: 44.89 seconds |
| Intel(R) Core(TM) i5-10210U CPU @ 1.60GHz x 8                     |
| Sensor Rates: Ouster @ 19.99 Hz, IMU @ 99.94 Hz                   |
|===================================================================|
| Position     {W}  [xyz] :: 1.5051 -0.5809 2.3530                  |
| Orientation  {W} [wxyz] :: -0.2946 -0.4288 0.1831 0.8342          |
| Lin Velocity {B}  [xyz] :: 1.4235 -0.8611 -0.1093                 |
| Ang Velocity {B}  [xyz] :: 0.0151 -0.0592 0.0305                  |
| Accel Bias        [xyz] :: 2.63260126 -3.64898181 4.58988619      |
| Gyro Bias         [xyz] :: -0.00235719 0.04470301 -0.02521588     |
|                                                                   |
| Distance Traveled  :: 58.0019 meters                              |
| Distance to Origin :: 2.8530 meters                               |
| Registration       :: keyframes: 2, deskewed points: 501          |
|                                                                   |
| Computation Time ::  14.54 ms    // Avg:  13.88 / Max:  92.68     |
| Cores Utilized   ::   0.80 cores // Avg:   0.94 / Max:   4.00     |
| CPU Load         ::  10.00 %     // Avg:  11.74 / Max:  50.00     |
| RAM Allocation   :: 105.98 MB                                     |
+-------------------------------------------------------------------+

代码如下：

  • ```c
    void dlio::OdomNode::debug() {

    // Total length traversed
    double length_traversed = 0.;
    Eigen::Vector3f p_curr = Eigen::Vector3f(0., 0., 0.);
    Eigen::Vector3f p_prev = Eigen::Vector3f(0., 0., 0.);
    for (const auto& t : this->trajectory) {
    if (p_prev == Eigen::Vector3f(0., 0., 0.)) {
    p_prev = t.first;
    continue;
    }
    p_curr = t.first;
    double l = sqrt(pow(p_curr[0] - p_prev[0], 2) + pow(p_curr[1] - p_prev[1], 2) + pow(p_curr[2] - p_prev[2], 2));

    if (l >= 0.1) {
  length_traversed += l;
  p_prev = p_curr;
}

    }
    this->length_traversed = length_traversed;

    // Average computation time
    double avg_comp_time =
    std::accumulate(this->comp_times.begin(), this->comp_times.end(), 0.0) / this->comp_times.size();

    // Average sensor rates
    int win_size = 100;
    double avg_imu_rate;
    double avg_lidar_rate;
    if (this->imu_rates.size() < win_size) {
    avg_imu_rate =
    std::accumulate(this->imu_rates.begin(), this->imu_rates.end(), 0.0) / this->imu_rates.size();
    } else {
    avg_imu_rate =
    std::accumulate(this->imu_rates.end()-win_size, this->imu_rates.end(), 0.0) / win_size;
    }
    if (this->lidar_rates.size() < win_size) {
    avg_lidar_rate =
    std::accumulate(this->lidar_rates.begin(), this->lidar_rates.end(), 0.0) / this->lidar_rates.size();
    } else {
    avg_lidar_rate =
    std::accumulate(this->lidar_rates.end()-win_size, this->lidar_rates.end(), 0.0) / win_size;
    }

    // RAM Usage
    double vm_usage = 0.0;
    double resident_set = 0.0;
    std::ifstream stat_stream(“/proc/self/stat”, std::ios_base::in); //get info from proc directory
    std::string pid, comm, state, ppid, pgrp, session, tty_nr;
    std::string tpgid, flags, minflt, cminflt, majflt, cmajflt;
    std::string utime, stime, cutime, cstime, priority, nice;
    std::string num_threads, itrealvalue, starttime;
    unsigned long vsize;
    long rss;
    stat_stream >> pid >> comm >> state >> ppid >> pgrp >> session >> tty_nr
    >> tpgid >> flags >> minflt >> cminflt >> majflt >> cmajflt
    >> utime >> stime >> cutime >> cstime >> priority >> nice
    >> num_threads >> itrealvalue >> starttime >> vsize >> rss; // don’t care about the rest
    stat_stream.close();
    long page_size_kb = sysconf(_SC_PAGE_SIZE) / 1024; // for x86-64 is configured to use 2MB pages
    vm_usage = vsize / 1024.0;
    resident_set = rss * page_size_kb;

    // CPU Usage
    struct tms timeSample;
    clock_t now;
    double cpu_percent;
    now = times(&timeSample);
    if (now <= this->lastCPU || timeSample.tms_stime < this->lastSysCPU ||
    timeSample.tms_utime < this->lastUserCPU) {
    cpu_percent = -1.0;
    } else {
    cpu_percent = (timeSample.tms_stime - this->lastSysCPU) + (timeSample.tms_utime - this->lastUserCPU);
    cpu_percent /= (now - this->lastCPU);
    cpu_percent /= this->numProcessors;
    cpu_percent *= 100.;
    }
    this->lastCPU = now;
    this->lastSysCPU = timeSample.tms_stime;
    this->lastUserCPU = timeSample.tms_utime;
    this->cpu_percents.push_back(cpu_percent);
    double avg_cpu_usage =
    std::accumulate(this->cpu_percents.begin(), this->cpu_percents.end(), 0.0) / this->cpu_percents.size();

    // Print to terminal
    printf(“\033[2J\033[1;1H”);

    std::cout << std::endl
    << “±------------------------------------------------------------------+” << std::endl;
    std::cout << “| Direct LiDAR-Inertial Odometry v” << this->version_ << " |"
    << std::endl;
    std::cout << “±------------------------------------------------------------------+” << std::endl;

    std::time_t curr_time = this->scan_stamp;
    std::string asc_time = std::asctime(std::localtime(&curr_time)); asc_time.pop_back();
    std::cout << "| " << std::left << asc_time;
    std::cout << std::right << std::setfill(’ ') << std::setw(42)
    << "Elapsed Time: " + to_string_with_precision(this->elapsed_time, 2) + " seconds "
    << “|” << std::endl;

    if ( !this->cpu_type.empty() ) {
    std::cout << "| " << std::left << std::setfill(’ ') << std::setw(66)
    << this->cpu_type + " x " + std::to_string(this->numProcessors)
    << “|” << std::endl;
    }

    if (this->sensor == dlio::SensorType::OUSTER) {
    std::cout << "| " << std::left << std::setfill(’ ‘) << std::setw(66)
    << “Sensor Rates: Ouster @ " + to_string_with_precision(avg_lidar_rate, 2)
    + " Hz, IMU @ " + to_string_with_precision(avg_imu_rate, 2) + " Hz”
    << “|” << std::endl;
    } else if (this->sensor == dlio::SensorType::VELODYNE) {
    std::cout << "| " << std::left << std::setfill(’ ‘) << std::setw(66)
    << “Sensor Rates: Velodyne @ " + to_string_with_precision(avg_lidar_rate, 2)
    + " Hz, IMU @ " + to_string_with_precision(avg_imu_rate, 2) + " Hz”
    << “|” << std::endl;
    } else if (this->sensor == dlio::SensorType::HESAI) {
    std::cout << "| " << std::left << std::setfill(’ ‘) << std::setw(66)
    << “Sensor Rates: Hesai @ " + to_string_with_precision(avg_lidar_rate, 2)
    + " Hz, IMU @ " + to_string_with_precision(avg_imu_rate, 2) + " Hz”
    << “|” << std::endl;
    } else {
    std::cout << "| " << std::left << std::setfill(’ ') << std::setw(66)
    << “Sensor Rates: Unknown LiDAR @ " + to_string_with_precision(avg_lidar_rate, 2)
    + " Hz, IMU @ " + to_string_with_precision(avg_imu_rate, 2) + " Hz”
    << “|” << std::endl;
    }

    std::cout << “|===================================================================|” << std::endl;

    std::cout << "| " << std::left << std::setfill(’ ‘) << std::setw(66)
    << "Position {W} [xyz] :: " + to_string_with_precision(this->state.p[0], 4) + " "
    + to_string_with_precision(this->state.p[1], 4) + " "
    + to_string_with_precision(this->state.p[2], 4)
    << “|” << std::endl;
    std::cout << "| " << std::left << std::setfill(’ ‘) << std::setw(66)
    << "Orientation {W} [wxyz] :: " + to_string_with_precision(this->state.q.w(), 4) + " "
    + to_string_with_precision(this->state.q.x(), 4) + " "
    + to_string_with_precision(this->state.q.y(), 4) + " "
    + to_string_with_precision(this->state.q.z(), 4)
    << “|” << std::endl;
    std::cout << "| " << std::left << std::setfill(’ ‘) << std::setw(66)
    << "Lin Velocity {B} [xyz] :: " + to_string_with_precision(this->state.v.lin.b[0], 4) + " "
    + to_string_with_precision(this->state.v.lin.b[1], 4) + " "
    + to_string_with_precision(this->state.v.lin.b[2], 4)
    << “|” << std::endl;
    std::cout << "| " << std::left << std::setfill(’ ‘) << std::setw(66)
    << "Ang Velocity {B} [xyz] :: " + to_string_with_precision(this->state.v.ang.b[0], 4) + " "
    + to_string_with_precision(this->state.v.ang.b[1], 4) + " "
    + to_string_with_precision(this->state.v.ang.b[2], 4)
    << “|” << std::endl;
    std::cout << "| " << std::left << std::setfill(’ ‘) << std::setw(66)
    << "Accel Bias [xyz] :: " + to_string_with_precision(this->state.b.accel[0], 8) + " "
    + to_string_with_precision(this->state.b.accel[1], 8) + " "
    + to_string_with_precision(this->state.b.accel[2], 8)
    << “|” << std::endl;
    std::cout << "| " << std::left << std::setfill(’ ') << std::setw(66)
    << "Gyro Bias [xyz] :: " + to_string_with_precision(this->state.b.gyro[0], 8) + " "
    + to_string_with_precision(this->state.b.gyro[1], 8) + " "
    + to_string_with_precision(this->state.b.gyro[2], 8)
    << “|” << std::endl;

    std::cout << “| |” << std::endl;

    std::cout << "| " << std::left << std::setfill(’ ‘) << std::setw(66)
    << “Distance Traveled :: " + to_string_with_precision(length_traversed, 4) + " meters”
    << “|” << std::endl;
    std::cout << "| " << std::left << std::setfill(’ ‘) << std::setw(66)
    << “Distance to Origin :: "
    + to_string_with_precision( sqrt(pow(this->state.p[0]-this->origin[0],2) +
    pow(this->state.p[1]-this->origin[1],2) +
    pow(this->state.p[2]-this->origin[2],2)), 4) + " meters”
    << “|” << std::endl;
    std::cout << "| " << std::left << std::setfill(’ ') << std::setw(66)
    << "Registration :: keyframes: " + std::to_string(this->keyframes.size()) + ", "
    + "deskewed points: " + std::to_string(this->deskew_size)
    << “|” << std::endl;
    std::cout << “| |” << std::endl;

    std::cout << std::right << std::setprecision(2) << std::fixed;
    std::cout << “| Computation Time :: "
    << std::setfill(’ ') << std::setw(6) << this->comp_times.back()1000. << " ms // Avg: "
    << std::setw(6) << avg_comp_time    1000. << " / Max: "
    << std::setw(6) << *std::max_element(this->comp_times.begin(), this->comp_times.end())*1000.
    << " |” << std::endl;
    std::cout << “| Cores Utilized :: "
    << std::setfill(’ ') << std::setw(6) << (cpu_percent/100.) * this->numProcessors << " cores // Avg: "
    << std::setw(6) << (avg_cpu_usage/100.) * this->numProcessors << " / Max: "
    << std::setw(6) << (*std::max_element(this->cpu_percents.begin(), this->cpu_percents.end()) / 100.)
    * this->numProcessors
    << " |” << std::endl;
    std::cout << “| CPU Load :: "
    << std::setfill(’ ') << std::setw(6) << cpu_percent << " % // Avg: "
    << std::setw(6) << avg_cpu_usage << " / Max: "
    << std::setw(6) << *std::max_element(this->cpu_percents.begin(), this->cpu_percents.end())
    << " |” << std::endl;
    std::cout << "| " << std::left << std::setfill(’ ') << std::setw(66)
    << “RAM Allocation :: " + to_string_with_precision(resident_set/1000., 2) + " MB”
    << “|” << std::endl;

    std::cout << “±------------------------------------------------------------------+” << std::endl;

    }

回调

callbackImu

步骤：

  • 计算两帧间的时间差：dt，if(dt==0) dt = 1./200
  • 转换IMU数据， transformImu
    • 转换角速度（在刚体上是相同的，所以只需旋转到 ROS 约定）： B V a n g l e = I B R ∗ I V a n g l e ^BV_{angle}=^B_IR * ^IV_\mathbf{angle} BVangle​=IB​R∗IVangle​
    • 转换线加速度： B a = I B R ∗ I a + I V a n g l e − I V a n g l e _ l a s t d t × ( − I B R ) + I V a n g l e × ( I V a n g l e × ( − I B R ) ) ^Ba =^B_IR * ^Ia + \frac{^IV_\mathbf{angle} - ^IV_\mathbf{angle\_last}}{dt}\times(-^B_IR)+^IV_\mathbf{angle}\times(^IV_\mathbf{angle}\times (-^B_IR)) Ba=IB​R∗Ia+dtIVangle​−IVangle_last​​×(−IB​R)+IVangle​×(IVangle​×(−IB​R))
      • 变换线性加速度（需要考虑由于平移差异而产生的分量）
  • 若 imu未标定时，进行标定
    • 求线加速度均值： a ⃗ a v g \vec a_\mathbf{avg} a avg​，和角速度均值： v ⃗ a v g \vec v_\mathbf{avg} v avg​
      • 分别对 角速度+线加速度 累计imu_calib_time时间，并求均值
    • 若需重力对齐时，估计重力矢量
      • 重力向量： g ⃗ = ( a ⃗ a v g − b a ) . n o r m ∗ g _ \vec g = (\vec a_\mathbf{avg} - b_a).\mathbf{norm} * g\_ g ​=(a avg​−ba​).norm∗g_
        • 注：加速度均值即当前重力方向，归一化长度到 g _ g\_ g_ 大小
      • 重力向量到重力的转换 q g r a v = g ⃗    T o    G ⃗ ( 0 , 0 , g ) q_\mathbf{grav} = \vec g \ \ \mathbf{To} \ \ \vec G_{(0,0,g)} qgrav​=g ​  To  G (0,0,g)​
        • 调用函数：Eigen::Quaternionf::FromTwoVectors
        • 如果偏航较小，则使用替代表示：±180°以内
    • 若标定加速度时， b a = a ⃗ a v g − g ⃗ b_a = \vec a_\mathbf{avg}- \vec g ba​=a avg​−g ​
      • 加速度均值减去重力矢量
    • 若标定角速度时， b g = v ⃗ a v g b_g=\vec v_\mathbf{avg} bg​=v avg​
    • 标定成功
  • 已标定，正常的工作流程：

    • 1、计算 imu_rate，注：低通滤波器求取

    • 2、计算修正后的 imu数据，注：减去bias偏差

      • 注意：ba,bg 都被减去了

    • 3、数据存入 imu_buffer中，并条件唤醒

      • 条件唤醒callbackPointCloud线程本次IMU数据存在
      • 放入数据时mtx_imu保护，且 push_front
      • 格式使用了 boost::circular_buffe 不用上锁

    • 4、已经进行 第一次优化了，则进行 状态state 传播 propagateState

      • 注：状态结构体，内部自带 互斥锁geo.mtx

      • 将加速度从身体坐标系转换为世界坐标系： W a = W q ∗ a ^W a = ^Wq * a Wa=Wq∗a

      • 位置更新： p = p + v ∗ d t + 0.5 ∗ a ∗ d t 2 p= p + v *d_t + 0.5 *a*d_t^2 p=p+v∗dt​+0.5∗a∗dt2​

        • 注：Z 方向 加速度 需要减去 重力大小，p,v 都是
        • a = a ^ − G ⃗ a = \hat a -\vec G a=a^−G

      • 线速度更新：

        • W v l i n e = W v l i n e + a ∗ d t ^Wv_\mathbf{line} = ^Wv_\mathbf{line}+a*d_t Wvline​=Wvline​+a∗dt​
        • B v = W q − 1 ∗ W v l i n e ^Bv =^Wq^{-1} * ^Wv_\mathbf{line} Bv=Wq−1∗Wvline​

      • 角度更新：

        • q w = q w q_w = q_w qw​=qw​
        • q v = q v + 1 2 ∗ v ∗ d t q_v = q_v +\frac 1 2 * v * d_t qv​=qv​+21​∗v∗dt​

      • 角速度更新：

        • B v a n g = v i m u ^Bv_\mathbf{ang} = v_\mathbf{imu} Bvang​=vimu​
        • W v a n g = W q ∗ B v a n g ^Wv_\mathbf{ang}=^Wq * ^Bv_\mathbf{ang} Wvang​=Wq∗Bvang​
void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) {

  this->first_imu_received = true;

  /// imu 数据到 base_link
  sensor_msgs::Imu::Ptr imu = this->transformImu( imu_raw );
  this->imu_stamp = imu->header.stamp;

  Eigen::Vector3f lin_accel;
  Eigen::Vector3f ang_vel;

  // Get IMU samples
  ang_vel[0] = imu->angular_velocity.x;
  ang_vel[1] = imu->angular_velocity.y;
  ang_vel[2] = imu->angular_velocity.z;

  lin_accel[0] = imu->linear_acceleration.x;
  lin_accel[1] = imu->linear_acceleration.y;
  lin_accel[2] = imu->linear_acceleration.z;

  if (this->first_imu_stamp == 0.) {
    this->first_imu_stamp = imu->header.stamp.toSec();
  }

  // IMU calibration procedure - do for three seconds
  if (!this->imu_calibrated) {

    static int num_samples = 0;
    static Eigen::Vector3f gyro_avg (0., 0., 0.);
    static Eigen::Vector3f accel_avg (0., 0., 0.);
    static bool print = true;

    if ((imu->header.stamp.toSec() - this->first_imu_stamp) < this->imu_calib_time_) {

      num_samples++;

      gyro_avg[0] += ang_vel[0];
      gyro_avg[1] += ang_vel[1];
      gyro_avg[2] += ang_vel[2];

      accel_avg[0] += lin_accel[0];
      accel_avg[1] += lin_accel[1];
      accel_avg[2] += lin_accel[2];

      if(print) {
        std::cout << std::endl << " Calibrating IMU for " << this->imu_calib_time_ << " seconds... ";
        std::cout.flush();
        print = false;
      }

    } else {

      std::cout << "done" << std::endl << std::endl;
	
      // 三秒数据做均值，得到 gyro+accel 平均值
      gyro_avg /= num_samples;
      accel_avg /= num_samples;

      Eigen::Vector3f grav_vec (0., 0., this->gravity_);

      // 需要重力对齐时， 得到重力向量
      if (this->gravity_align_) {

        // Estimate gravity vector - Only approximate if biases have not been pre-calibrated
        grav_vec = (accel_avg - this->state.b.accel).normalized() * abs(this->gravity_);
        Eigen::Quaternionf grav_q = Eigen::Quaternionf::FromTwoVectors(grav_vec, Eigen::Vector3f(0., 0., this->gravity_));

        // set gravity aligned orientation
        this->state.q = grav_q;
        this->T.block(0,0,3,3) = this->state.q.toRotationMatrix();
        this->lidarPose.q = this->state.q;

        // rpy
        auto euler = grav_q.toRotationMatrix().eulerAngles(2, 1, 0);
        double yaw = euler[0] * (180.0/M_PI);
        double pitch = euler[1] * (180.0/M_PI);
        double roll = euler[2] * (180.0/M_PI);

        // use alternate representation if the yaw is smaller
        if (abs(remainder(yaw + 180.0, 360.0)) < abs(yaw)) {
          yaw   = remainder(yaw + 180.0,   360.0);
          pitch = remainder(180.0 - pitch, 360.0);
          roll  = remainder(roll + 180.0,  360.0);
        }
        std::cout << " Estimated initial attitude:" << std::endl;
        std::cout << "   Roll  [deg]: " << to_string_with_precision(roll, 4) << std::endl;
        std::cout << "   Pitch [deg]: " << to_string_with_precision(pitch, 4) << std::endl;
        std::cout << "   Yaw   [deg]: " << to_string_with_precision(yaw, 4) << std::endl;
        std::cout << std::endl;
      }

      if (this->calibrate_accel_) {

        // subtract gravity from avg accel to get bias
        this->state.b.accel = accel_avg - grav_vec;

        std::cout << " Accel biases [xyz]: " << to_string_with_precision(this->state.b.accel[0], 8) << ", "
                                             << to_string_with_precision(this->state.b.accel[1], 8) << ", "
                                             << to_string_with_precision(this->state.b.accel[2], 8) << std::endl;
      }

      if (this->calibrate_gyro_) {

        this->state.b.gyro = gyro_avg;

        std::cout << " Gyro biases  [xyz]: " << to_string_with_precision(this->state.b.gyro[0], 8) << ", "
                                             << to_string_with_precision(this->state.b.gyro[1], 8) << ", "
                                             << to_string_with_precision(this->state.b.gyro[2], 8) << std::endl;
      }

      this->imu_calibrated = true;

    }

  } else {

    double dt = imu->header.stamp.toSec() - this->prev_imu_stamp;
    if (dt == 0) { dt = 1.0/200.0; }
    this->imu_rates.push_back( 1./dt );	// 激光rate

    // Apply the calibrated bias to the new IMU measurements
    this->imu_meas.stamp = imu->header.stamp.toSec();
    this->imu_meas.dt = dt;
    this->prev_imu_stamp = this->imu_meas.stamp;

    Eigen::Vector3f lin_accel_corrected = (this->imu_accel_sm_ * lin_accel) - this->state.b.accel;
    Eigen::Vector3f ang_vel_corrected = ang_vel - this->state.b.gyro;

    this->imu_meas.lin_accel = lin_accel_corrected;
    this->imu_meas.ang_vel = ang_vel_corrected;

    // Store calibrated IMU measurements into imu buffer for manual integration later.
    this->mtx_imu.lock();
    this->imu_buffer.push_front(this->imu_meas);
    this->mtx_imu.unlock();

    // Notify the callbackPointCloud thread that IMU data exists for this time
    this->cv_imu_stamp.notify_one();

    if (this->geo.first_opt_done) {
      // Geometric Observer: Propagate State
      this->propagateState();
    }

  }

}
callbackPointCloud

  • 闭环标志锁住：main_loop_running = true

  • 若 dlio未初始化，则进行初始化 initializeDLIO

    • imu数据收到，且已经标定完成

  • 将传入的scan转为 dlio格式 getScanFromROS

    • 移除Nan点，调用pcl::removeNaNFromPointCloud
    • 移除crop box滤波， 滤除±1m之内的，pcl::CropBox<PointType>
    • 激光类型确定，field.name = t/time/timestamp 分：OUSTER, VELODYNE, HESAI
    • 若未知，则不进行去畸变

  • 预处理点 preprocessPoints

    • 去畸变
      • 若需去畸变时，调用 deskewPointcloud后，! first_valid_scan时 return
      • 否则，进行数据的整理

        • ! first_valid_scan时 ，进行 first_valid_scan=true

          • 第一帧该激光时间戳小于 imu_buffer最开始时，return，
          • 否则 this->first_valid_scan = true

        • 迭代得到点云数组的状态，integrateImu

    • 基于参数是否使用体素滤波 vf_use_

  • 检测 激光是否符合要求

    • 第一次有效，不符合return
    • 当前激光点个数小于 gicp最小阈值，return

  • 独立线程计算指标 computeMetrics

    • computeSpaciousness 平均长度

      • 计算当前激光水平长度 x 2 + y 2 \sqrt{x^2+y^2} x2+y2 ​的中位数，并用低通滤波器均衡化 0.95 L m i + 0.5 L c u r r 0.95L_{mi}+0.5L_{curr} 0.95Lmi​+0.5Lcurr​
      • 变量 ：spaciousness

    • computeDensity 平均密度

      • 当前密度使用gicp.source_density_，也用低通滤波器均衡化
      • 变量：density

  • 若设定自适应参数时，激光进行自适应 setAdaptiveParams

    • 基于平均长度spaciousness设置 关键帧阈值距离：

      • s p < 0.5 ? s p = 0.5     s p > 5 ? s p = 5 sp< 0.5?sp=0.5 \ \ \ sp>5?sp=5 sp<0.5?sp=0.5   sp>5?sp=5
      • keyframe_thresh_dist_ = sp

    • 基于平均长度+密度设置 icp相关点的距离

      • den<0.5*gicp_max_corr_dist_? den=0.5*gicp_max_corr_dist_
      • den>2*gicp_max_corr_dist_? den=2*gicp_max_corr_dist_
      • sp<5? den=0.5*gicp_max_corr_dist_
      • sp>5? den=2*gicp_max_corr_dist_
      • gicp.setMaxCorrespondenceDistance(den)

  • 设置新的帧作为输入源 setInputSource

    • this->gicp.setInputSource(this->current_scan)
    • this->gicp.calculateSourceCovariances();

  • 若关键帧个数为0时，设置初始帧作为关键帧

    • 初始化输入目标 initializeInputTarget
    • 闭环标志：main_loop_running = false
    • 启动异步线程进行进行构建子图，直到结束buildKeyframesAndSubmap
    • return

  • 得到下一个位姿getNextPose，(Get the next pose via IMU + S2M + GEO)

  • 更新当前关键帧位姿和地图 updateKeyframes

  • 如果需要的话构建关键帧法线和子图，如果没有则等待闭环结束

    • 需要构建关键字和子图：

      • 闭环标志：main_loop_running = false
      • 启动异步线程进行进行构建子图，直到结束buildKeyframesAndSubmap

    • 不需要时:this->submap_build_cv.notify_one();

  • 发布数据 到ros接口，也是独立线程 std::thread( &dlio::OdomNode::publishToROS

  • 更新静态数据：

  • debug 打印，也是 独立线程std::thread( &dlio::OdomNode::debug

void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& pc) {

  std::unique_lock<decltype(this->main_loop_running_mutex)> lock(main_loop_running_mutex);
  this->main_loop_running = true;
  lock.unlock();

  double then = ros::Time::now().toSec();

  if (this->first_scan_stamp == 0.) {
    this->first_scan_stamp = pc->header.stamp.toSec();
  }

  // DLIO Initialization procedures (IMU calib, gravity align)
  if (!this->dlio_initialized) {
    this->initializeDLIO();
  }

  // Convert incoming scan into DLIO format
  this->getScanFromROS(pc);

  // Preprocess points
  this->preprocessPoints();

  if (!this->first_valid_scan) {
    return;
  }

  if (this->current_scan->points.size() <= this->gicp_min_num_points_) {
    ROS_FATAL("Low number of points in the cloud!");
    return;
  }

  // Compute Metrics
  this->metrics_thread = std::thread( &dlio::OdomNode::computeMetrics, this );
  this->metrics_thread.detach();

  // Set Adaptive Parameters
  if (this->adaptive_params_) {
    this->setAdaptiveParams();
  }

  // Set new frame as input source
  this->setInputSource();

  // Set initial frame as first keyframe
  if (this->keyframes.size() == 0) {
    this->initializeInputTarget();
    this->main_loop_running = false;
    this->submap_future =
      std::async( std::launch::async, &dlio::OdomNode::buildKeyframesAndSubmap, this, this->state );
    this->submap_future.wait(); // wait until completion
    return;
  }

  // Get the next pose via IMU + S2M + GEO
  this->getNextPose();

  // Update current keyframe poses and map
  this->updateKeyframes();

  // Build keyframe normals and submap if needed (and if we're not already waiting)
  if (this->new_submap_is_ready) {
    this->main_loop_running = false;
    this->submap_future =
      std::async( std::launch::async, &dlio::OdomNode::buildKeyframesAndSubmap, this, this->state );
  } else {
    lock.lock();
    this->main_loop_running = false;
    lock.unlock();
    this->submap_build_cv.notify_one();
  }

  // Update trajectory
  this->trajectory.push_back( std::make_pair(this->state.p, this->state.q) );

  // Update time stamps
  this->lidar_rates.push_back( 1. / (this->scan_stamp - this->prev_scan_stamp) );
  this->prev_scan_stamp = this->scan_stamp;
  this->elapsed_time = this->scan_stamp - this->first_scan_stamp;

  // Publish stuff to ROS
  pcl::PointCloud<PointType>::ConstPtr published_cloud;
  if (this->densemap_filtered_) {
    published_cloud = this->current_scan;
  } else {
    published_cloud = this->deskewed_scan;
  }
  this->publish_thread = std::thread( &dlio::OdomNode::publishToROS, this, published_cloud, this->T_corr );
  this->publish_thread.detach();

  // Update some statistics
  this->comp_times.push_back(ros::Time::now().toSec() - then);
  this->gicp_hasConverged = this->gicp.hasConverged();

  // Debug statements and publish custom DLIO message
  this->debug_thread = std::thread( &dlio::OdomNode::debug, this );
  this->debug_thread.detach();

  this->geo.first_opt_done = true;

}

其他

getNextPose

步骤：

  • 判断是否有新地图 准备好了，submap_future.wait_for(std::chrono::seconds(0)) == std::future_status::ready);

  • 若 有新地图准备好了 且 地图已经更新了，则跟新target数据

    • gicp target更新：
      • 设置当前全局地图为 模板 gicp.registerInputTarget
      • 设置 全局 gicp.target_kdtree_
      • 设置全局协方差 gicp.setTargetCovariances
    • >全局地图已更新设为false

  • 以全局 IMU 变换作为初始猜测与当前子图对齐

    • this->gicp.align(*aligned);

  • 得到最终的位姿：

    • 修正的位姿： T c o r r T_{corr} Tcorr​
    • 修正： T = T c o r r ∗ T p r i o r T = T_{corr} * T_{prior} T=Tcorr​∗Tprior​

  • 更新 下一个全局位姿 propagateGICP

    • 因为 源点云 和 目标点云 都在全局框架中，因此转换也是全局的

    • 位姿 T (Matrix4d) -> Pose(vector3f+ Quaternionf) 的转换，存储到 lidarPose

  • 更新状态量：updateState

    • state biias 的更新：

      • q e = q s t a t e ∗ ∗ q l i d a r q_{e} = q^*_{state} * q_{lidar} qe​=qstate∗​∗qlidar​

      • q c o r r = 1 − a b s ( q e . w ) , q e . v e c q_{corr} = {1-\mathbf{abs}(qe.w),qe.vec} qcorr​=1−abs(qe.w),qe.vec

      • q c o r r = q l i d a r ∗ q c o r r q_{corr}=q_{lidar} * q_{corr} qcorr​=qlidar​∗qcorr​

      • p e r r = p l i d a r − p s t a t e p_{err}= p_{lidar}-p_{state} perr​=plidar​−pstate​

      • p e b = q s t a t e ∗ ∗ p e r r p_{eb} = q_{state}^* * p_{err} peb​=qstate∗​∗perr​

      • b a c c = b a c c − d t ∗ p e b b_{acc} = b_{acc} - d_t * p_{eb} bacc​=bacc​−dt​∗peb​

      • b g y r o − = d t ∗ { q e w ∗ q e x , q e w ∗ q e y , q e w ∗ q e z } b_{gyro} -= d_t * \{q_{ew}*q_{ex},q_{ew}*q_{ey},q_{ew}*q_{ez}\} bgyro​−=dt​∗{qew​∗qex​,qew​∗qey​,qew​∗qez​}

      • 增加阈值限制的这个高级用法：this->state.b.accel.array().min(abias_max).max(-abias_max);

    • state 平移 p p p 的更新：

      • p s t a t e = p s t a t e + d t ∗ p e r r p_{state} = p_{state} +d_t * p_{err} pstate​=pstate​+dt​∗perr​
      • v l i n w = v l i n w + d t ∗ p e r r {v_{lin_w} = v_{lin_w} + d_t * p_{err}} vlinw​​=vlinw​​+dt​∗perr​
      • q s t a t e = q s t a t e + d t ∗ q c o r r q_{state}=q_{state} + d_t*q_{corr} qstate​=qstate​+dt​∗qcorr​

    • geo.prev_p,prev_q,prev_vel 的更新，直接赋值 state的

void dlio::OdomNode::getNextPose() {

  // Check if the new submap is ready to be used
  this->new_submap_is_ready = (this->submap_future.wait_for(std::chrono::seconds(0)) == std::future_status::ready);

  if (this->new_submap_is_ready && this->submap_hasChanged) {

    // Set the current global submap as the target cloud
    this->gicp.registerInputTarget(this->submap_cloud);

    // Set submap kdtree
    this->gicp.target_kdtree_ = this->submap_kdtree;

    // Set target cloud's normals as submap normals
    this->gicp.setTargetCovariances(this->submap_normals);

    this->submap_hasChanged = false;
  }

  // Align with current submap with global IMU transformation as initial guess
  pcl::PointCloud<PointType>::Ptr aligned (boost::make_shared<pcl::PointCloud<PointType>>());
  this->gicp.align(*aligned);

  // Get final transformation in global frame
  this->T_corr = this->gicp.getFinalTransformation(); // "correction" transformation
  this->T = this->T_corr * this->T_prior;

  // Update next global pose
  // Both source and target clouds are in the global frame now, so tranformation is global
  this->propagateGICP();

  // Geometric observer update
  this->updateState();

}
updateKeyframes

  • 关键字存储格式：

    • std::vector<std::pair<std::pair<Eigen::Vector3f, Eigen::Quaternionf>,
                      pcl::PointCloud<PointType>::ConstPtr>> keyframes;

    • keyframe：Pose+pointcloud <-- pose：position+rotation

  • 步骤：

    • integrateImu遍历已有关键帧，找到与 当前位姿相关的 关键帧index
      • 直接计算欧氏距离， Δ d = ∣ ∣ p s t a t e − p i ∣ ∣ \Delta d = ||p_{state}-p_{i}|| Δd=∣∣pstate​−pi​∣∣
      • 统计小于1.5倍阈值的个数 num_nearby：
        • Δ d < 1.5 ∗ k e y f r a m e _ t h r e s h _ d i s t _ \Delta d < 1.5 * \mathbf{keyframe\_thresh\_dist\_} Δd<1.5∗keyframe_thresh_dist_? ++num_nearby;
      • 存储最近的下标closest_idx以及最近的距离closest_d
    • 得到最近的 位置+旋转，进而得到与当前state的 差异(位置+角度)
      • 位置差异： Δ d = ∣ ∣ p s t a t e − p i ∣ ∣ \Delta d = ||p_{state}-p_{i}|| Δd=∣∣pstate​−pi​∣∣
      • 角度差异：
        • δ q = ∣ q s t a t e ∗ q c l o s e − 1 ∣ \delta q = |q_{state}*q_{close}^{-1}| δq=∣qstate​∗qclose−1​∣
        • 得到旋转角度： θ = 2 ∗ a t a n 2 { δ q v e c . n o r m ( ) , δ q w } \theta = 2*\mathbf{atan2}\{\delta q_{vec.norm()},\delta q_w\} θ=2∗atan2{δqvec.norm()​,δqw​}
        • 旋转角度差异： Θ = 180. ∗ θ / π \Theta = 180. * \theta / \pi Θ=180.∗θ/π
    • 新关键帧评判标准，感觉1，2 可以合起来
      • 1、若 位置差异 大于 位置阈值 或者 角度差异大于 角度阈值时，newKeyframe = true;
      • 2、若 位置差异 小于 位置阈值 时，newKeyframe = false;
      • 3、若 位置差异 小于 位置阈值 或者 角度差异大于 角度阈值 且满足 位置小于1.5倍的个数<=1 时，newKeyframe = true;
    • 若当前帧添加新关键帧，则 放入数据
      • keyframes
      • keyframe_timestamps
      • keyframe_normals keyframe_normals.push_back(this->gicp.getSourceCovariances())
      • keyframe_transformations keyframe_transformations.push_back(this->T_corr); 
void dlio::OdomNode::updateKeyframes() {

  // calculate difference in pose and rotation to all poses in trajectory
  float closest_d = std::numeric_limits<float>::infinity();
  int closest_idx = 0;
  int keyframes_idx = 0;

  int num_nearby = 0;

  for (const auto& k : this->keyframes) {

    // calculate distance between current pose and pose in keyframes
    float delta_d = sqrt( pow(this->state.p[0] - k.first.first[0], 2) +
                          pow(this->state.p[1] - k.first.first[1], 2) +
                          pow(this->state.p[2] - k.first.first[2], 2) );

    // count the number nearby current pose
    if (delta_d <= this->keyframe_thresh_dist_ * 1.5){
      ++num_nearby;
    }

    // store into variable
    if (delta_d < closest_d) {
      closest_d = delta_d;
      closest_idx = keyframes_idx;
    }

    keyframes_idx++;

  }

  // get closest pose and corresponding rotation
  Eigen::Vector3f closest_pose = this->keyframes[closest_idx].first.first;
  Eigen::Quaternionf closest_pose_r = this->keyframes[closest_idx].first.second;

  // calculate distance between current pose and closest pose from above
  float dd = sqrt( pow(this->state.p[0] - closest_pose[0], 2) +
                   pow(this->state.p[1] - closest_pose[1], 2) +
                   pow(this->state.p[2] - closest_pose[2], 2) );

  // calculate difference in orientation using SLERP
  Eigen::Quaternionf dq;

  if (this->state.q.dot(closest_pose_r) < 0.) {
    Eigen::Quaternionf lq = closest_pose_r;
    lq.w() *= -1.; lq.x() *= -1.; lq.y() *= -1.; lq.z() *= -1.;
    dq = this->state.q * lq.inverse();
  } else {
    dq = this->state.q * closest_pose_r.inverse();
  }

  double theta_rad = 2. * atan2(sqrt( pow(dq.x(), 2) + pow(dq.y(), 2) + pow(dq.z(), 2) ), dq.w());
  double theta_deg = theta_rad * (180.0/M_PI);

  // update keyframes
  bool newKeyframe = false;

  if (abs(dd) > this->keyframe_thresh_dist_ || abs(theta_deg) > this->keyframe_thresh_rot_) {
    newKeyframe = true;
  }

  if (abs(dd) <= this->keyframe_thresh_dist_) {
    newKeyframe = false;
  }

  if (abs(dd) <= this->keyframe_thresh_dist_ && abs(theta_deg) > this->keyframe_thresh_rot_ && num_nearby <= 1) {
    newKeyframe = true;
  }

  if (newKeyframe) {

    // update keyframe vector
    std::unique_lock<decltype(this->keyframes_mutex)> lock(this->keyframes_mutex);
    this->keyframes.push_back(std::make_pair(std::make_pair(this->lidarPose.p, this->lidarPose.q), this->current_scan));
    this->keyframe_timestamps.push_back(this->scan_header_stamp);
    this->keyframe_normals.push_back(this->gicp.getSourceCovariances());
    this->keyframe_transformations.push_back(this->T_corr);
    lock.unlock();

  }

}
buildKeyframesAndSubmap
  • 说明：
    • 已处理关键帧个数 num_processed_keyframes，全局变量

步骤：

  • 从 已处理关键帧 开始，遍历关键帧，根据关键帧的修正量更新 激光点+激光点协方差

    • 取当前帧的点云+协方差raw_keyframe+raw_covariances，修正量T

    • 点云基于修正量 T T T转换：pcl::transformPointCloud (*raw_keyframe, *transformed_keyframe, T);

      • 注意：这个在生成新关键帧时会保留修正量，但是未直接更新关键帧

    • 点云协方差进行转换：

      • std::transform(raw_covariances->begin(), raw_covariances->end(), transformed_covariances->begin(),
                   [&Td](Eigen::Matrix4d cov) { return Td * cov * Td.transpose(); });

    • 利用修正后的数据 更新 激光点+激光点协方差 std::transform(

    • 发布关键帧，使用的是独立线程：publish_keyframe_thread = std::thread

  • 暂停以防止从正在运行的主循环中窃取资源 pauseSubmapBuildIfNeeded

    • 互斥锁，等待主线程 main获取前 K 个最近邻关键帧姿势的索引不进行 main_loop_running

  • 构建Submap buildSubmap

  • 结束

void dlio::OdomNode::buildKeyframesAndSubmap(State vehicle_state) {

  // transform the new keyframe(s) and associated covariance list(s)
    std::unique_lock<decltype(this->keyframes_mutex)> lock(this->keyframes_mutex);

  for (int i = this->num_processed_keyframes; i < this->keyframes.size(); i++) {
    pcl::PointCloud<PointType>::ConstPtr raw_keyframe = this->keyframes[i].second;
    std::shared_ptr<const nano_gicp::CovarianceList> raw_covariances = this->keyframe_normals[i];
    Eigen::Matrix4f T = this->keyframe_transformations[i];
    lock.unlock();

    Eigen::Matrix4d Td = T.cast<double>();

    pcl::PointCloud<PointType>::Ptr transformed_keyframe (boost::make_shared<pcl::PointCloud<PointType>>());
    pcl::transformPointCloud (*raw_keyframe, *transformed_keyframe, T);

    std::shared_ptr<nano_gicp::CovarianceList> transformed_covariances (std::make_shared<nano_gicp::CovarianceList>(raw_covariances->size()));
    std::transform(raw_covariances->begin(), raw_covariances->end(), transformed_covariances->begin(),
                   [&Td](Eigen::Matrix4d cov) { return Td * cov * Td.transpose(); });

    ++this->num_processed_keyframes;

    lock.lock();
    this->keyframes[i].second = transformed_keyframe;
    this->keyframe_normals[i] = transformed_covariances;

    this->publish_keyframe_thread = std::thread( &dlio::OdomNode::publishKeyframe, this, this->keyframes[i], this->keyframe_timestamps[i] );
    this->publish_keyframe_thread.detach();
  }

  lock.unlock();

  // Pause to prevent stealing resources from the main loop if it is running.
  this->pauseSubmapBuildIfNeeded();

  this->buildSubmap(vehicle_state);
}
buildSubmap
  • 需：锁住关键帧 keyframes_mutex

步骤

  • 遍历 已处理完成的关键帧，求得 距给定 车辆状态的距离及索引

    • δ d = ∣ ∣ p s t a t e − p i k e y ∣ ∣ \delta d = ||p_{state}-p_{i_{key}}|| δd=∣∣pstate​−pikey​​∣∣，keyframe_nn.push(i)

  • 获取前 K 个最近邻关键帧姿势的索引 pushSubmapIndices

    • 利用优先队列 std::priority_queue取

      • for (auto d : dists) {
    if (pq.size() >= k && pq.top() > d) {
      pq.push(d);
      pq.pop();
    } else if (pq.size() < k) {
      pq.push(d);
    }
}

    • 获取第 k 个最小的元素的值，该元素应该位于堆的顶部

    • 遍历关键帧组，将小于等于 k值的 frame存起来。 pushSubmapIndices

  • 得到凸包中距离前10的索引

    • 计算凸包， computeConvexHull
      • 已处理关键帧个数至少需4个，否则直接返回
      • 基于关键帧组的位置 创建一个点云 cloud
      • for已处理关键帧，将其 position 放入创建的点云中
      • 将其放入凸包检查中， convex_hull.setInputCloud(cloud)
      • 计算所有输入点的凸多边形 convex_hull.reconstruct
      • 取凸多边形的 索引 convex_hull.getHullPointIndices
    • 获取凸包上每个关键帧的距离
      • 基于索引在 距离 数组中查找
    • 获取凸包中 距离前10的索引 pushSubmapIndices

  • 得到凹包中距离前10的索引

    • 计算凹包，computeConcaveHull
      • 已处理关键帧个数至少需5个，否则直接返回
      • 基于关键帧组的位置 创建一个点云 cloud
        • for已处理关键帧，将其 position 放入创建的点云中
      • 将其放入凹包检查中， concave_hull.setInputCloud(cloud)
      • 计算所有输入点的凹多边形 concave_hull.reconstruct
      • 取凹多边形的 索引 concave_hull.getHullPointIndices
    • 获取凹包上每个关键帧的距离
      • 基于索引在 距离 数组中查找
    • 获取凹包中 距离前10的索引 pushSubmapIndices

  • 对当前和上一个子图 kf 索引列表进行排序 std::sort，删除 std::unique

    • submap_kf_idx_curr
    • submap_kf_idx_prev

  • 若二者不相等，则 基于 submap_kf_idx_curr 的点云重新生成地图

    • 状态地图已经改变了：submap_hasChanged = true
    • 重新生成 点云+点云法向量：submap_cloud+submap_normals
    • 基于新生成的点云 重新生成 kd_tree_
    • 更新前submap索引 submap_kf_idx_prev = submap_kf_idx_curr
void dlio::OdomNode::buildSubmap(State vehicle_state) {

  // clear vector of keyframe indices to use for submap
  this->submap_kf_idx_curr.clear();

  // calculate distance between current pose and poses in keyframe set
  std::unique_lock<decltype(this->keyframes_mutex)> lock(this->keyframes_mutex);
  std::vector<float> ds;
  std::vector<int> keyframe_nn;
  for (int i = 0; i < this->num_processed_keyframes; i++) {
    float d = sqrt( pow(vehicle_state.p[0] - this->keyframes[i].first.first[0], 2) +
                    pow(vehicle_state.p[1] - this->keyframes[i].first.first[1], 2) +
                    pow(vehicle_state.p[2] - this->keyframes[i].first.first[2], 2) );
    ds.push_back(d);
    keyframe_nn.push_back(i);
  }
  lock.unlock();

  // get indices for top K nearest neighbor keyframe poses
  this->pushSubmapIndices(ds, this->submap_knn_, keyframe_nn);

  // get convex hull indices
  this->computeConvexHull();

  // get distances for each keyframe on convex hull
  std::vector<float> convex_ds;
  for (const auto& c : this->keyframe_convex) {
    convex_ds.push_back(ds[c]);
  }

  // get indices for top kNN for convex hull
  this->pushSubmapIndices(convex_ds, this->submap_kcv_, this->keyframe_convex);

  // get concave hull indices
  this->computeConcaveHull();

  // get distances for each keyframe on concave hull
  std::vector<float> concave_ds;
  for (const auto& c : this->keyframe_concave) {
    concave_ds.push_back(ds[c]);
  }

  // get indices for top kNN for concave hull
  this->pushSubmapIndices(concave_ds, this->submap_kcc_, this->keyframe_concave);

  // sort current and previous submap kf list of indices
  std::sort(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end());
  std::sort(this->submap_kf_idx_prev.begin(), this->submap_kf_idx_prev.end());
  
  // remove duplicate indices
  auto last = std::unique(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end());
  this->submap_kf_idx_curr.erase(last, this->submap_kf_idx_curr.end());

  // check if submap has changed from previous iteration
  if (this->submap_kf_idx_curr != this->submap_kf_idx_prev){

    this->submap_hasChanged = true;

    // Pause to prevent stealing resources from the main loop if it is running.
    this->pauseSubmapBuildIfNeeded();

    // reinitialize submap cloud and normals
    pcl::PointCloud<PointType>::Ptr submap_cloud_ (boost::make_shared<pcl::PointCloud<PointType>>());
    std::shared_ptr<nano_gicp::CovarianceList> submap_normals_ (std::make_shared<nano_gicp::CovarianceList>());

    for (auto k : this->submap_kf_idx_curr) {

      // create current submap cloud
      lock.lock();
      *submap_cloud_ += *this->keyframes[k].second;
      lock.unlock();

      // grab corresponding submap cloud's normals
      submap_normals_->insert( std::end(*submap_normals_),
          std::begin(*(this->keyframe_normals[k])), std::end(*(this->keyframe_normals[k])) );
    }

    this->submap_cloud = submap_cloud_;
    this->submap_normals = submap_normals_;

    // Pause to prevent stealing resources from the main loop if it is running.
    this->pauseSubmapBuildIfNeeded();

    this->gicp_temp.setInputTarget(this->submap_cloud);
    this->submap_kdtree = this->gicp_temp.target_kdtree_;

    this->submap_kf_idx_prev = this->submap_kf_idx_curr;
  }
}
deskewPointcloud
  • 去畸变，就是各个时间戳都相对于某一时间戳

步骤：

  • 各个点的时间戳相对于这个时间： sweep_ref_time = this->scan_header_stamp
  • 按时间戳对点进行排序并构建时间戳列表
    • 定义三个函数对象，时间戳比较大小，时间戳是否相等，提取时间点
    • 将激光点按照时间进行排序 std::partial_sort_copy
    • 滤除使得时间戳唯一：boost::adaptors::indexed() | boost::adaptors::adjacent_filtered
    • 从点中提取时间戳并将它们放入自己的列表中
      • for 循环points_unique_timestamps，并放入 timestamps、unique_time_indices
    • 将当前激光帧时间设置为中间时间 scan_stamp = timestamps[median_pt_index]
  • 在 IMU 数据出现之前不处理扫描
    • !this->first_valid_scan时，可以初始化，然后 return
  • 利用 imu数据进行 进行去畸变
    • 将 imu数据 按照 时间戳 timestamps 中按照 全局位姿进行预积分，得到timestamps中各个点的位姿
      • 调用函数 integrateImu，得到各个点的位姿：frames
    • 预积分出问题时，如果扫描开始和结束之间没有帧，则可能意味着存在同步问题
      • 坏的同步基于激光和imu，因此直接转到 base_link系 pcl::transformPointCloud
      • deskew_status = false，结束本次循环
    • 预积分未出问题时，进行同步
      • 多线程处理 #pragma omp parallel for num_threads(this->num_threads_)
      • for 遍历每个点，进行如下操作：
        • 计算当前点时间base到世界坐标系：$^W_{B}T = ^W_{L}T * ^L_{B}T $
        • 当前点转到 世界坐标系 $^W_{p_t}T = ^W_{B}T * ^B_{p_t}T $
      • 去畸变完成
void dlio::OdomNode::deskewPointcloud() {

  pcl::PointCloud<PointType>::Ptr deskewed_scan_ (boost::make_shared<pcl::PointCloud<PointType>>());
  deskewed_scan_->points.resize(this->original_scan->points.size());

  // individual point timestamps should be relative to this time
  double sweep_ref_time = this->scan_header_stamp.toSec();

  // sort points by timestamp and build list of timestamps
  std::function<bool(const PointType&, const PointType&)> point_time_cmp;
  std::function<bool(boost::range::index_value<PointType&, long>,
                     boost::range::index_value<PointType&, long>)> point_time_neq;
  std::function<double(boost::range::index_value<PointType&, long>)> extract_point_time;

  if (this->sensor == dlio::SensorType::OUSTER) {

    point_time_cmp = [](const PointType& p1, const PointType& p2)
      { return p1.t < p2.t; };
    point_time_neq = [](boost::range::index_value<PointType&, long> p1,
                        boost::range::index_value<PointType&, long> p2)
      { return p1.value().t != p2.value().t; };
    extract_point_time = [&sweep_ref_time](boost::range::index_value<PointType&, long> pt)
      { return sweep_ref_time + pt.value().t * 1e-9f; };

  } else if (this->sensor == dlio::SensorType::VELODYNE) {

    point_time_cmp = [](const PointType& p1, const PointType& p2)
      { return p1.time < p2.time; };
    point_time_neq = [](boost::range::index_value<PointType&, long> p1,
                        boost::range::index_value<PointType&, long> p2)
      { return p1.value().time != p2.value().time; };
    extract_point_time = [&sweep_ref_time](boost::range::index_value<PointType&, long> pt)
      { return sweep_ref_time + pt.value().time; };

  } else if (this->sensor == dlio::SensorType::HESAI) {

    point_time_cmp = [](const PointType& p1, const PointType& p2)
      { return p1.timestamp < p2.timestamp; };
    point_time_neq = [](boost::range::index_value<PointType&, long> p1,
                        boost::range::index_value<PointType&, long> p2)
      { return p1.value().timestamp != p2.value().timestamp; };
    extract_point_time = [&sweep_ref_time](boost::range::index_value<PointType&, long> pt)
      { return pt.value().timestamp; };

  }

  // copy points into deskewed_scan_ in order of timestamp
  std::partial_sort_copy(this->original_scan->points.begin(), this->original_scan->points.end(),
                         deskewed_scan_->points.begin(), deskewed_scan_->points.end(), point_time_cmp);

  // filter unique timestamps
  auto points_unique_timestamps = deskewed_scan_->points
                                  | boost::adaptors::indexed()
                                  | boost::adaptors::adjacent_filtered(point_time_neq);

  // extract timestamps from points and put them in their own list
  std::vector<double> timestamps;
  std::vector<int> unique_time_indices;
  for (auto it = points_unique_timestamps.begin(); it != points_unique_timestamps.end(); it++) {
    timestamps.push_back(extract_point_time(*it));
    unique_time_indices.push_back(it->index());
  }
  unique_time_indices.push_back(deskewed_scan_->points.size());

  int median_pt_index = timestamps.size() / 2;
  this->scan_stamp = timestamps[median_pt_index]; // set this->scan_stamp to the timestamp of the median point

  // don't process scans until IMU data is present
  if (!this->first_valid_scan) {
    if (this->imu_buffer.empty() || this->scan_stamp <= this->imu_buffer.back().stamp) {
      return;
    }

    this->first_valid_scan = true;
    this->T_prior = this->T; // assume no motion for the first scan
    pcl::transformPointCloud (*deskewed_scan_, *deskewed_scan_, this->T_prior * this->extrinsics.baselink2lidar_T);
    this->deskewed_scan = deskewed_scan_;
    this->deskew_status = true;
    return;
  }

  // IMU prior & deskewing for second scan onwards
  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> frames;
  frames = this->integrateImu(this->prev_scan_stamp, this->lidarPose.q, this->lidarPose.p,
                              this->geo.prev_vel.cast<float>(), timestamps);
  this->deskew_size = frames.size(); // if integration successful, equal to timestamps.size()

  // if there are no frames between the start and end of the sweep
  // that probably means that there's a sync issue
  if (frames.size() != timestamps.size()) {
    ROS_FATAL("Bad time sync between LiDAR and IMU!");

    this->T_prior = this->T;
    pcl::transformPointCloud (*deskewed_scan_, *deskewed_scan_, this->T_prior * this->extrinsics.baselink2lidar_T);
    this->deskewed_scan = deskewed_scan_;
    this->deskew_status = false;
    return;
  }

  // update prior to be the estimated pose at the median time of the scan (corresponds to this->scan_stamp)
  this->T_prior = frames[median_pt_index];

#pragma omp parallel for num_threads(this->num_threads_)
  for (int i = 0; i < timestamps.size(); i++) {

    Eigen::Matrix4f T = frames[i] * this->extrinsics.baselink2lidar_T;

    // transform point to world frame
    for (int k = unique_time_indices[i]; k < unique_time_indices[i+1]; k++) {
      auto &pt = deskewed_scan_->points[k];
      pt.getVector4fMap()[3] = 1.;
      pt.getVector4fMap() = T * pt.getVector4fMap();
    }
  }

  this->deskewed_scan = deskewed_scan_;
  this->deskew_status = true;

}
integrateImu
  • 参数说明：
    • start_time ： prev_scan_stamp
    • q_init+p_init： lidarPose
    • v_init：geo.prev_vel
    • sorted_timestamps：{this->scan_stamp}
  • 流程：
    • 若 无 sorted_timestamps 或者 start_time 大于 sorted_timestamps 最晚时间 时，返回空
    • 在 imu队列中 取 小于 start_time，大于 end_time的下标 imuMeasFromTimeRange
    • 记 first_imu： f 1 f_1 f1​、second_imu ： f 2 f_2 f2​为 start_time 前后下标，计算其旋转 （这儿用到了角加速度）
      • q_init 为 f 1 f_1 f1​、 f 2 f_2 f2​ 之间的 旋转
      • f 1 f_1 f1​对应的旋转： q 1 = q i n i t ∗ q { 1 , − 1 2 ( w 1 + v f 2 − v f 1 d t ( t i n i t − t 1 ) ) ∗ ( t i n i t − t 1 ) } q_1 = q_{init} * q\{1,-\frac 1 2(w_1+\frac{v_{f_2}-v_{f_1}}{d_t}(t_{init}-t_1))*(t_{init}-t_1)\} q1​=qinit​∗q{1,−21​(w1​+dt​vf2​​−vf1​​​(tinit​−t1​))∗(tinit​−t1​)}
      • f 2 f_2 f2​对应的旋转的旋转： q 2 = q 1 ∗ q { 1 , 1 2 ( w 1 + 0.5 ( w 2 − w 1 ) ∗ ( t 2 − t 1 ) } q_2 = q_1 * q\{1,\frac 1 2(w_1+0.5(w_2-w_1)*(t_2-t_1)\} q2​=q1​∗q{1,21​(w1​+0.5(w2​−w1​)∗(t2​−t1​)}
    • 将 f 1 、 f 2 f_1、f_2 f1​、f2​的加速度转换到世界坐标系：
      • a 1 w = q 1 ∗ a 1 b a_{1w} = q_1*a_{1b} a1w​=q1​∗a1b​
      • a 2 w = q 1 ∗ a 2 b a_{2w} = q_1*a_{2b} a2w​=q1​∗a2b​
    • 计算两者之间的加加速度，然后计算 f 1 f_1 f1​对应的 v 和 p
      • j = a 2 w − a 1 w d t \mathbf j = \frac {a_{2w}-a_{1w}}{dt} j=dta2w​−a1w​​
      • 令： i d t = t i n i t − t 1 idt = t_{init}-t_1 idt=tinit​−t1​
      • v 1 = v i n i t − a 1 w ∗ ( i d t ) − 1 2 j ( i d t ) 2 v_1 = v_{init}- a_{1w}*(idt) -\frac 1 2 \mathbf j (idt)^2 v1​=vinit​−a1w​∗(idt)−21​j(idt)2
      • p i n i t = p i n i t − v i n i t ∗ i d t + 0.5 ∗ a 1 w ∗ i d t 2 + 1 6 j ( i d t ) 3 p_{init}=p_{init}-v_{init}*idt +0.5 * a_{1w} *idt^2+\frac 1 6 \mathbf j(idt)^3 pinit​=pinit​−vinit​∗idt+0.5∗a1w​∗idt2+61​j(idt)3
    • 返回线性差值 integrateImuInternal 
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
dlio::OdomNode::integrateImu(double start_time, Eigen::Quaternionf q_init, Eigen::Vector3f p_init,
                             Eigen::Vector3f v_init, const std::vector<double>& sorted_timestamps) {

  /// 若无 sorted_timestamps 时，返回空，sorted_timestamps 激光帧时间
  const std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> empty;

  if (sorted_timestamps.empty() || start_time > sorted_timestamps.front()) {
    // invalid input, return empty vector
    return empty;
  }

  /// 取 imu队列中，包围 
  boost::circular_buffer<ImuMeas>::reverse_iterator begin_imu_it;
  boost::circular_buffer<ImuMeas>::reverse_iterator end_imu_it;
  if (this->imuMeasFromTimeRange(start_time, sorted_timestamps.back(), begin_imu_it, end_imu_it) == false) {
    // not enough IMU measurements, return empty vector
    return empty;
  }

  // Backwards integration to find pose at first IMU sample
  const ImuMeas& f1 = *begin_imu_it;
  const ImuMeas& f2 = *(begin_imu_it+1);

  // Time between first two IMU samples
  double dt = f2.dt;

  // Time between first IMU sample and start_time
  double idt = start_time - f1.stamp;

  // Angular acceleration between first two IMU samples
  Eigen::Vector3f alpha_dt = f2.ang_vel - f1.ang_vel;
  Eigen::Vector3f alpha = alpha_dt / dt;

  // Average angular velocity (reversed) between first IMU sample and start_time
  Eigen::Vector3f omega_i = -(f1.ang_vel + 0.5*alpha*idt);

  // Set q_init to orientation at first IMU sample
  q_init = Eigen::Quaternionf (
    q_init.w() - 0.5*( q_init.x()*omega_i[0] + q_init.y()*omega_i[1] + q_init.z()*omega_i[2] ) * idt,
    q_init.x() + 0.5*( q_init.w()*omega_i[0] - q_init.z()*omega_i[1] + q_init.y()*omega_i[2] ) * idt,
    q_init.y() + 0.5*( q_init.z()*omega_i[0] + q_init.w()*omega_i[1] - q_init.x()*omega_i[2] ) * idt,
    q_init.z() + 0.5*( q_init.x()*omega_i[1] - q_init.y()*omega_i[0] + q_init.w()*omega_i[2] ) * idt
  );
  q_init.normalize();

  // Average angular velocity between first two IMU samples
  Eigen::Vector3f omega = f1.ang_vel + 0.5*alpha_dt;

  // Orientation at second IMU sample
  Eigen::Quaternionf q2 (
    q_init.w() - 0.5*( q_init.x()*omega[0] + q_init.y()*omega[1] + q_init.z()*omega[2] ) * dt,
    q_init.x() + 0.5*( q_init.w()*omega[0] - q_init.z()*omega[1] + q_init.y()*omega[2] ) * dt,
    q_init.y() + 0.5*( q_init.z()*omega[0] + q_init.w()*omega[1] - q_init.x()*omega[2] ) * dt,
    q_init.z() + 0.5*( q_init.x()*omega[1] - q_init.y()*omega[0] + q_init.w()*omega[2] ) * dt
  );
  q2.normalize();

  // Acceleration at first IMU sample
  Eigen::Vector3f a1 = q_init._transformVector(f1.lin_accel);
  a1[2] -= this->gravity_;

  // Acceleration at second IMU sample
  Eigen::Vector3f a2 = q2._transformVector(f2.lin_accel);
  a2[2] -= this->gravity_;

  // Jerk between first two IMU samples
  Eigen::Vector3f j = (a2 - a1) / dt;

  // Set v_init to velocity at first IMU sample (go backwards from start_time)
  v_init -= a1*idt + 0.5*j*idt*idt;

  // Set p_init to position at first IMU sample (go backwards from start_time)
  p_init -= v_init*idt + 0.5*a1*idt*idt + (1/6.)*j*idt*idt*idt;

  return this->integrateImuInternal(q_init, p_init, v_init, sorted_timestamps, begin_imu_it, end_imu_it);
}
imuMeasFromTimeRange
  • 返回 imu_deque中 包含 [start_time,end_time]的 下标，并且 起始下标 begin_imu_it，end_imu_it
bool dlio::OdomNode::imuMeasFromTimeRange(double start_time, double end_time,
                                          boost::circular_buffer<ImuMeas>::reverse_iterator& begin_imu_it,
                                          boost::circular_buffer<ImuMeas>::reverse_iterator& end_imu_it) {

  /// 条件唤醒等待，直到 imu数据 的时间戳 包围 end_time
  if (this->imu_buffer.empty() || this->imu_buffer.front().stamp < end_time) {
    // Wait for the latest IMU data
    std::unique_lock<decltype(this->mtx_imu)> lock(this->mtx_imu);
    this->cv_imu_stamp.wait(lock, [this, &end_time]{ return this->imu_buffer.front().stamp >= end_time; });
  }

  // 得到 imu队列中 第一帧 时间戳小于 end_time的 last_imu_it
  auto imu_it = this->imu_buffer.begin();
  auto last_imu_it = imu_it;
  imu_it++;	// 这儿加了一次，因此下面也可加一次
  while (imu_it != this->imu_buffer.end() && imu_it->stamp >= end_time) {
    last_imu_it = imu_it;
    imu_it++;
  }

  //  得到 imu队列中 第一帧 时间戳小于 start_time的 imu_it
  while (imu_it != this->imu_buffer.end() && imu_it->stamp >= start_time) {
    imu_it++;
  }

  // 理论有值，imu_it不应该到最末端
  if (imu_it == this->imu_buffer.end()) {
    // not enough IMU measurements, return false
    return false;
  }
  imu_it++;

  // Set reverse iterators (to iterate forward in time)
  end_imu_it = boost::circular_buffer<ImuMeas>::reverse_iterator(last_imu_it);
  begin_imu_it = boost::circular_buffer<ImuMeas>::reverse_iterator(imu_it);

  return true;
}
integrateImuInternal

步骤：

  • 初始化状态量，在世界坐标系： q , p , v , a q,p,v,a q,p,v,a
  • 变量begin_imu_it->end_imu_it数据，
    • 的前后两 imu， f 0 = f_0= f0​= prev_imu_it， f=prev_imu_it+1
    • 由前一阵imu f 0 f_0 f0​ 数据，计算当前帧 f f f 的信息
      • 计算角加速度： a w = w f − w f 0 d t a_{w}=\frac {w_f - w_{f_0}}{d_t} aw​=dt​wf​−wf0​​​，平均角速度： w a v g = w + 1 2 a w ∗ d t {w_{avg}=w + \frac 1 2 a_w *dt} wavg​=w+21​aw​∗dt
      • 得到 f f f 的旋转： q f = q f 0 ∗ q { 1 , 1 2 w a v g ∗ d t } q_f =q_{f_0}*q\{1,\frac 1 2 w_{avg}*d_t\} qf​=qf0​​∗q{1,21​wavg​∗dt​}
      • 得到 f f f 的线加速度： a w = q f ∗ a b − G ⃗ a_w = q_f * a_b - \vec G aw​=qf​∗ab​−G ，计算线加加速度： j = a w − a 0 w d t j=\frac {a_w - a_{0w}}{dt} j=dtaw​−a0w​​
    • 若 sorted_timestamps的迭代stamp_it时间戳在 该两帧imu时间间隔内，则进行 加速度差值
      • 计算与前一帧的时间差 idt
      • 计算旋转： q i = q f 0 ∗ q { 1 , 1 2 ( w 0 + 1 2 a w i d t ) ∗ i d t } q_{i} =q_{f_0}*q\{1,\frac 1 2 (w_0+\frac 1 2 a_w \mathbf{idt})*\mathbf{idt}\} qi​=qf0​​∗q{1,21​(w0​+21​aw​idt)∗idt}
      • 位置： p i = p 0 + v 0 ∗ i d t + 1 2 a 0 ∗ i d t 2 + 1 6 j ∗ i d t 3 p_i = p_0+v_0*\mathbf{idt}+\frac 1 2 a_0*\mathbf{idt}^2+\frac 1 6 j * \mathbf{idt}^3 pi​=p0​+v0​∗idt+21​a0​∗idt2+61​j∗idt3
      • 将该位姿加起来，并 stamp_it++
    • 计算 当前imu帧 f f f的 位置，速度
      • p j = p 0 + v 0 ∗ d t + 1 2 a 0 ∗ d t 2 + 1 6 j ∗ a 0 3 p_j = p_0+v_0*d_t+\frac 1 2 a_0*d_t^2+\frac 1 6 j * a_0^3 pj​=p0​+v0​∗dt​+21​a0​∗dt2​+61​j∗a03​
      • v j = v 0 + a 0 ∗ d t + 1 2 j ∗ d t 2 v _j = v_0+ a_0*d_t + \frac 1 2 j * d_t^2 vj​=v0​+a0​∗dt​+21​j∗dt2​
    • prev_imu_it = imu_it
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
dlio::OdomNode::integrateImuInternal(Eigen::Quaternionf q_init, Eigen::Vector3f p_init, Eigen::Vector3f v_init,
                                     const std::vector<double>& sorted_timestamps,
                                     boost::circular_buffer<ImuMeas>::reverse_iterator begin_imu_it,
                                     boost::circular_buffer<ImuMeas>::reverse_iterator end_imu_it) {

  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> imu_se3;

  // Initialization
  Eigen::Quaternionf q = q_init;
  Eigen::Vector3f p = p_init;
  Eigen::Vector3f v = v_init;
  Eigen::Vector3f a = q._transformVector(begin_imu_it->lin_accel);
  a[2] -= this->gravity_;

  // Iterate over IMU measurements and timestamps
  auto prev_imu_it = begin_imu_it;
  auto imu_it = prev_imu_it + 1;

  auto stamp_it = sorted_timestamps.begin();

  for (; imu_it != end_imu_it; imu_it++) {

    const ImuMeas& f0 = *prev_imu_it;
    const ImuMeas& f = *imu_it;

    // Time between IMU samples
    double dt = f.dt;

    // Angular acceleration
    Eigen::Vector3f alpha_dt = f.ang_vel - f0.ang_vel;
    Eigen::Vector3f alpha = alpha_dt / dt;

    // Average angular velocity
    Eigen::Vector3f omega = f0.ang_vel + 0.5*alpha_dt;

    // Orientation
    q = Eigen::Quaternionf (
      q.w() - 0.5*( q.x()*omega[0] + q.y()*omega[1] + q.z()*omega[2] ) * dt,
      q.x() + 0.5*( q.w()*omega[0] - q.z()*omega[1] + q.y()*omega[2] ) * dt,
      q.y() + 0.5*( q.z()*omega[0] + q.w()*omega[1] - q.x()*omega[2] ) * dt,
      q.z() + 0.5*( q.x()*omega[1] - q.y()*omega[0] + q.w()*omega[2] ) * dt
    );
    q.normalize();

    // Acceleration
    Eigen::Vector3f a0 = a;
    a = q._transformVector(f.lin_accel);
    a[2] -= this->gravity_;

    // Jerk
    Eigen::Vector3f j_dt = a - a0;
    Eigen::Vector3f j = j_dt / dt;

    // Interpolate for given timestamps
    while (stamp_it != sorted_timestamps.end() && *stamp_it <= f.stamp) {
      // Time between previous IMU sample and given timestamp
      double idt = *stamp_it - f0.stamp;

      // Average angular velocity
      Eigen::Vector3f omega_i = f0.ang_vel + 0.5*alpha*idt;

      // Orientation
      Eigen::Quaternionf q_i (
        q.w() - 0.5*( q.x()*omega_i[0] + q.y()*omega_i[1] + q.z()*omega_i[2] ) * idt,
        q.x() + 0.5*( q.w()*omega_i[0] - q.z()*omega_i[1] + q.y()*omega_i[2] ) * idt,
        q.y() + 0.5*( q.z()*omega_i[0] + q.w()*omega_i[1] - q.x()*omega_i[2] ) * idt,
        q.z() + 0.5*( q.x()*omega_i[1] - q.y()*omega_i[0] + q.w()*omega_i[2] ) * idt
      );
      q_i.normalize();

      // Position
      Eigen::Vector3f p_i = p + v*idt + 0.5*a0*idt*idt + (1/6.)*j*idt*idt*idt;

      // Transformation
      Eigen::Matrix4f T = Eigen::Matrix4f::Identity();
      T.block(0, 0, 3, 3) = q_i.toRotationMatrix();
      T.block(0, 3, 3, 1) = p_i;

      imu_se3.push_back(T);

      stamp_it++;
    }

    // Position
    p += v*dt + 0.5*a0*dt*dt + (1/6.)*j_dt*dt*dt;

    // Velocity
    v += a0*dt + 0.5*j_dt*dt;

    prev_imu_it = imu_it;

  }

  return imu_se3;

}

Code-Map

构造

  • 读取参数：

    • odom_frame="odom"; publish_full_map_=true; publish_freq_=1.0; leaf_size_=0.5;
    • 获取节点 NS 并删除前导字符 ns
    • 连接帧名称字符串 odom_frame = ns + "/" + this->odom_frame;

  • 按一定频率 发布 全局地图：pointcloud2

  • 订阅 topic

    • 关键帧：“keyframes”, 1, &dlo::MapNode::keyframeCB,
    • 保存地图指令：“save_pcd”, &dlio::MapNode::savePcd 
dlio::MapNode::MapNode(ros::NodeHandle node_handle) : nh(node_handle) {

  this->getParams();

  this->publish_timer = this->nh.createTimer(ros::Duration(this->publish_freq_), &dlio::MapNode::publishTimer, this);

  this->keyframe_sub = this->nh.subscribe("keyframes", 10,
      &dlio::MapNode::callbackKeyframe, this, ros::TransportHints().tcpNoDelay());
  this->map_pub = this->nh.advertise<sensor_msgs::PointCloud2>("map", 100);
  this->save_pcd_srv = this->nh.advertiseService("save_pcd", &dlio::MapNode::savePcd, this);

  this->dlio_map = pcl::PointCloud<PointType>::Ptr (boost::make_shared<pcl::PointCloud<PointType>>());

  pcl::console::setVerbosityLevel(pcl::console::L_ERROR);

}

callbackKeyframe

  • 将关键帧数据格式由 ros->pcl pcl::fromROSMsg
  • 关键帧点云数据进行体素滤波降采样 voxelgrid.filter
  • 关键帧加载到全局地图中 pcl格式，直接+号 
void dlio::MapNode::callbackKeyframe(const sensor_msgs::PointCloud2ConstPtr& keyframe) {

  // convert scan to pcl format
  pcl::PointCloud<PointType>::Ptr keyframe_pcl =
    pcl::PointCloud<PointType>::Ptr (boost::make_shared<pcl::PointCloud<PointType>>());
  pcl::fromROSMsg(*keyframe, *keyframe_pcl);

  // voxel filter
  this->voxelgrid.setLeafSize(this->leaf_size_, this->leaf_size_, this->leaf_size_);
  this->voxelgrid.setInputCloud(keyframe_pcl);
  this->voxelgrid.filter(*keyframe_pcl);

  // save filtered keyframe to map for rviz
  *this->dlio_map += *keyframe_pcl;

}

savePcd

  • 地图进行提速滤波后保存

步骤：文章来源地址https://www.toymoban.com/news/detail-701398.html

  • 取地图体素大小 + 保存路径
  • 将 全局地图 进行体素降采样
  • 保存

GICP

calculateSourceCovariances

End

到了这里，关于Direct LiDAR-Inertial Odometry的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处： 如若内容造成侵权/违法违规/事实不符，请点击违法举报进行投诉反馈，一经查实，立即删除！

分享到：
领支付宝红包 赞助服务器费用

相关文章

  • 相机雷达标定direct_visual_lidar_calibration部署

    之前探索出来autoware适合标定,但是到现场发现autoware不仅采集数据麻烦,同份数据标定出来的值稳定性和重复性比较差,所以重新寻找相关方案. 最终发现direct_visual_lidar_calibration 比较符合需求,编译依赖需要显示依赖Iridescence,下载了好久才成功了,所以我将这个包上传到csdn,方便下

    2024年02月16日
    浏览(50)
  • 论文阅读及复现——《CT_ICP: Real-time Elastic LiDAR Odometry with Loop Closure》

    论文阅读及复现——《CT_ICP: Real-time Elastic LiDAR Odometry with Loop Closure》

    论文网址：https://arxiv.org/abs/2109.12979 源码网址：https://github.com/jedeschaud/ct_icp 复现过程：https://blog.csdn.net/qq_44164791/article/details/132188049?spm=1001.2014.3001.5502 提出了一个具有扫 描内姿态连续性 和 扫描间不连续性 的新型弹性激光雷达里程计 使用稀疏体素结构存储稠密点云的局部地

    2024年02月11日
    浏览(38)

  • 【雷达相机外参标定】direct_visual_lidar_calibration安装

    标定雷达和相机时使用direct_visual_lidar_calibration。 https://github.com/koide3/direct_visual_lidar_calibration?tab=readme-ov-file https://koide3.github.io/direct_visual_lidar_calibration/ 主页中有安装指南，但不详细，没有表明依赖包版本。 以下流程仅适用于ubuntu20.04 ros1 。笔者尝试使用ubuntu18.04安装，会遇到

    2024年04月27日
    浏览(44)
  • PSEUDO-LIDAR++：自动驾驶中 3D 目标检测的精确深度

    PSEUDO-LIDAR++：自动驾驶中 3D 目标检测的精确深度

    论文地址：PSEUDO-LIDAR++: ACCURATE DEPTH FOR 3D OBJECT DETECTION IN AUTONOMOUS DRIVING 论文代码：https://github.com/mileyan/Pseudo_Lidar_V2 3D 检测汽车和行人等物体在自动驾驶中发挥着不可或缺的作用。现有方法很大程度上依赖昂贵的激光雷达传感器来获取准确的深度信息。虽然最近推出了伪激光雷

    2024年01月23日
    浏览(48)

  • Matlab 一种基于机载LiDAR点云电力线自动提取方法之二

    之前的方法在面对地面为水面时，由于地面点的缺失会导致电力线提取错误，因此这里使用CSF地面点滤波改进电力线的提取过程。关于CSF滤波的相关配置可以详看：Matlab CSF地面点滤波（插件），改进之后的代码如下所示。

    2024年01月19日
    浏览(41)
  • 自动驾驶 2D 单目\双目\多目视觉方法 一（Pseudo-LiDAR，Mono3D，FCOS3D，PSMNet）

    自动驾驶 2D 单目\双目\多目视觉方法 一（Pseudo-LiDAR，Mono3D，FCOS3D，PSMNet）

    自动驾驶中必不可少的3D场景感知。因为深度信息、目标三维尺寸等在2D感知中是无法获得的，而这些信息才是自动驾驶系统对周围环境作出正确判断的关键。想得到3D信息，最直接的方法就是采用激光雷达（LiDAR）。但是，LiDAR也有其缺点，比如成本较高，车规级产品量产困难

    2024年02月02日
    浏览(38)
  • ORB_SLAM3启动流程以stereo_inertial_realsense_D435i为例

    ORB_SLAM3启动流程以stereo_inertial_realsense_D435i为例

    概述 ORB-SLAM3 是第一个同时具备纯视觉（visual）数据处理、视觉+惯性（visual-inertial）数据处理、和构建多地图（multi-map）功能，支持单目、双目以及 RGB-D 相机，同时支持针孔相机、鱼眼相机模型的 SLAM 系统。 主要创新点： 1.在 IMU 初始化阶段引入 MAP。该初始化方法可以实时

    2024年02月12日
    浏览(48)

  • 视觉增强RTK论文（1）—— GNSS-Stereo-Inertial SLAM for Arable Farming

    农业任务自动化速度的加快要求现场机器人采用高精度和鲁棒的定位系统。同时定位和映射（SLAM）方法不可避免地会在探索性轨迹上积累漂移，并且主要依赖于位置重新访问和循环闭合来保持一个有界的全局定位误差。环状闭合技术在农田中具有显著的挑战性，因为不同视野

    2024年02月03日
    浏览(43)
  • 【KITTI数据集Odometry序列00-10标定文件中的参数关系解读】

    【KITTI数据集Odometry序列00-10标定文件中的参数关系解读】

    KITTI Odometry数据集是大量研究感知、salm、跟踪学者经常接触的数据集。但是对于新手来说，下载相应数据集进行坐标转换的过程中，往往会碰到对calib、pose文件中诸多矩阵不理解的情况。本人查找相关资料的时候发现，针对KITTI Odometry数据集的参数文件解释很少。因此，本人

    2024年02月15日
    浏览(34)
  • SimVODIS++: Neural Semantic Visual Odometry in Dynamic Environments 论文阅读

    SimVODIS++: Neural Semantic Visual Odometry in Dynamic Environments 论文阅读

    题目 ：SimVODIS++: Neural Semantic Visual Odometry in Dynamic Environments 作者 ：Ue-Hwan Kim , Se-Ho Kim , and Jong-Hwan Kim , Fellow, IEEE 时间 ：2022 来源 ： IEEE ROBOTICS AND AUTOMATION LETTERS（RAL） 语义的缺乏和动态对象导致的性能下降阻碍了其在现实场景中的应用。 为了克服这些限制，我们在Simultanero

    2024年02月09日
    浏览(42)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包，优惠每天领

二维码1

领取红包

二维码2

领红包