Direct LiDAR Odometry

xiaoma_bk

已于 2023-09-08 18:05:38 修改

阅读量2.4k

点赞数 7

分类专栏： SLAM 技术相关文章标签：人工智能

于 2023-02-28 19:20:08 首次发布

本文链接：https://blog.csdn.net/xiaoma_bk/article/details/129267978

版权

SLAM 技术相关专栏收录该内容

3 篇文章 1 订阅

订阅专栏

DLO-SLAM

这篇SLAM论文《Direct LiDAR Odometry: Fast Localization with Dense Point Clouds》作为NASA喷气推进实验室CoSTAR团队研究和开发的新作，收到了学术界广泛的关注。其主要用作DARPA地下挑战的里程计，提出了一种能够实现高速高精度处理高速实时处理密集点云的激光里程计(LO)的思路，下面是他们的Github开源代码。

点评

代码实现中一些小的 trick还是不错的。
本文采用稠密点云进行匹配，说是：精度比提取特征(loam系列)的高。
由于计算量大的问题，本文首先不进行点云去畸变降低耗时，同时动态确定阈值筛选关键帧
自适应阈值筛选关键帧，窄道时阈值会缩小。
采用激光与激光匹配和激光与submap匹配，其中submap采用关键帧空间构建子图。
子图由邻近关键帧+关键帧凸包+关键帧凹包构成，基于帧标记确定是否需要重新生成。
本文采用 NanoGIcp 轻量级激光点匹配。
子图协方差、法向量由链接关键帧近似，代码中采用scan2scan直接赋值，不需重新计算

论文

在这里插入图片描述

点云预处理，比较简单，应用了两个滤波器，

一是滤除了自身周围1立方米的点
二是通过单位为0.25m的体素格降采样，不需要像loam那样提取特征点。

GICP的位姿计算

分两部：scan-to-scan + point-to-map

scan-to-scan

$\hat X^L_k = \underset{X^L_k}{arg \min} \varepsilon (X^L_kP_k^s,P_k^t)$
- 其中：转换关系 $\hat X^L_k$ ，源点 $P^s_k$ ，目标 $P_k^t$ ，其中 $P_k^t= P_{k-1}^s$
- 其中误差项： $\varepsilon (X^L_kP_k^s,P_k^t) = \sum_i^N d_i^T(C^t_{k,i}+X^L_k C^s_{k,i} {X^L_k}^T)^{-1}d_i$
因此 $\hat X^L_k = \underset{X^L_k}{arg \min} \sum_i^N d_i^T(C^t_{k,i}+X^L_k C^s_{k,i} {X^L_k}^T)^{-1}d_i$
N个相关点形成的误差函数， $d_i = p_i^t-X_k^L p_i^s,\ \ \ p_i^s \in P_k^s,p_i^t \in P_k^t,\forall i$
先验： IMU ？ $\tilde X_k^L = \tilde X_k^B$ : $\tilde X_k^L = \mathbf I$

scan-to-map

$\hat X^W_k = \underset{X^W_k}{arg \min} \varepsilon (X^W_kP_k^s,S_k)$
- $\hat X^W_k = \underset{X^L_k}{arg \min} \sum_j^M d_i^T(C^S_{k,i}+X^W_k C^s_{k,i} {X^W_k}^T)^{-1}d_i$
- 初始位姿使用scan-to-scan： $\tilde X^W_k = \hat X^W_{k-1} \hat X^L_k$

优化先验

$\hat {w}_k = w_k +b^w_k +n^w_k$
$q_{k+1}=q_k+(\frac 1 2 q_k \otimes w_k)\Delta t$
zero translation

基于关键帧的快速子图优化

并不是直接将点云存储到kdtree中，而是创建一个关键帧的副本点云进行搜索，其中每个关键帧都与关键帧字典中的相应点云关联。因此，用于帧图匹配的局部子图，可以通过字典中的点云串联生成，而不用通过地理位置的搜索。
这样做的好处有两方面：一是关键帧空间比点云空间更加节约计算资源；二是基于关键帧的子图构建的子图比基于距离的子图构建的子图更加宽松，这个意味着基于距离的子图构建重复位置比较多，而基于关键帧的子图具有较少的重复。
子图的拼接： $C^S_K = \sum ^N_n C^S_{k,n}$

自适应关键帧

Nano-GICP

FastGICP + NanoFLANN 两个包进行合并
使用NanoGICP的NanoFLANN来高效构建KD-tree, 然后通过FastGICP进行高效匹配.

OdomNode

代码结构

在这里插入图片描述

主函数 main

描述：

主函数基本啥也没干，就定义了OdomNode，然后等待ros结束，捕捉了程序终止的指令
OdomNode 主要是两个回调，激光和imu回调，下面有其详细的描述

void controlC(int sig) {

  dlo::OdomNode::abort();

}

