ROS入門——PCL激光雷達點雲處理（1）

前言：

參考書：《ros機器人高效編程》

源碼地址：http://www.hzcourse.com/web/refbook/detail/7182/226

配置：ubuntu 16.04 、ROS-kinetic

零、創建ros工作空間

1、在home下新建文件夾

mkdir -p ~/catkin_ws/src

2、進入src文件夾並初始化

cd ~/catkin_ws/src
catkin_init_workspace

執行完該命令後，src目錄下會多出一個 CMakeLists.txt 文件，這個文件一般不需要我們修改。

3、返回到catkin_ws下，進行編譯(注意：每次編譯必須在這個ws工作空間下才能編譯成功！)

cd ~/catkin_ws/
catkin_make

執行完該命令後，發現工作空間catkin_ws中有三個目錄： build devel src。可以從5、看到它們的作用。

4、source一下將工作空間加入環境變量

source devel/setup.bash

注意： 這一步只重新加載了setup.bash文件。如果關閉並打開一個新的命令行窗口，也需要再輸入該命令將得到同樣的效果。

所以建議採用一勞永逸的方法：.

5、.bashrc文件在用戶的home文件夾(/home/USERNAME/.bashrc)下。每次用戶打開終端，這個文件會加載命令行窗口或終端的配置。所以可以添加命令或進行配置以方便用戶使用。在bashrc文件添加source命令：

echo "source ~/catkin_ws/devel/setup.bash">> ~/.bashrc

或者也可以打開.bashrc文件採用手動修改的方式添加source ~/catkin_ws/devel/setup.bash：

gedit ~/.bashrc

添加完畢，你的bashrc文件應該有兩句source:

5、理解工作空間

工作空間結構：

源空間(src文件夾)，放置了功能包、項目、複製的包等。在這個空間中,，最重要的一個文件是CMakeLists.txt。當在工作空間中配置包時,src文件夾中有CMakeLists.txt因爲cmake調用它。這個文件是通過catkin_init_workspace命令創建的。

編譯空間(build space):在build文件夾裏,cmake和catkin爲功能包和項目保存緩存信息、配置和其他中間文件。

開發空間(Development(devel)space):devel文件夾用來保存編譯後的程序,這些是無須安裝就能用來測試的程序。一旦項目通過測試,就可以安裝或導出功能包從而與其他開發人員分享。

一、創建節點發布點雲數據並可視化

1、在ros工作空間的src目錄下新建包（包含依賴項）

catkin_create_pkg chapter10_tutorials pcl_conversions pcl_ros pcl_msgs sensor_msgs

3、在軟件包中新建src目錄

rospack profile
roscd chapter10_tutorials
mkdir src

4、在src目錄下新建pcl_create.cpp，該程序創建了100個隨機座標的點雲並以1Hz的頻率，topic爲“pcl_output"發佈。frame設爲odom。

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_create");

    ros::NodeHandle nh;
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);
    pcl::PointCloud<pcl::PointXYZ> cloud;
    sensor_msgs::PointCloud2 output;

    // Fill in the cloud data
    cloud.width  = 100;
    cloud.height = 1;
    cloud.points.resize(cloud.width * cloud.height);

    for (size_t i = 0; i < cloud.points.size (); ++i)
    {
        cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
    }

    //Convert the cloud to ROS message
    pcl::toROSMsg(cloud, output);
    output.header.frame_id = "odom";

    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

5、修改cmakelist.txt內容

cmake_minimum_required(VERSION 2.8.3)
project(chapter10_tutorials)
find_package(catkin REQUIRED COMPONENTS
  pcl_conversions
  pcl_ros
  roscpp
  sensor_msgs
  rospy
)

find_package(PCL REQUIRED)
catkin_package()

include_directories(
  ${catkin_INCLUDE_DIRS}
  ${PCL_INCLUDE_DIRS}
)

link_directories(${PCL_LIBRARY_DIRS})


add_executable(pcl_create src/pcl_create.cpp)
target_link_libraries(pcl_create ${catkin_LIBRARIES} ${PCL_LIBRARIES})

