hdl_localization试读

安装

简单易行，环境友好。首先附上网址：koide3/hdl_localization.

前置环境我直接用的apt-get
//安装pcl

sudo apt-get install libpcl-dev

//安装其他依赖（记得替换命令中的kinetic为自己的版本，一共4个地方。忘记那个自动的怎么写了。。。）

sudo apt-get install ros-kinetic-geodesy ros-kinetic-pcl-ros ros-kinetic-nmea-msgs ros-kinetic-libg2o

安装好环境之后，按照readme直接走就可以。
创建工作空间→git clone hdl_localization→git clone ndt_omp→编译¹

实验效果

总的来说效果很好，从官方给的数据集和实验室之前的包都跑过，定位效果颇好。不用imu也可。

等会跑一个

hdl_localization包

总览

该软件是使用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>("init_ori_w", 1.0), private_nh.param<double>("init_ori_x", 0.0), private_nh.param<double>("init_ori_y", 0.0), private_nh.param