int main(int argc, char** argv) {

  ros::init(argc, argv, "dlo_odom_node");
  ros::NodeHandle nh("~");

  signal(SIGTERM, controlC);
  sleep(0.5);

  dlo::OdomNode node(nh);
  ros::AsyncSpinner spinner(0);
  spinner.start();
  node.start();
  ros::waitForShutdown();

  return 0;

}

核心为 dlo::OdomNode

核心成员：

// 订阅
ros::Subscriber icp_sub;
ros::Subscriber imu_sub;
this->icp_sub = this->nh.subscribe("pointcloud", 1, &dlo::OdomNode::icpCB, this);
this->imu_sub = this->nh.subscribe("imu", 1, &dlo::OdomNode::imuCB, this);


//发布
ros::Publisher odom_pub;
ros::Publisher pose_pub;
ros::Publisher keyframe_pub;
ros::Publisher kf_pub;
this->odom_pub = this->nh.advertise<nav_msgs::Odometry>("odom", 1);
this->pose_pub = this->nh.advertise<geometry_msgs::PoseStamped>("pose", 1);
this->kf_pub = this->nh.advertise<nav_msgs::Odometry>("kfs", 1, true);
this->keyframe_pub = this->nh.advertise<sensor_msgs::PointCloud2>("keyframe", 1, true);

回调

激光回调函数 icpCB

主要功能：

imu -> s2s -> s2m 的一个match 得到姿态
说明：
- 红色：代表调用的函数，有些短的步骤已经在下面解释了
- 绿色：代表一些文章独特名词
- 蓝色：代表一些理解

步骤：

两个检测，不满足直接返回
- 若点云中点个数过少
- DLO 程序未初始化，则初始化（IMU 校准，重力对齐） initializeDLO
  - 若使用imu 但未标定时，直接返回
  - 若使用imu+已经标定+需重力对齐+未初始化
    - 进行重力对齐 gravityAlign
  - 若提供初始位姿，则初始位姿进行赋值 ·initial_position_·
    - 该步骤可以使用初始位姿，代表可以连续建图
数据预处理，点云+关键帧阈值
- 点云预处理（去 Nan点 + 区域滤波+ 体素滤波） preprocessPoints
  - 去除Nan：pcl::removeNaNFromPointCloud
  - Crop box滤波：crop.filter
  - 体素滤波：vf_scan.filter
- 计算关键帧指标，现启独立线程计算 dlo::OdomNode::computeMetrics
  - 该帧点云长度中位数进行低通滤波，然后基于长度设定关键帧选择阈值
    - computeSpaciousness 平均长度
    - 计算当前激光水平长度 $\sqrt{x^2+y^2}$ 的中位数，并用低通滤波器均衡化 $0.95L_{mi}+0.5L_{curr}$
    - 变量：spaciousness
- 基于指标确定关键帧阈值 setAdaptiveParams
- 若目标信息为空时，进行初始化 initializeInputTarget，然后 return
当前帧设为源数据 setInputSources，得到当前帧的位姿 getNextPose
- 设置 S2S gicp 的输入源 gicp_s2s.setInputSource \ \ gicp.registerInputSource
- 当前帧的位姿： IMU + S2S + S2M 三者共同作用得到的
更新关键帧 updateKeyframes ，得到关键帧周围数量 + 最近关键帧距离及id
启动独立线程，发送数据到ros publishToROS
启动独立线程，调试语句并发布自定义 DLO 消息

/**
 * ICP Point Cloud Callback
 **/
