hdl_localization代码解析

简介

hdl_localization是基于UKF滤波框架，融合了ndt点云配准结果，在已经构建的点云地图上实习激光重定位的一种方法。在使用16线激光雷达进行机器人定位时，不用IMU也可以取得不错的效果。

安装依赖

依赖库

1.Ros-Melodic
2.Pcl-1.8
3.Open-MP
4.Eigen3.3(及以上)

依赖包

1.ndt_omp
2.fast-gicp
3.hdl_global_localization

运行

rosparam set use_sim_time true
roslaunch hdl_localization hdl_localization.launch

roscd hdl_localization/rviz
rviz -d hdl_localization.rviz

注意

1.如果进行纯定位时的初始位姿在地图坐标系附近，在launch文件中可以将 “specify_init_pose" 设为 ”true“，这样，其默认的三维位置（0，0，0）和默认的表示旋转的四元数（0，0，0，1）就可以很好的给予点云一个初始状态，有利于其后续匹配和重定位。
2.如果想在地图中任意位置进行重定位，需要在开启rviz -d hdl_localization.rviz后，选择rviz上方的2D pose estimator，并在地图中左键点击和鼠标拖动，选择一个与真实位置相近的位置与航向。

效果

UKF知识补充（无迹卡尔曼滤波）

网页链接
原始高斯分布经过非线性变换之后其实是一个不规则的非线性分布，在EKF中我们在高斯均值点附近用泰勒级数近似非线性分布，从而获得近似高斯分布。但是问题在于，这个近似高斯分布有时不是足够精确，单单一个均值点往往无法预测上图中这种很不规则的分布的。这个时候我们就需要无迹卡尔曼滤波UKF了，通过无迹转换将非线性函数变换的结果近似成高斯分布。
以下是无迹变换执行的步骤：

1.计算Sigma点集

2.为每个Sigma点分配权重

3.把所有单个Sigma点代入非线性函数f

4.对经过上述加权和转换的点近似新的高斯分布

5.计算新高斯分布的均值和方差。

代码阅读

总览

该项目是使用nodelet统一管理的，apps为定义的两个类，也就是程序实现。include内为状态估计器和无迹卡尔曼的实现。launch是启动文件。rviz内为rviz的配置文件。data为实例的定位用点云地图。

launch

定义了几个参数，使用nodelet运行了velodyne_nodelet_manager、globalmap_server_nodelet、hdl_localization_nodelet三个节点。如果只用于仿真，可以在 arguments 前面加上。

 <param name="use_sim_time" value="true"/>

apps(程序实现)

本文件夹是只有两个cpp文件，直接继承了nodelet的类。

globalmap_server_nodelet

类GlobalmapServerNodelet 继承了 nodelet::Nodelet。
关于ros，声明了三个句柄，1个发布，1个计时器，1个globalmap的变量。

ros::NodeHandle nh;
ros::NodeHandle mt_nh;
ros::NodeHandle private_nh;

ros::Publisher globalmap_pub;

ros::WallTimer globalmap_pub_timer;
pcl::PointCloud<PointT>::Ptr globalmap;

globalmap_server_nodelet::onInit()

这里是在重写了初始化函数。同时利用计时器出发回调函数。

  void onInit() override {
   
  //定义三个节点，
    nh = getNodeHandle();
    mt_nh = getMTNodeHandle();
    private_nh = getPrivateNodeHandle();

    initialize_params();

    // publish globalmap with "latched" publisher
    globalmap_pub = nh.advertise<sensor_msgs::PointCloud2>("/globalmap", 5, true);
    globalmap_pub_timer = nh.createWallTimer(ros::WallDuration(0.05), &GlobalmapServerNodelet::pub_once_cb, this, true, true); //20Hz
  }

globalmap_server_nodelet::initialize_params()

在程序initialize_params()中，完成了读取地图pcd文件的功能，并对该地图进行下采样，最终的globalmap是下采样的地图。

void initialize_params() {
   
    // read globalmap from a pcd file
    std::string globalmap_pcd = private_nh.param<std::string>("globalmap_pcd", "");
    globalmap.reset(new pcl::PointCloud<PointT>());
    pcl::io::loadPCDFile(globalmap_pcd, *globalmap);
    globalmap->header.frame_id = "map";

	//TODO：这个实际上是没有到这里来的，初步想法是没有utm文件。类似于经纬度的坐标文件。
    std::ifstream utm_file(globalmap_pcd + ".utm");
    if (utm_file.is_open() && private_nh.param<bool>("convert_utm_to_local", true)) {
   
        std::cout << "now utf_file is open" << std::endl;
      double utm_easting;
      double utm_northing;
      double altitude;
      utm_file >> utm_easting >> utm_northing >> altitude;
      for(auto& pt : globalmap->points) {
   
        pt.getVector3fMap() -= Eigen::Vector3f(utm_easting, utm_northing, altitude);
      }
      ROS_INFO_STREAM("Global map offset by UTM reference coordinates (x = " 
                      << utm_easting << ", y = " << utm_northing << ") and altitude (z = " << altitude << ")");
    }
    //endTODO

    // downsample globalmap
    double downsample_resolution = private_nh.param<double>("downsample_resolution", 0.1);
    boost::shared_ptr<pcl::VoxelGrid<PointT>> voxelgrid(new pcl::VoxelGrid<PointT>());
    voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
    voxelgrid->setInputCloud(globalmap);

    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    voxelgrid->filter(*filtered);

    globalmap = filtered;
  }

