基于ROS系统的3D点云单目标行人跟踪

终于正式进入研究生生活了,因为实验室项目需求,使用3D激光雷达数据实现单目标行人跟踪。
首先需要在Linux下用C++实现该功能,对于点云数据,需要使用PCL库进行处理。
雷达采集的数据为.bag文件,为连续帧数据,但不能被直接读取。因此通过ROS系统将.bag文件转为.pcd文件,相当于获取逐帧数据,在这基础上进行处理。
对于输入点云数据,首先要过滤较远范围的点云数据,减小接下来的搜索范围。

        pcl::PassThrough<pcl::PointXYZ> pass;

        pass.setInputCloud (cloud);            //设置输入点云
        pass.setFilterFieldName ("x");         //设置过滤时所需要点云类型的Z字段
        pass.setFilterLimits (-7.0, 7.0);        //设置在过滤字段的范围
        pass.filter (*cloud);
        
        pass.setInputCloud (cloud);
        pass.setFilterFieldName ("y");         //设置过滤时所需要点云类型的Z字段
        pass.setFilterLimits (-7.0, 7.0);        //设置在过滤字段的范围
        pass.filter (*cloud);
        
        pass.setInputCloud (cloud);
        pass.setFilterFieldName ("z");         //设置过滤时所需要点云类型的Z字段
        pass.setFilterLimits (-1.5, 0.5);        //设置在过滤字段的范围
        pass.filter (*cloud);

在选定的范围中,需要通过聚类算法,按不同点云所在位置归为不同整体。通常聚类方法包括K-Means算法和欧式聚类方法。由于K-Means算法需要事先知道聚几类,才能完成聚类工作,而跟踪复杂环境无法提前设定聚类数目,因此最后使用欧式聚类方法,根据实际效果,并进行部分参数设定。

        pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
        ec.setClusterTolerance (0.55); //设置近邻搜索的搜索半径为55cm
        ec.setMinClusterSize (30);//设置一个聚类需要的最少点数目为30
        ec.setMaxClusterSize (1000); //设置一个聚类需要的最大点数目为1000
        ec.setSearchMethod (tree);//设置点云的搜索机制
        ec.setInputCloud (cloud);
        ec.extract (cluster_indices);//从点云中提取聚类,并将点云索引保存在cluster_indices中   

在我们提取的所有聚类中计算聚类的xyz轴上长度,根据相应特征便可以检测到行人。
行人检测结果

接下来考虑跟踪问题,因为任务是对单目标进行跟踪,因此不需要加跟踪器,只需使用检测结果。同时对于没有检测出行人的帧和前后帧行人检测位置变化较大的帧都认为检测不佳,对于这些帧使用的行人位置为:

            centroid.x = person_x_later + v_x;
            centroid.y = person_y_later + v_y;
            centroid.z = person_z_later;

即这些帧行人的位置由上一帧行人的位置和速度计算而得。最后的效果为
行人跟踪结果

因为最后该项目用在机器人上,需要使用ROS系统。ROS(Robot Operating System)是一个机器人软件平台,我自身理解使用ROS系统的一大重要原因是对于各个传感器(摄像头、GPS、激光雷达等)的接口和库函数集成很完备。
对于已经实现的C++语言转移到ROS系统上,其最核心的部分为ROS的发布信息和订阅功能。而该程序需要实现在订阅获取发布点云数据的基础上,处理点云数据,最后进行结果发布,因此该程序需要同时拥有ROS的订阅和发布功能。其基本构架为:

class SubscribeAndPublish
{
public:
  SubscribeAndPublish()
  {
    //Topic you want to publish
    pub_ = n_.advertise<sensor_msgs::PointCloud2>("pcl_show", 1);
    //Topic you want to subscribe
    sub_ = n_.subscribe("pcl_output", 1, &SubscribeAndPublish::callback, this);

    //Topic you want to subscribe
    
  }
 
  void callback(const sensor_msgs::PointCloud2 &input)
  {
    …… (点云处理程序)
    pub_.publish(output);
  }
private:
  ros::NodeHandle n_; 
  ros::Publisher pub_;
  ros::Subscriber sub_;
 
};//End of class SubscribeAndPublish

int main (int argc, char **argv)
{
  ros::init (argc, argv, "pcl_write");
  SubscribeAndPublish SAPObject;
  ros::spin();
  return 0;
}

最后还需要对于pcl::Pointcloud数据和ROS中点云信息发布格式sensor_msgs::PointCloud2之间进行合理转化。
当然接下来会将该套单目标跟踪算法放在实际的测试平台(一只机器狗)上进行测试,后期可能还会加入卡尔曼滤波跟踪算法。

©著作权归作者所有,转载或内容合作请联系作者
平台声明:文章内容(如有图片或视频亦包括在内)由作者上传并发布,文章内容仅代表作者本人观点,简书系信息发布平台,仅提供信息存储服务。

推荐阅读更多精彩内容

  • 2017年无人驾驶还处于积极研发和普及期,民用的测试和技术探索也主要在小型汽车方面,各大巨头都将2020年作为一个...
    方弟阅读 7,235评论 1 16
  • 作者:温利武 班级:1402019 学号:14020199041 【嵌牛导读】:本文梳理总结了前面11篇涉及...
    wlw_1fdf阅读 1,102评论 2 6
  • 春天里江南风吹过小草旁边 游人走过江南路上风吹过人身边 湖里吹过江南风 江南风吹过江南雨下来
    王密亮阅读 236评论 0 1
  • 在武汉洪山牵引力教育Python培训要学习多长时间才可以掌握呢? 相信有了解的人都知道,Python目前是首选的A...
    Hannah汪阅读 100评论 0 0
  • 第8天·21天告别拖延 #玩卡不卡·每日一抽# 每一位都可以通过这张卡片觉察自己: 1、直觉他叫什么名字?麻子 2...
    田__田阅读 124评论 0 0