void dlo::OdomNode::icpCB(const sensor_msgs::PointCloud2ConstPtr& pc) {

  double then = ros::Time::now().toSec();
  /// 当前时刻更新
  this->scan_stamp = pc->header.stamp;
  this->curr_frame_stamp = pc->header.stamp.toSec();

  // If there are too few points in the pointcloud, try again
  /// 如果点云中的点太少，该帧直接丢弃
  this->current_scan = pcl::PointCloud<PointType>::Ptr (new pcl::PointCloud<PointType>);
  pcl::fromROSMsg(*pc, *this->current_scan);
  if (this->current_scan->points.size() < this->gicp_min_num_points_) {
    ROS_WARN("Low number of points!");
    return;
  }

  // DLO Initialization procedures (IMU calib, gravity align)
  /// DLO 初始化程序（IMU 校准，重力对齐）
  if (!this->dlo_initialized) {
    this->initializeDLO();
    return;
  }

  // Preprocess points 预处理点云
  this->preprocessPoints();

  // Compute Metrics  计算关键帧指标，独立线程计算
  this->metrics_thread = std::thread( &dlo::OdomNode::computeMetrics, this );
  this->metrics_thread.detach(); // detach()的作用是将子线程和主线程的关联分离

  // Set Adaptive Parameters  根据空间度量设置关键帧阈值
  if (this->adaptive_params_use_){
    this->setAdaptiveParams();
  }

  // Set initial frame as target  初始化目标信息
  if(this->target_cloud == nullptr) {
    this->initializeInputTarget();
    return;
  }

  // Set source frame 设置源数据
  this->source_cloud = pcl::PointCloud<PointType>::Ptr (new pcl::PointCloud<PointType>);
  this->source_cloud = this->current_scan;
  // Set new frame as input source for both gicp objects
  this->setInputSources();

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

  // 更新当前关键帧，得到关键帧周围数量 + 最近关键帧距离及id
  // Update current keyframe poses and map
  this->updateKeyframes();

  // Update trajectory  更新轨迹
  this->trajectory.push_back( std::make_pair(this->pose, this->rotq) );

  // Update next time stamp
  this->prev_frame_stamp = this->curr_frame_stamp;

  // Update some statistics 计算本次耗时
  this->comp_times.push_back(ros::Time::now().toSec() - then);

  // Publish stuff to ROS  启动独立线程，发送数据到ros
  this->publish_thread = std::thread( &dlo::OdomNode::publishToROS, this );
  this->publish_thread.detach();

  // Debug statements and publish custom DLO message
  // 启动独立线程，调试语句并发布自定义 DLO 消息
  this->debug_thread = std::thread( &dlo::OdomNode::debug, this );
  this->debug_thread.detach();

}

imu回调 imuCB

功能：

该回调中主要做的活：先静止几秒算bias，然后数据去偏差放入队列中

步骤：

得到imu的测量数据，角速度+线速度
若未标定过，则进行求bias偏差
- 先取静止三秒的陀螺仪和加速度计数据
- 然后求均值作为偏差
将测量数据减去bias得到实际测量值
将实际测量值放入队列中

void dlo::OdomNode::imuCB(const sensor_msgs::Imu::ConstPtr& imu) {
	
  // 不使用imu时，直接return
  if (!this->imu_use_) {
    return;
  }

  double ang_vel[3], lin_accel[3];

  // Get IMU samples 得到imu的测量数据，角速度+线速度
  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_time == 0.) {
    this->first_imu_time = imu->header.stamp.toSec();
  }

  // IMU calibration procedure - do for three seconds IMU 校准程序 - 做三秒钟 
  if (!this->imu_calibrated) { //imu未标定时

    static int num_samples = 0;
    static bool print = true;

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

      num_samples++;

      this->imu_bias.gyro.x += ang_vel[0];
      this->imu_bias.gyro.y += ang_vel[1];
      this->imu_bias.gyro.z += ang_vel[2];

      this->imu_bias.accel.x += lin_accel[0];
      this->imu_bias.accel.y += lin_accel[1];
      this->imu_bias.accel.z += lin_accel[2];

      // 一次打印，imu开始标定
      if(print) {
        std::cout << "Calibrating IMU for " << this->imu_calib_time_ << " seconds... "; std::cout.flush();
        print = false;
      }

    } else {

      // 计算 陀螺仪b和加速度g的平均偏差，标定成功
      this->imu_bias.gyro.x /= num_samples;
      this->imu_bias.gyro.y /= num_samples;
      this->imu_bias.gyro.z /= num_samples;

      this->imu_bias.accel.x /= num_samples;
      this->imu_bias.accel.y /= num_samples;
      this->imu_bias.accel.z /= num_samples;

      this->imu_calibrated = true;

      std::cout << "done" << std::endl;
      std::cout << "  Gyro biases [xyz]: " << this->imu_bias.gyro.x << ", " << this->imu_bias.gyro.y << ", " << this->imu_bias.gyro.z << std::endl << std::endl;

    }

  } else {

    // Apply the calibrated bias to the new IMU measurements
    this->imu_meas.stamp = imu->header.stamp.toSec();
	
    // 去过畸变的 bias，将其放入队列中
    this->imu_meas.ang_vel.x = ang_vel[0] - this->imu_bias.gyro.x;
    this->imu_meas.ang_vel.y = ang_vel[1] - this->imu_bias.gyro.y;
    this->imu_meas.ang_vel.z = ang_vel[2] - this->imu_bias.gyro.z;

    this->imu_meas.lin_accel.x = lin_accel[0];
    this->imu_meas.lin_accel.y = lin_accel[1];
    this->imu_meas.lin_accel.z = lin_accel[2];

    // Store into circular buffer
    this->mtx_imu.lock();
    this->imu_buffer.push_front(this->imu_meas);
    this->mtx_imu.unlock();

  }

}

其他函数

重力对齐 gravityAlign

步骤：