6、進入工作空間編譯包

cd ~/catkin_ws
catkin_make --pkg chapter10_tutorials

7、若編譯成功，新窗口打開ros，新窗口運行pcl_create節點

roscore
rosrun chapter10_tutorials pcl_create

8、新窗口打開rviz，add topic"pcl_output"，Global options 設置Frame爲odom

rviz

二、加載pcd文件、保存點云爲pcd文件

1、加載pcd文件併發布爲pcl_output點雲：在src下新建pcl_read.cpp，內容爲：

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>

main(int argc, char **argv)
{
    ros::init (argc, argv, "pcl_read");

    ros::NodeHandle nh;
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);

    sensor_msgs::PointCloud2 output;
    pcl::PointCloud<pcl::PointXYZ> cloud;

    pcl::io::loadPCDFile ("test_pcd.pcd", cloud);

    pcl::toROSMsg(cloud, output);
    output.header.frame_id = "odom";

    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

2、保存topic發佈的點云爲pcd文件，在src下新建pcl_write.cpp內容爲：

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>

void cloudCB(const sensor_msgs::PointCloud2 &input)
{
    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::fromROSMsg(input, cloud);
    pcl::io::savePCDFileASCII ("write_pcd_test.pcd", cloud);
}

main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_write");
    ros::NodeHandle nh;
    ros::Subscriber bat_sub = nh.subscribe("pcl_output", 10, cloudCB);
    ros::spin();

    return 0;
}

3、添加內容到cmakelist.txt

add_executable(pcl_read src/pcl_read.cpp)
add_executable(pcl_write src/pcl_write.cpp)

target_link_libraries(pcl_read ${catkin_LIBRARIES} ${PCL_LIBRARIES})
target_link_libraries(pcl_write ${catkin_LIBRARIES} ${PCL_LIBRARIES})

4、在catkin_ws空間下編譯包（同上）

5、打開不同的窗口，在pcd文件夾下分別運行節點（因爲pcl_read要讀取pcd文件）

roscore
roscd chapter10_tutorials/data && rosrun chapter10_tutorials pcl_read
roscd chapter10_tutorials/data && rosrun chapter10_tutorials pcl_write

6、可視化同上

三、cloud_viewer可視化pcd文件的點雲

新建cpp文件，所有步驟同上。

#include <iostream>
#include <ros/ros.h>
#include <pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>

class cloudHandler
{
public:
    cloudHandler()
    : viewer("Cloud Viewer")
    {
     pcl::PointCloud<pcl::PointXYZ> cloud;
     pcl::io::loadPCDFile ("0.pcd", cloud);
     viewer.showCloud(cloud.makeShared());
     viewer_timer = nh.createTimer(ros::Duration(0.1), &cloudHandler::timerCB, this);
    }

    void timerCB(const ros::TimerEvent&)
    {
        if (viewer.wasStopped())
        {
            ros::shutdown();
        }
    }

protected:
    ros::NodeHandle nh;
    pcl::visualization::CloudViewer viewer;
    ros::Timer viewer_timer;
};

main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_filter");
    cloudHandler handler;
    ros::spin();
    return 0;
}

編譯並在pcd數據文件夾下運行節點，可得下圖。可以按住ctrl鍵滑輪放大縮小。

四、點雲預處理——濾波和縮減採樣

1、濾波刪除離羣值pcl_filter.cpp，處理流程同上

#include <iostream>
#include <ros/ros.h>
#include <pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/statistical_outlier_removal.h>

class cloudHandler
{
public:
    cloudHandler()
    : viewer("Cloud Viewer")
    {
     pcl::PointCloud<pcl::PointXYZ> cloud;
     pcl::PointCloud<pcl::PointXYZ> cloud_filtered;
     pcl::io::loadPCDFile ("0.pcd", cloud);

     pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statFilter;//統計離羣值算法
        statFilter.setInputCloud(cloud.makeShared());//輸入點雲
        statFilter.setMeanK(10);//均值濾波
        statFilter.setStddevMulThresh(0.4);//方差0.4
        statFilter.filter(cloud_filtered);//輸出結果到點雲

     viewer.showCloud(cloud_filtered.makeShared());
     viewer_timer = nh.createTimer(ros::Duration(0.1), &cloudHandler::timerCB, this);
    }

