Bootstrap

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

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;
  }

悦读

道可道，非常道；名可名，非常名。 无名，天地之始，有名，万物之母。 故常无欲，以观其妙，常有欲，以观其徼。 此两者，同出而异名，同谓之玄，玄之又玄，众妙之门。

最新收录
记录一次系统磁盘扩容
iOS 8 WebKit框架概览
MySql错误：#1054 - Unknown column 'Cs001' in 'field list' 解决办法
int 3中断与软件调试
linux 命令cp -a的用法
PotPlayer视频镜像如何恢复
python+requests接口自动化测试实战
机器学习（五） -- 监督学习（6） --逻辑回归
PreparedStatement的参数问题（已解决）
通信-手机-专业术语
;