原理：缓存1s以内加速度的值，将初始方向设为：重力方向与加速度方向的夹角
取一秒之内的 imu 加速度，并计算其均值加速度，并归一化
定义重力矢量（假设点向下） $\mathbf{grav}= \{ 0,0,1\}$
计算两个向量之间的角度，作为重力对齐方向，并进行归一化 Eigen::Quaternionf::FromTwoVectors
设置重力对齐方向，并打印 grav_q.toRotationMatrix().eulerAngles(2, 1, 0)

void dlo::OdomNode::gravityAlign() {

  // get average acceleration vector for 1 second and normalize
  Eigen::Vector3f lin_accel = Eigen::Vector3f::Zero();
  
  ///< 取一秒之内的 imu 加速度，并计算其 均值加速度  
  const double then = ros::Time::now().toSec();
  int n=0;
  while ((ros::Time::now().toSec() - then) < 1.) {
    lin_accel[0] += this->imu_meas.lin_accel.x;
    lin_accel[1] += this->imu_meas.lin_accel.y;
    lin_accel[2] += this->imu_meas.lin_accel.z;
    ++n;
  }
  lin_accel[0] /= n; lin_accel[1] /= n; lin_accel[2] /= n;

  // normalize  归一化（单位1）
  double lin_norm = sqrt(pow(lin_accel[0], 2) + pow(lin_accel[1], 2) + pow(lin_accel[2], 2));
  lin_accel[0] /= lin_norm; lin_accel[1] /= lin_norm; lin_accel[2] /= lin_norm;

  // define gravity vector (assume point downwards) 定义重力矢量（假设点向下）
  Eigen::Vector3f grav;
  grav << 0, 0, 1;

  // calculate angle between the two vectors 计算两个向量之间的角度
  Eigen::Quaternionf grav_q = Eigen::Quaternionf::FromTwoVectors(lin_accel, grav);

  // normalize  角度进行归一化
  double grav_norm = sqrt(grav_q.w()*grav_q.w() + grav_q.x()*grav_q.x() + grav_q.y()*grav_q.y() + grav_q.z()*grav_q.z());
  grav_q.w() /= grav_norm; grav_q.x() /= grav_norm; grav_q.y() /= grav_norm; grav_q.z() /= grav_norm;

  // set gravity aligned orientation  设置重力对齐方向
  this->rotq = grav_q;
  this->T.block(0,0,3,3) = this->rotq.toRotationMatrix();
  this->T_s2s.block(0,0,3,3) = this->rotq.toRotationMatrix();
  this->T_s2s_prev.block(0,0,3,3) = this->rotq.toRotationMatrix();

  // 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);

  std::cout << "done" << std::endl;
  std::cout << "  Roll [deg]: " << roll << std::endl;
  std::cout << "  Pitch [deg]: " << pitch << std::endl << std::endl;
}

设定关键帧阈值setAdaptiveParams

步骤：

根据空间度量spaciousness设置关键帧阈值
关键帧步长：
- 若激光均值长度 $[20,+\infty]$ ：10.0 m
- 若激光均值长度 $[10, 20]$ ：5.0 m
- 若激光均值长度 $[5, 10]$ ：1.0 m
- 若激光均值长度 $[0.0, 5.0]$ ：0.5 m

void dlo::OdomNode::setAdaptiveParams() {

  // Set Keyframe Thresh from Spaciousness Metric
  if (this->metrics.spaciousness.back() > 20.0){
    this->keyframe_thresh_dist_ = 10.0;
  } else if (this->metrics.spaciousness.back() > 10.0 && this->metrics.spaciousness.back() <= 20.0) {
    this->keyframe_thresh_dist_ = 5.0;
  } else if (this->metrics.spaciousness.back() > 5.0 && this->metrics.spaciousness.back() <= 10.0) {
    this->keyframe_thresh_dist_ = 1.0;
  } else if (this->metrics.spaciousness.back() <= 5.0) {
    this->keyframe_thresh_dist_ = 0.5;
  }

  // set concave hull alpha
  this->concave_hull.setAlpha(this->keyframe_thresh_dist_);
}

updateKeyframes

步骤：

将当前点云基于全局位姿转换到 map坐标系 transformCurrentScan
- this->current_scan_t = pcl::PointCloud::Ptr (new pcl::PointCloud);
  pcl::transformPointCloud (*this->current_scan, *this->current_scan_t, this->T);
遍历关键帧，计算当前姿势附近的数量 + 最近距离及id
- 计算当前姿势和关键帧姿势之间的距离 $\Delta d = ||p_{state}-p_{i}||$
- 统计小于1.5倍阈值的个数 num_nearby：
  - $\Delta d < 1.5 * \mathbf{keyframe\_thresh\_dist\_}$ ? ++num_nearby;
