欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识！
您现在的位置是： 首页

hdl_graph_slam（1）点云预处理源码详解

程序员文章站 2022-06-02 21:48:37
...

HDL_graph_slam 点云预处理 prefiltering_nodelete

收到点云原始数据后第一步是对点云进行预处理，降低样本数量加快计算速度。hdl这里用到的主要有体素降采样，离群点移除（可选择使用半径或者统计学离群点移除法），还有一个简单的最大距离滤波。算法基本构建于pcl库，本篇分析一下预处理节点源码。

  • 整体结构，变量，函数
#include <string>

#include <ros/ros.h>
#include <ros/time.h>
#include <pcl_ros/transforms.h>
#include <pcl_ros/point_cloud.h>
#include <tf/transform_listener.h>

#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>

#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/statistical_outlier_removal.h>

namespace hdl_graph_slam {
    class PrefilteringNodelet: public:: Nodelet{		//预处理类继承nodelet类
        public:		//公有成员
       		typedef pcl::PointXYZI PointT;		//数据类型定义
            PrefilteringNodelet() {}			//构造函数
            virtual ~PrefilteringNodelet() {}	//虚析构函数
            virtual void onInit() {}			//虚初始化函数
        private: 	//私有成员
        	ros::NodeHandle nh;					//ROS公共句柄
            ros::NodeHandle private_nh;			//ROS私有句柄

            ros::Subscriber imu_sub;			//imu消息订阅器
            std::vector<sensor_msgs::ImuConstPtr> imu_queue; //IMU队列向量

            ros::Subscriber points_sub;			//点云消息订阅器
            ros::Publisher points_pub;			//点云消息发布器

            ros::Publisher colored_pub;			//色彩消息发布器

            tf::TransformListener tf_listener;  //tf监听器

            std::string base_link_frame;		//baselink坐标系名称

            bool use_distance_filter;			//是否使用距离滤波
            double distance_near_thresh;		//距离滤波近点阈值
            double distance_far_thresh;			//距离滤波远点阈值

            pcl::Filter<PointT>::Ptr downsample_filter;		//降采样滤波器
            pcl::Filter<PointT>::Ptr outlier_removal_filter;		//离群点去除滤波器
        