同时，每隔0.05s发布一次。（onInit定义的）

  void pub_once_cb(const ros::WallTimerEvent& event) {
   
    globalmap_pub.publish(globalmap);
  }

hdl_localization_nodelet

类 HdlLocalizationNodelet 继承了 nodelet::Nodelet，先看初始化。

hdl_localization_nodelet::onInit()

  void onInit() override {
   
    //依然是三个句柄
    nh = getNodeHandle();
    mt_nh = getMTNodeHandle();
    private_nh = getPrivateNodeHandle();
	//这里的时间用了boost库里的 circular_buffer<double>。感兴趣的可以自己百度一下，毕竟……我也没用过。
    processing_time.resize(16);
    //这些参数，又来了。
    initialize_params();
    
    //这个默认的base_link, launch里覆盖了。实际上是velodyne。参数类的在launch里改写了一部分，这里就不一一赘述了。可以自己对比来看，比较容易。
    odom_child_frame_id = private_nh.param<std::string>("odom_child_frame_id", "base_link");
    //是否使用imu
    use_imu = private_nh.param<bool>("use_imu", true);
    //imu是否倒置
    invert_imu = private_nh.param<bool>("invert_imu", false); 
    if(use_imu) {
   //如果使用imu，则定义订阅函数。
      NODELET_INFO("enable imu-based prediction");
      imu_sub = mt_nh.subscribe("/gpsimu_driver/imu_data", 256, &HdlLocalizationNodelet::imu_callback, this);
    }
    //点云数据、全局地图、初始位姿的订阅。initialpose_sub只是用于rviz划点用的。
    points_sub = mt_nh.subscribe("/velodyne_points", 5, &HdlLocalizationNodelet::points_callback, this);
    globalmap_sub = nh.subscribe("/globalmap", 1, &HdlLocalizationNodelet::globalmap_callback, this);
    initialpose_sub = nh.subscribe("/initialpose", 8, &HdlLocalizationNodelet::initialpose_callback, this);
	//发布里程计信息，以及对齐后的点云数据。
    pose_pub = nh.advertise<nav_msgs::Odometry>("/odom", 5, false);
    aligned_pub = nh.advertise<sensor_msgs::PointCloud2>("/aligned_points", 5, false);
  }

hdl_localization_nodelet::initialize_params()

初始化参数

  void initialize_params() {
   
    // intialize scan matching method
    double downsample_resolution = private_nh.param<double>("downsample_resolution", 0.1);
    std::string ndt_neighbor_search_method = private_nh.param<std::string>("ndt_neighbor_search_method", "DIRECT7");

    double ndt_resolution = private_nh.param<double>("ndt_resolution", 1.0);
    boost::shared_ptr<pcl::VoxelGrid<PointT>> voxelgrid(new pcl::VoxelGrid<PointT>());
    voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
    downsample_filter = voxelgrid;
	//定义了ndt和glcp
    pclomp::NormalDistributionsTransform<PointT, PointT>::Ptr ndt(new pclomp::NormalDistributionsTransform<PointT, PointT>());
    pclomp::GeneralizedIterativeClosestPoint<PointT, PointT>::Ptr gicp(new pclomp::GeneralizedIterativeClosestPoint<PointT, PointT>());
	//ndt参数与搜索方法。默认DIRECT7，作者说效果不好可以尝试改为DIRECT1.
    ndt->setTransformationEpsilon(0.01);
    ndt->setResolution(ndt_resolution);
    if(ndt_neighbor_search_method == "DIRECT1") {
   
      NODELET_INFO("search_method DIRECT1 is selected");
      ndt->setNeighborhoodSearchMethod(pclomp::DIRECT1);
      registration = ndt;
    } else if(ndt_neighbor_search_method == "DIRECT7") {
   
      NODELET_INFO("search_method DIRECT7 is selected");
      ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7);
      registration = ndt;
    } else if(ndt_neighbor_search_method == "GICP_OMP"){
   
      NODELET_INFO("search_method GICP_OMP is selected");
      registration = gicp;
    }
     else {
   
      if(ndt_neighbor_search_method == "KDTREE") {
   
        NODELET_INFO("search_method KDTREE is selected");
      } else {
   
        NODELET_WARN("invalid search method was given");
        NODELET_WARN("default method is selected (KDTREE)");
      }
      ndt->setNeighborhoodSearchMethod(pclomp::KDTREE);
      registration = ndt;
    }
    
    // initialize pose estimator
    //设置起点用。
    if(private_nh.param<bool>("specify_init_pose", true)) {
   
      NODELET_INFO("initialize pose estimator with specified parameters!!");
      pose_estimator.reset(new hdl_localization::PoseEstimator(registration,
        ros::Time::now(),
        Eigen::Vector3f(private_nh.param<double>("init_pos_x", 0.0), private_nh.param<double>("init_pos_y", 0.0), private_nh.param<double>("init_pos_z", 0.0)),
        Eigen::Quaternionf(private_nh.param<double>(