- 存储最近的下标closest_idx以及最近的距离closest_d
得到最近的位置+旋转，进而得到与当前state的差异(位置+角度)
- 位置差异： $\Delta d = ||p_{state}-p_{i}||$
- 角度差异：
  - $\delta q = |q_{state}*q_{close}^{-1}|$
  - 得到旋转角度： $\theta = 2*\mathbf{atan2}\{\delta q_{vec.norm()},\delta q_w\}$
  - 旋转角度差异： $\Theta = 180. * \theta / \pi$
新关键帧评判标准，感觉1，2 可以合起来确定是否为关键帧
- 1、若位置差异大于位置阈值或者角度差异大于角度阈值时，newKeyframe = true;
- 2、若位置差异小于位置阈值时，newKeyframe = false;
- 3、若位置差异小于位置阈值或者角度差异大于角度阈值且满足位置小于1.5倍的个数<=1 时，newKeyframe = true;
如果当前帧添加新关键帧，放入关键帧队列和normal队列
- keyframes .push
- keyframe_normals 来自 gicp_s2s.calculateSourceCovariances()

void dlo::OdomNode::updateKeyframes() {

  // transform point cloud
  this->transformCurrentScan();

  // 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->pose[0] - k.first.first[0], 2) + pow(this->pose[1] - k.first.first[1], 2) + pow(this->pose[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->pose[0] - closest_pose[0], 2) + pow(this->pose[1] - closest_pose[1], 2) + pow(this->pose[2] - closest_pose[2], 2) );

  // calculate difference in orientation
  Eigen::Quaternionf dq = this->rotq * (closest_pose_r.inverse());

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

  // update keyframe
  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) {

    ++this->num_keyframes;

    // voxelization for submap
    if (this->vf_submap_use_) {
      this->vf_submap.setInputCloud(this->current_scan_t);
      this->vf_submap.filter(*this->current_scan_t);
    }

    // update keyframe vector
    this->keyframes.push_back(std::make_pair(std::make_pair(this->pose, this->rotq), this->current_scan_t));

    // compute kdtree and keyframe normals (use gicp_s2s input source as temporary storage because it will be overwritten by setInputSources())
    *this->keyframes_cloud += *this->current_scan_t;
    *this->keyframe_cloud = *this->current_scan_t;

    this->gicp_s2s.setInputSource(this->keyframe_cloud);
    this->gicp_s2s.calculateSourceCovariances();
    this->keyframe_normals.push_back(this->gicp_s2s.getSourceCovariances());

    this->publish_keyframe_thread = std::thread( &dlo::OdomNode::publishKeyframe, this );
    this->publish_keyframe_thread.detach();

  }

}

初始化目标数据 initializeInputTarget

步骤：