    void timerCB(const ros::TimerEvent&)
    {
        if (viewer.wasStopped())
        {
            ros::shutdown();
        }
    }

protected:
    ros::NodeHandle nh;
    
    pcl::visualization::CloudViewer viewer;
    ros::Timer viewer_timer;
};

main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_filter");
    cloudHandler handler;
    ros::spin();
    return 0;
}

濾波結果：

2、濾波以後縮減採樣pcl_dawnSample.cpp，採用體素柵格的方法，將點雲分割爲若干的小立方體（體素），以體素重心的點代表這個體素中所有的點。

public:
    cloudHandler()
    : viewer("Cloud Viewer")
    {
     pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::PointCloud<pcl::PointXYZ> cloud_filtered;
 pcl::PointCloud<pcl::PointXYZ> cloud_downsampled;

     pcl::io::loadPCDFile ("0.pcd", cloud);

//剔除離羣值
      pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statFilter;
        statFilter.setInputCloud(cloud.makeShared());
        statFilter.setMeanK(10);
        statFilter.setStddevMulThresh(0.2);
        statFilter.filter(cloud_filtered);
//縮減採樣
 	pcl::VoxelGrid<pcl::PointXYZ> voxelSampler;//初始化 體素柵格濾波器
        voxelSampler.setInputCloud(cloud_filtered.makeShared());//輸入點雲
        voxelSampler.setLeafSize(0.01f, 0.01f, 0.01f);//每個體素的長寬高0.01m
        voxelSampler.filter(cloud_downsampled);//輸出點雲結果

     viewer.showCloud(cloud_downsampled.makeShared());

     viewer_timer = nh.createTimer(ros::Duration(0.1), &cloudHandler::timerCB, this);
    }

縮減採樣結果：

五、點雲預處理——點雲分割

1、pcl_segmentation.cpp採用RANSAC算法提取點雲的plane平面。處理步驟同一

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>