        	void initialize_params(){}			//参数初始化函数
    		void imu_callback(pcl::PointCloud<PointT>::ConstPtr src_cloud)		//imu消息回调函数
            void cloud_callback(pcl::PointCloud<PointT>::ConstPtr src_cloud){}		//点云消息回调函数
            pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud) const{}		//降采样函数
        	pcl::PointCloud<PointT>::ConstPtr outlier_removal(const pcl::PointCloud<PointT>::ConstPtr& cloud) const{}		 //离群点去除函数
        	pcl::PointCloud<PointT>::ConstPtr distance_filter(const pcl::PointCloud<PointT>::ConstPtr& cloud) const{}		 //距离滤波器函数
        	pcl::PointCloud<PointT>::ConstPtr deskewing(const pcl::PointCloud<PointT>::ConstPtr& cloud) {} 			   //点云去畸变函数
    }
}
  • 参数初始化 initialize_params 
  void initialize_params() {
      std::string downsample_method = private_nh.param<std::string>("downsample_method", "VOXELGRID");		//ros借口读取降采样方法参数
      double downsample_resolution = private_nh.param<double>("downsample_resolution", 0.1);		//ros借口读取降采样方法分辨率（leafsize）
  
      if(downsample_method == "VOXELGRID") {		//体素降采样法具体设置
        std::cout << "downsample: VOXELGRID " << downsample_resolution << std::endl;
        // create a shared pointer filter using pcl voxel grid
        boost::shared_ptr<pcl::VoxelGrid<PointT>> voxelgrid(new pcl::VoxelGrid<PointT>());		//新建体素滤波器对象
        voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);			//设置体素滤波器分辨率
        downsample_filter = voxelgrid;		//命名滤波器
      } else if(downsample_method == "APPROX_VOXELGRID") {		//设置为近似体素滤波器
        std::cout << "downsample: APPROX_VOXELGRID " << downsample_resolution << std::endl;
        boost::shared_ptr<pcl::ApproximateVoxelGrid<PointT>> approx_voxelgrid(new pcl::ApproximateVoxelGrid<PointT>());
        approx_voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
        downsample_filter = approx_voxelgrid;
      } else {
        if(downsample_method != "NONE") {
          std::cerr << "warning: unknown downsampling type (" << downsample_method << ")" << std::endl;
          std::cerr << "       : use passthrough filter" << std::endl;
        }
        std::cout << "downsample: NONE" << std::endl;
      }
  
      std::string outlier_removal_method = private_nh.param<std::string>("outlier_removal_method", "STATISTICAL");		//离群点滤波器方法参数读取
      if(outlier_removal_method == "STATISTICAL") {
        int mean_k = private_nh.param<int>("statistical_mean_k", 20);		//读取统计滤波k均值
        double stddev_mul_thresh = private_nh.param<double>("statistical_stddev", 1.0);		//读取标准差
        std::cout << "outlier_removal: STATISTICAL " << mean_k << " - " << stddev_mul_thresh << std::endl;
  
        pcl::StatisticalOutlierRemoval<PointT>::Ptr sor(new pcl::StatisticalOutlierRemoval<PointT>());		//新建滤波器对象
        sor->setMeanK(mean_k);		//设置均值
        sor->setStddevMulThresh(stddev_mul_thresh);		//设置标准差
        outlier_removal_filter = sor;		//命名滤波器
      } else if(outlier_removal_method == "RADIUS") {			//设置半径离群点滤波器
        double radius = private_nh.param<double>("radius_radius", 0.8);
        int min_neighbors = private_nh.param<int>("radius_min_neighbors", 2);
        std::cout << "outlier_removal: RADIUS " << radius << " - " << min_neighbors << std::endl;
  
        pcl::RadiusOutlierRemoval<PointT>::Ptr rad(new pcl::RadiusOutlierRemoval<PointT>());
        rad->setRadiusSearch(radius);
        rad->setMinNeighborsInRadius(min_neighbors);
        outlier_removal_filter = rad;
      } else {
        std::cout << "outlier_removal: NONE" << std::endl;
      }
  
      use_distance_filter = private_nh.param<bool>("use_distance_filter", true);
      distance_near_thresh = private_nh.param<double>("distance_near_thresh", 1.0);
      distance_far_thresh = private_nh.param<double>("distance_far_thresh", 100.0);
  
      base_link_frame = private_nh.param<std::string>("base_link_frame", "");
    }

  • 点云回调函数 cloud_callback

    void cloud_callback(pcl::PointCloud<PointT>::ConstPtr src_cloud) {		//输入原始点云指针
    if(src_cloud->empty()) {		//判断点云不为空
      return;
    }

    src_cloud = deskewing(src_cloud);		//点云去畸变

    // if base_link_frame is defined, transform the input cloud to the frame
    if(!base_link_frame.empty()) {			//判断并转换点云到baselink坐标系
      if(!tf_listener.canTransform(base_link_frame, src_cloud->header.frame_id, ros::Time(0))) {
        std::cerr << "failed to find transform between " << base_link_frame << " and " << src_cloud->header.frame_id << std::endl;
      }

      tf::StampedTransform transform;
      tf_listener.waitForTransform(base_link_frame, src_cloud->header.frame_id, ros::Time(0), ros::Duration(2.0));
      tf_listener.lookupTransform(base_link_frame, src_cloud->header.frame_id, ros::Time(0), transform);

      pcl::PointCloud<PointT>::Ptr transformed(new pcl::PointCloud<PointT>());
      pcl_ros::transformPointCloud(*src_cloud, *transformed, transform);
      transformed->header.frame_id = base_link_frame;
      transformed->header.stamp = src_cloud->header.stamp;
      src_cloud = transformed;
    }

    pcl::PointCloud<PointT>::ConstPtr filtered = distance_filter(src_cloud);		//应用距离滤波函数
    filtered = downsample(filtered);		//应用降采样函数降采样
    filtered = outlier_removal(filtered);		//应用离群点函数去除离群点
    points_pub.publish(filtered); 			//发布预处理过后的点云
  }

  • 降采样函数 downsample

    pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {
    if(!downsample_filter) {		//确定降采样滤波器已被设置成功
      return cloud;
    }

    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    downsample_filter->setInputCloud(cloud);		//输入点云
    downsample_filter->filter(*filtered);			//点云滤波
    filtered->header = cloud->header;				//header复制

    return filtered;								//返回过滤后的点云
  }

  • 离群点去除函数 outlier_removal

    pcl::PointCloud<PointT>::ConstPtr outlier_removal(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {
    if(!outlier_removal_filter) {		确定离群点滤波器已被设置成功
      return cloud;
    }

    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    outlier_removal_filter->setInputCloud(cloud);		//输入点云
    outlier_removal_filter->filter(*filtered);			//滤波
    filtered->header = cloud->header;

    return filtered;									//返回过滤后的点云
  }

  • 距离滤波函数 distance_filter

    pcl::PointCloud<PointT>::ConstPtr distance_filter(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {
    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    filtered->reserve(cloud->size());

    std::copy_if(cloud->begin(), cloud->end(), std::back_inserter(filtered->points), [&](const PointT& p) {
      double d = p.getVector3fMap().norm();
      return d > distance_near_thresh && d < distance_far_thresh;
    });		//筛选在距离阈值范围内点

    filtered->width = filtered->size();
    filtered->height = 1;
    filtered->is_dense = false;

    filtered->header = cloud->header;

    return filtered;
  }

  • 去畸变函数 deskewing

    pcl::PointCloud<PointT>::ConstPtr deskewing(const pcl::PointCloud<PointT>::ConstPtr& cloud) {
    ros::Time stamp = pcl_conversions::fromPCL(cloud->header.stamp);		//获得点云时间戳数据
    if(imu_queue.empty()) {
      return cloud;		//查看队列是否有数据
    }

    // the color encodes the point number in the point sequence
    if(colored_pub.getNumSubscribers()) {		//发布带颜色的点云数据
      pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored(new pcl::PointCloud<pcl::PointXYZRGB>());
      colored->header = cloud->header;
      colored->is_dense = cloud->is_dense;
      colored->width = cloud->width;
      colored->height = cloud->height;
      colored->resize(cloud->size());

      for(int i = 0; i < cloud->size(); i++) {
        double t = static_cast<double>(i) / cloud->size();
        colored->at(i).getVector4fMap() = cloud->at(i).getVector4fMap();
        colored->at(i).r = 255 * t;
        colored->at(i).g = 128;
        colored->at(i).b = 255 * (1 - t);
      }
      colored_pub.publish(colored);
    }

    sensor_msgs::ImuConstPtr imu_msg = imu_queue.front();

    auto loc = imu_queue.begin();
    for(; loc != imu_queue.end(); loc++) {
      imu_msg = (*loc);
      if((*loc)->header.stamp > stamp) {
        break;
      }
    }

    imu_queue.erase(imu_queue.begin(), loc);

    Eigen::Vector3f ang_v(imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z);		//获取imu角速度vector
    ang_v *= -1;

    pcl::PointCloud<PointT>::Ptr deskewed(new pcl::PointCloud<PointT>()); //初始化定义的去畸变点云
    deskewed->header = cloud->header;
    deskewed->is_dense = cloud->is_dense;
    deskewed->width = cloud->width;
    deskewed->height = cloud->height;
    deskewed->resize(cloud->size());

    double scan_period = private_nh.param<double>("scan_period", 0.1);
    for(int i = 0; i < cloud->size(); i++) {
      const auto& pt = cloud->at(i);

      // TODO: transform IMU data into the LIDAR frame
      double delta_t = scan_period * static_cast<double>(i) / cloud->size();
      Eigen::Quaternionf delta_q(1, delta_t / 2.0 * ang_v[0], delta_t / 2.0 * ang_v[1], delta_t / 2.0 * ang_v[2]);		//通过角速度和扫描时间差计算畸变四元数
      Eigen::Vector3f pt_ = delta_q.inverse() * pt.getVector3fMap();		//计算校正后的点云

      deskewed->at(i) = cloud->at(i);		//获得校正后的点云
      deskewed->at(i).getVector3fMap() = pt_;		//
    }

    return deskewed;
  }
相关标签： SLAM c++ slam

上一篇： 关于SQL优化的小知识

下一篇： 广东所有大学录取分数线2021参考：2020年各高校在广东录取分数线一览表

推荐阅读