先将当前点云数据转为 NanoGICP 格式的数据
- gicp_s2s 设置 Target，gicp_s2s.setInputTarget
- 并求协方差 gicp_s2s.calculateTargetCovariances
初始化关键帧 first_keyframe
若使用 submap时，初始化 submap，并进行体素滤波 vf_submap
保留历史关键帧树
- 关键帧位姿+ 是否第一次 keyframes.push_back
- 关键帧点云 keyframes_cloud
- 当前关键帧点云 keyframe_cloud
计算 kdtree 和关键帧法线
- gicp_s2s 设置： gicp_s2s.setInputSource
- gicp_s2s 计算协方差：gicp_s2s.calculateSourceCovariances();
- 关键帧法线：keyframe_normals.push_back(this->gicp_s2s.getSourceCovariances()
发布关键帧点云，独立线程 publish_keyframe_thread = std::thread
关键帧个数+1

得到下一个位姿 getNextPose

步骤：

基于s2s得到全局姿态，先得到相对关系 $^{pre}_{cur}T$ ，在得全局位姿 $^G_{now}T=^G_{pre}T*^{pre}_{cur}T$
- 若使用imu时，则imu预积分得帧间位姿 integrateIMU： imu_SE3
  - 角度：取[pre_frame,curr_frame_stamp) IMU预积分
  - 位置： $[0 ， 0 ， 0]$
- 否则：imu_SE3 作为初值，得到 s2s的关系 gicp_s2s.align(*aligned
- 基于上次全局姿态进而得到当前位姿位姿 propagateS2S
得到 submap地图帧
- 先基于当前位姿得到submap地图帧索引 getSubmapKeyframes
  - 欧式距离 dist + 凸包距离dist + 凹包距离dist
- 若索引有变化，则重新生成地图+法向量 gicp.setInputTarget +gicp.setTargetCovariances
以 s2s 为初值，进行 s2m匹配，得到最终的全局位姿 gicp.align(*aligned, this->T_s2s);
数据进行更新，s2s值更新，last值更新等 propagateS2M()

void dlo::OdomNode::getNextPose() {

  //
  // FRAME-TO-FRAME PROCEDURE
  //

  // Align using IMU prior if available
  pcl::PointCloud<PointType>::Ptr aligned (new pcl::PointCloud<PointType>);

  // 若使用imu,则 imu进行积分，并进行对齐
  if (this->imu_use_) {
    this->integrateIMU();
    this->gicp_s2s.align(*aligned, this->imu_SE3);
  } else {
    this->gicp_s2s.align(*aligned);
  }

  // Get the local S2S transform     通过 点到点匹配器 得到最后一帧的姿态
  Eigen::Matrix4f T_S2S = this->gicp_s2s.getFinalTransformation();

  // Get the global S2S transform //计算得到最后一帧与前一帧的相对关系
  this->propagateS2S(T_S2S);

  // reuse covariances from s2s for s2m
  // 为 s2m 重用 s2s 的协方差
  this->gicp.source_covs_ = this->gicp_s2s.source_covs_;

  // Swap source and target (which also swaps KdTrees internally) for next S2S
  // 为下一个 S2S 交换源和目标（也在内部交换 KdTrees）
  this->gicp_s2s.swapSourceAndTarget();

  //
  // FRAME-TO-SUBMAP  
  //

  // Get current global submap人 得到该帧submap
  this->getSubmapKeyframes();

  // 如果子图已经改变，则s2m匹配的target就改变了
  if (this->submap_hasChanged) {

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

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

  // Align with current submap with global S2S transformation as initial guess
  // 与当前子图对齐，将全局 S2S 转换作为初始猜测
  this->gicp.align(*aligned, this->T_s2s);

  // Get final transformation in global frame 在全局坐标系下得到最终的转换
  this->T = this->gicp.getFinalTransformation();

  // Update the S2S transform for next propagation 更新s2s的转换为下次迭代做准备
  this->T_s2s_prev = this->T;

  // Update next global pose
  // Both source and target clouds are in the global frame now, so tranformation is global
  //更新下一个全局姿势
  // 源云和目标云现在都在全局框架中，所以转换是全局的，不需要啥操作
  this->propagateS2M();

  // Set next target cloud as current source cloud  将下一个目标云设置为当前源云
  *this->target_cloud = *this->source_cloud;

}

得到 getSubmapKeyframes

步骤：

先清空用于生成子图的关键帧索引容器submap_kf_idx_curr.clear()
计算每个帧到当前帧的欧氏距离（平移量） $\Delta d = ||p_{state}-p_{i}||$
将每种符合前K个最近邻帧放入 submap队列
1. 查找来自距离关键帧的前k个最邻近帧
2. 查找来自凸包关键帧的前k个最邻近帧 computeConvexHull
3. 查找来自凹包关键帧的前k个最邻近帧 computeConcaveHull
将submap关键帧index 进行排序，然后删除重复的id
1. sort 排序+unique删除重复
当前submap_index 与旧submap_index 是否一致
1. 一致，则直接结束
2. 否则，重新生成 submap点云地图+法向量，并更新旧submap_index

void dlo::OdomNode::getSubmapKeyframes() {

  // clear vector of keyframe indices to use for submap
  // 清空用于子图的关键帧索引容器
  this->submap_kf_idx_curr.clear();

  //
  // TOP K NEAREST NEIGHBORS FROM ALL KEYFRAMES
  // 来自所有关键帧的 前K个最近邻帧

  // calculate distance between current pose and poses in keyframe set
  std::vector<float> ds;
  std::vector<int> keyframe_nn; int i=0;
  Eigen::Vector3f curr_pose = this->T_s2s.block(0,3,3,1);

  for (const auto& k : this->keyframes) {
    // 计算位姿（平移量）的欧氏距离
    float d = sqrt( pow(curr_pose[0] - k.first.first[0], 2) + pow(curr_pose[1] - k.first.first[1], 2) + pow(curr_pose[2] - k.first.first[2], 2) );
    ds.push_back(d);
    keyframe_nn.push_back(i); i++;
  }

  // get indices for top K nearest neighbor keyframe poses
  // 得到 k个最近 关键帧位姿的下标
  this->pushSubmapIndices(ds, this->submap_knn_, keyframe_nn);

  //
  // TOP K NEAREST NEIGHBORS FROM CONVEX HULL
  //

  // get convex hull indices  得到关键帧位姿凸包的index
  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 indicies for top kNN for convex hull 
  // 筛选出凸包 k个邻近关键帧的下标
  this->pushSubmapIndices(convex_ds, this->submap_kcv_, this->keyframe_convex);

  //
  // TOP K NEAREST NEIGHBORS FROM CONCAVE HULL
  //

  // get concave hull indices 获取凹包的indexs
  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 indicies for top kNN for convex hull
  this->pushSubmapIndices(concave_ds, this->submap_kcc_, this->keyframe_concave);

  //
  // BUILD SUBMAP
  //

  // concatenate all submap clouds and normals
  // 将submap关键帧 进行排序，然后删除重复的id
  std::sort(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end());
  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());

  // 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());

  // check if submap has changed from previous iteration 检测submap是否改变
  if (this->submap_kf_idx_curr == this->submap_kf_idx_prev){
    this->submap_hasChanged = false;
  } else {
    this->submap_hasChanged = true;

    // reinitialize submap cloud, normals
    // 重新初始化 子图 点云+法线，并赋值上一次点云
    pcl::PointCloud<PointType>::Ptr submap_cloud_ (boost::make_shared<pcl::PointCloud<PointType>>());
    this->submap_normals.clear();

    for (auto k : this->submap_kf_idx_curr) {

      // create current submap cloud 生成submap
      *submap_cloud_ += *this->keyframes[k].second;

      // grab corresponding submap cloud's normals
      this->submap_normals.insert( std::end(this->submap_normals), std::begin(this->keyframe_normals[k]), std::end(this->keyframe_normals[k]) );
    }
	
    this->submap_cloud = submap_cloud_;
    this->submap_kf_idx_prev = this->submap_kf_idx_curr;
  }

}

取k个最近帧下标 pushSubmapIndices

思路：

先按照优先队列的思路，找到第k小的dist元素
然后遍历 frame_dist，将小于等于该值的 frame放入 submap_kf_idx_curr

void dlo::OdomNode::pushSubmapIndices(std::vector<float> dists, int k, std::vector<int> frames) {

  // maintain max heap of at most k elements
  // 定义一个 优先队列
  std::priority_queue<float> pq;

  /// 优先队列，默认大顶堆，top最大，这样只保留了 较小的 K个数
  for (auto d : dists) {
    if (pq.size() >= k && pq.top() > d) {
      pq.push(d);
      pq.pop();
    } else if (pq.size() < k) {
      pq.push(d);
    }
  }

  // get the kth smallest element, which should be at the top of the heap
  // / 获取第k小的元素，它应该在堆顶
  float kth_element = pq.top();

  // get all elements smaller or equal to the kth smallest element
  // 获取小于或等于第 k 个最小元素的所有元素
  for (int i = 0; i < dists.size(); ++i) {
    if (dists[i] <= kth_element)
      this->submap_kf_idx_curr.push_back(frames[i]);
  }

}

关键帧凸包 computeConvexHull

思路：

将位姿作为点云，基于凸包进行索引，凹包类似

步骤：

将关键帧位姿 push 作为点云
计算点云的凸包，
获取凸包上关键帧的索引
赋值关键帧凸包索引

void dlo::OdomNode::computeConvexHull() {

  // at least 4 keyframes for convex hull
  if (this->num_keyframes < 4) {
    return;
  }

  // create a pointcloud with points at keyframes
  pcl::PointCloud<PointType>::Ptr cloud = pcl::PointCloud<PointType>::Ptr (new pcl::PointCloud<PointType>);

  for (const auto& k : this->keyframes) {
    PointType pt;
    pt.x = k.first.first[0];
    pt.y = k.first.first[1];
    pt.z = k.first.first[2];
    cloud->push_back(pt);
  }

  // calculate the convex hull of the point cloud 计算点云的凸包
  this->convex_hull.setInputCloud(cloud);

  // get the indices of the keyframes on the convex hull  获取凸包上关键帧的索引
  pcl::PointCloud<PointType>::Ptr convex_points = pcl::PointCloud<PointType>::Ptr (new        pcl::PointCloud<PointType>);
  this->convex_hull.reconstruct(*convex_points);

  pcl::PointIndices::Ptr convex_hull_point_idx = pcl::PointIndices::Ptr (new pcl::PointIndices);
  this->convex_hull.getHullPointIndices(*convex_hull_point_idx);

  this->keyframe_convex.clear();
  for (int i=0; i<convex_hull_point_idx->indices.size(); ++i) {
    this->keyframe_convex.push_back(convex_hull_point_idx->indices[i]);
  }

}

MapNode

主函数

该主函数主要构建了 MapNode，然后等待直到 ros关闭为止

#include "dlo/map.h"

void controlC(int sig) {

  dlo::MapNode::abort();

}

int main(int argc, char** argv) {

  ros::init(argc, argv, "dlo_map_node");
  ros::NodeHandle nh("~");

  signal(SIGTERM, controlC);
  sleep(0.5);

  dlo::MapNode node(nh);
  ros::AsyncSpinner spinner(1);
  spinner.start();
  node.start();
  ros::waitForShutdown();

  return 0;

}

MapNode 核心成员

stop 外界信号触发

class dlo::MapNode {

public:

  MapNode(ros::NodeHandle node_handle);
  ~MapNode();

  static void abort() {
    abort_ = true;
  }

  void start();
  void stop();	

private:

  void abortTimerCB(const ros::TimerEvent& e);
  void publishTimerCB(const ros::TimerEvent& e);

  void keyframeCB(const sensor_msgs::PointCloud2ConstPtr& keyframe);

  bool savePcd(direct_lidar_odometry::save_pcd::Request& req,
               direct_lidar_odometry::save_pcd::Response& res);

  void getParams();

  ros::NodeHandle nh;
  ros::Timer abort_timer;
  ros::Timer publish_timer;

  ros::Subscriber keyframe_sub;
  ros::Publisher map_pub;

  ros::ServiceServer save_pcd_srv;

  pcl::PointCloud<PointType>::Ptr dlo_map;
  pcl::VoxelGrid<PointType> voxelgrid;

  ros::Time map_stamp;
  std::string odom_frame;

  bool publish_full_map_;
  double publish_freq_;
  double leaf_size_;

  static std::atomic<bool> abort_;

};

构造函数 MapNode

步骤：

获取参数
- odom_frame="odom"; publish_full_map_=true; publish_freq_=1.0; leaf_size_=0.5;
- 获取节点 NS 并删除前导字符 ns
- 连接帧名称字符串 odom_frame = ns + "/" + this->odom_frame;
创建定时器 abort_timer，100hz 检测程序是否停止，停止则使ros停止
如果发布整个地图时，则创建发布地图定时器，按一定周期发布点云地图PointCloud2
订阅 topic 关键帧：“keyframes”, 1, &dlo::MapNode::keyframeCB,

dlo::MapNode::MapNode(ros::NodeHandle node_handle) : nh(node_handle) {

  // 得到参数
  this->getParams();

  this->abort_timer = this->nh.createTimer(ros::Duration(0.01), &dlo::MapNode::abortTimerCB, this);

  if (this->publish_full_map_){
    this->publish_timer = this->nh.createTimer(ros::Duration(this->publish_freq_), &dlo::MapNode::publishTimerCB, this);
  }
  
  // 订阅关键帧，发布点云地图
  this->keyframe_sub = this->nh.subscribe("keyframes", 1, &dlo::MapNode::keyframeCB, this);
  this->map_pub = this->nh.advertise<sensor_msgs::PointCloud2>("map", 1);

  // 保存地图到 pcd 服务
  this->save_pcd_srv = this->nh.advertiseService("save_pcd", &dlo::MapNode::savePcd, this);

  // initialize map 初始化地图
  this->dlo_map = pcl::PointCloud<PointType>::Ptr (new pcl::PointCloud<PointType>);

  ROS_INFO("DLO Map Node Initialized");
}

关键帧回调 keyframeCB

步骤：

将关键帧数据格式由 ros->pcl pcl::fromROSMsg
关键帧点云数据进行体素滤波降采样 voxelgrid.filter
关键帧加载到全局地图中 pcl格式，直接+号
发布地图

void dlo::MapNode::keyframeCB(const sensor_msgs::PointCloud2ConstPtr& keyframe) {

  // convert scan to pcl format    : ros type to pcl
  pcl::PointCloud<PointType>::Ptr keyframe_pcl = pcl::PointCloud<PointType>::Ptr (new 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 keyframe to map  地图中保存该关键帧
  this->map_stamp = keyframe->header.stamp;
  *this->dlo_map += *keyframe_pcl;

  if (!this->publish_full_map_) {
    // 发布
    if (keyframe_pcl->points.size() == keyframe_pcl->width * keyframe_pcl->height) {
      sensor_msgs::PointCloud2 map_ros;
      pcl::toROSMsg(*keyframe_pcl, map_ros);
      map_ros.header.stamp = ros::Time::now();
      map_ros.header.frame_id = this->odom_frame;
      this->map_pub.publish(map_ros);
    }
  }

}

保存地图 savePcd

地图进行提速滤波后保存

步骤：

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

bool dlo::MapNode::savePcd(direct_lidar_odometry::save_pcd::Request& req,
                           direct_lidar_odometry::save_pcd::Response& res) {

  // 地图格式转换
  pcl::PointCloud<PointType>::Ptr m =
    pcl::PointCloud<PointType>::Ptr (boost::make_shared<pcl::PointCloud<PointType>>(*this->dlo_map));

  float leaf_size = req.leaf_size;
  std::string p = req.save_path;

  std::cout << std::setprecision(2) << "Saving map to " << p + "/dlo_map.pcd" << "... "; std::cout.flush();

  // voxelize map  地图进行体素滤波
  pcl::VoxelGrid<PointType> vg;
  vg.setLeafSize(leaf_size, leaf_size, leaf_size);
  vg.setInputCloud(m);
  vg.filter(*m);

  // save map 保存地图
  int ret = pcl::io::savePCDFileBinary(p + "/dlo_map.pcd", *m);
  res.success = ret == 0;

  if (res.success) {
    std::cout << "done" << std::endl;
  } else {
    std::cout << "failed" << std::endl;
  }

  return res.success;

}