main(int argc, char **argv)
{
    ros::init (argc, argv, "pcl_filter");
    ros::NodeHandle nh;
    //初始化
     pcl::PointCloud<pcl::PointXYZ> cloud;
     pcl::PointCloud<pcl::PointXYZ> cloud_filtered;
     pcl::PointCloud<pcl::PointXYZ> cloud_downsampled;
     pcl::PointCloud<pcl::PointXYZ> cloud_segmented;
    ros::Publisher pcl_pub0 = nh.advertise<sensor_msgs::PointCloud2> ("pcl_cloud", 1);
    ros::Publisher pcl_pub1 = nh.advertise<sensor_msgs::PointCloud2> ("pcl_segment", 1);
    ros::Publisher ind_pub = nh.advertise<pcl_msgs::PointIndices>("point_indices", 1);
    ros::Publisher coef_pub = nh.advertise<pcl_msgs::ModelCoefficients>("planar_coef", 1);
        
    sensor_msgs::PointCloud2 output0;
    sensor_msgs::PointCloud2 output1;
    pcl::io::loadPCDFile ("0.pcd", cloud);
    pcl::toROSMsg(cloud, output0);
    output0.header.frame_id = "odom";
//剔除離羣值
      pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statFilter;
        statFilter.setInputCloud(cloud.makeShared());
        statFilter.setMeanK(10);
        statFilter.setStddevMulThresh(0.2);
        statFilter.filter(cloud_filtered);
//體素柵格法下采樣
 	pcl::VoxelGrid<pcl::PointXYZ> voxelSampler;
        voxelSampler.setInputCloud(cloud_filtered.makeShared());
        voxelSampler.setLeafSize(0.01f, 0.01f, 0.01f);
        voxelSampler.filter(cloud_downsampled);
//RANSAC算法 分割
	pcl::ModelCoefficients coefficients;//初始化模型係數
        pcl::PointIndices::Ptr inliers(new pcl::PointIndices());//初始化索引參數
        pcl::SACSegmentation<pcl::PointXYZ> segmentation;//創建算法
        segmentation.setModelType(pcl::SACMODEL_PLANE);//設置分割模型爲平面模型
        segmentation.setMethodType(pcl::SAC_RANSAC);//設置迭代算法
        segmentation.setMaxIterations(1000);//設置最大迭代次數
        segmentation.setDistanceThreshold(0.01);//設置到模型的最大距離
        segmentation.setInputCloud(cloud_downsampled.makeShared());//輸入點雲
        segmentation.segment(*inliers, coefficients);//輸出點雲結果  ×inliers是結果點雲的索引，coe是模型係數
//發佈模型係數
        pcl_msgs::ModelCoefficients ros_coefficients;
        pcl_conversions::fromPCL(coefficients, ros_coefficients);//pcl->msg
        
//發佈抽樣的內點索引
        pcl_msgs::PointIndices ros_inliers;
        pcl_conversions::fromPCL(*inliers, ros_inliers);
        

//創建分割點雲，從點雲中提取內點
        pcl::ExtractIndices<pcl::PointXYZ> extract;
        extract.setInputCloud(cloud_downsampled.makeShared());
        extract.setIndices(inliers);
        extract.setNegative(false);
        extract.filter(cloud_segmented);
        pcl::toROSMsg(cloud_segmented, output1);
        output1.header.frame_id = "odom";

    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        pcl_pub0.publish(output0);
        pcl_pub1.publish(output1);
        
        ind_pub.publish(ros_inliers);
        coef_pub.publish(ros_coefficients);//發佈
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

出現警告：

[pcl::VoxelGrid::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.

濾波縮減採樣後的結果：

平面濾波結果：

濾波縮減採樣結果：

平面濾波結果：

2、（這是一個錯誤的程序，等以後學明白了再來解釋爲什麼錯誤。viewer可以顯示分割出的點雲，不知道爲啥沒有發佈成功，在rviz中不能顯示，rostopic echo topic 也不能顯示消息。）

#include <iostream>
#include <ros/ros.h>
#include <pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_cloud.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>


class cloudHandler
{
public:
    cloudHandler()
     : viewer("Cloud Viewer")
    {
//初始化
     pcl::PointCloud<pcl::PointXYZ> cloud;
     pcl::PointCloud<pcl::PointXYZ> cloud_filtered;
     pcl::PointCloud<pcl::PointXYZ> cloud_downsampled;
     pcl::PointCloud<pcl::PointXYZ> cloud_segmented;
     pcl_pubb = nh.advertise<sensor_msgs::PointCloud2>("pcl_cloud", 1);
     pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_segmented", 1);
     ind_pub = nh.advertise<pcl_msgs::PointIndices>("point_indices", 1);
     coef_pub = nh.advertise<pcl_msgs::ModelCoefficients>("planar_coef", 1);
        
     pcl::io::loadPCDFile ("test_pcd.pcd", cloud);
//剔除離羣值
      pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statFilter;
        statFilter.setInputCloud(cloud.makeShared());
        statFilter.setMeanK(10);
        statFilter.setStddevMulThresh(0.2);
        statFilter.filter(cloud_filtered);
//體素柵格法下采樣
 	pcl::VoxelGrid<pcl::PointXYZ> voxelSampler;
        voxelSampler.setInputCloud(cloud_filtered.makeShared());
        voxelSampler.setLeafSize(0.01f, 0.01f, 0.01f);
        voxelSampler.filter(cloud_downsampled);

//RANSAC算法 分割
	pcl::ModelCoefficients coefficients;//初始化模型係數
        pcl::PointIndices::Ptr inliers(new pcl::PointIndices());//初始化索引參數
        pcl::SACSegmentation<pcl::PointXYZ> segmentation;//創建算法
        segmentation.setModelType(pcl::SACMODEL_PLANE);//設置分割模型爲平面模型
        segmentation.setMethodType(pcl::SAC_RANSAC);//設置迭代算法
        segmentation.setMaxIterations(1000);//設置最大迭代次數
        segmentation.setDistanceThreshold(0.01);//設置到模型的最大距離
        segmentation.setInputCloud(cloud_downsampled.makeShared());//輸入點雲
        segmentation.segment(*inliers, coefficients);//輸出點雲結果  ×inliers是結果點雲的索引，coe是模型係數
//發佈模型係數
        pcl_msgs::ModelCoefficients ros_coefficients;
        pcl_conversions::fromPCL(coefficients, ros_coefficients);//pcl->msg
        coef_pub.publish(ros_coefficients);//發佈
//發佈抽樣的內點索引
        pcl_msgs::PointIndices ros_inliers;
        pcl_conversions::fromPCL(*inliers, ros_inliers);
        ind_pub.publish(ros_inliers);

//創建分割點雲，從點雲中提取內點
        pcl::ExtractIndices<pcl::PointXYZ> extract;
        extract.setInputCloud(cloud_downsampled.makeShared());
        extract.setIndices(inliers);
        extract.setNegative(false);
        extract.filter(cloud_segmented);

//發佈點雲
        sensor_msgs::PointCloud2 output;
        pcl::toROSMsg(cloud_segmented, output);
        pcl_pub.publish(output);
        sensor_msgs::PointCloud2 outputt;
	pcl::toROSMsg(cloud,outputt);
        pcl_pubb.publish(outputt);

//可視化
      viewer.showCloud(cloud_segmented.makeShared());

      viewer_timer = nh.createTimer(ros::Duration(0.1), &cloudHandler::timerCB, this);
      }



    void timerCB(const ros::TimerEvent&)
    {
        if (viewer.wasStopped())
        {
            ros::shutdown();
        }
    }

protected:
    ros::NodeHandle nh;
    
    pcl::visualization::CloudViewer viewer;
    ros::Timer viewer_timer;
    ros::Publisher pcl_pubb,pcl_pub, ind_pub, coef_pub;
};

main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_filter");

    cloudHandler handler;

    ros::spin();

    return 0;
}

六、播放bag並可視化激光雷達點雲

1、查看無人艇的視頻和雷達數據20200116.bag信息：

rosbag info 20200116.bag

發現時長約3分鐘，topic有fix(衛星導航數據)、heading(姿態四元數)、points_raw（激光雷達點雲）、rosout(節點圖)、camera_info(攝像機信息)、image_raw(攝像機圖片)、time_reference(時間)、vel(速度？角速度？)

2、回放bag文件：

rosbag play 20200116.bag

3、打開rviz:

rosrun rviz rviz

4、點擊add--->add topic--->Pointcloud2(或 image)

5、Global Options ：Fixed Frame 改成pandar

（剛開始沒有改fixed frame，add topic以後總是顯示錯誤，後來在src下的雷達的包裏看到readme文件裏面寫着：4. Change fixed frame to `pandar`。所以在vriz裏修改了，圖像和點雲都可以顯示了。

ROS入門——PCL激光雷達點雲處理（1）

前言：

零、創建ros工作空間

一、創建節點發布點雲數據並可視化

二、加載pcd文件、保存點云爲pcd文件

三、cloud_viewer可視化pcd文件的點雲

四、點雲預處理——濾波和縮減採樣

五、點雲預處理——點雲分割

六、播放bag並可視化激光雷達點雲

DAPPER 事務 TRANSACTION

數學建模：無車承運人平臺線路定價

圖像處理——皮膚分割方法彙總（最全最有用的）

win10+ ubuntu16.04 雙系統及無線、輸入法、deepin-wineQQ微信等配置(親測)

ROS入門——ubuntu16.04 安裝和使用ROS 報錯

tensorflow學習（9）風格遷移中Gram矩陣的理解（最生動最直觀）

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結