Velodyne雷达.PCAP文件转PCL点云格式文件(.PCD)

背景介绍

关于pcl,各种介绍文章以及官网也不少, 这篇仅仅针对pcap格式的雷达数据包转码pcl(官网给出的方法)进行一个复现, 补充一些个人在转换过程中的心得.毕竟此前从未接触过pcl相关领域.

相关参考文章

不得不说, 在使用过程中,对于pcl库的使用, 配置相关的依赖环境也折腾了很多时间…

pcl官方-macOS对pcl以及其依赖环境的安装
pcl官网-pacp转pcl
官方pcap数据下载地址
pcl文件(.pcd)的数据格式理解

读取pcl文件(.pcd)并显示

这里仅仅截取一段pcd文件,展示一下pcd文件的文件格式,知道的小伙伴就跳过好了,还没了解的可以自己先去了解

# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH 35947
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 35947
DATA ascii
-1.10698 3.272394 -0.447241
-1.80195 3.367094 -0.704211
-4.12496 5.602794 2.824819
2.44725 3.493394 1.427299
0.41545 3.145894 -0.179121

贴一下读取并可视化的代码pcd文件的代码,这里我写了一个PcdRead类, 这里我们读取pcl官网给出的经典的兔子点云数据.

//
// Created by tianNanYiHao on 2019-07-18.
//

#include "PcdRead.h"

#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace pcl;

/**
 * 读取pcd文件内容
 * @param filePath 文件路径
 */
void PcdRead::readPcdFile(const string filePath) {

    //创建了一个名为cloud的指针，储存XYZ类型的点云数据
    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);

    /*打开点云文件*/
    if (io::loadPCDFile<PointXYZ>(filePath, *cloud) == -1) {
        PCL_ERROR("Couldn't read file xxx.pcd\n");
        return ;
    }
    cout << "Loaded:" << cloud->width * cloud->height << "data points from test_pcd.pcd with the following fields:"
         << endl;
    for (size_t i = 0; i < cloud->points.size(); ++i) {
        cout << "      " << cloud->points[i].x << "   " << cloud->points[i].y << "   " << cloud->points[i].z
                  << "   " << endl;
    }

    cout << cloud->points.size() << endl;
    cout << "from  rabbit.pcd" << endl;


    /*点云pcd文件可视化*/
    visualization::PCLVisualizer::Ptr viewer (new visualization::PCLVisualizer ("3D Viewer"));
    viewer->addPointCloud (cloud, "rabbit-cloud");
    viewer->spin();
}

写入点云数据,并生成pcd格式文件

写入点云数据的相关文章其实网上很多,这里也贴一下,毕竟是pcap转pcd过程中的一个前置知识环节,同样知道的小伙伴也可以快速调过,这里补充一些,PcdWrite类大家只要看关于点云相关的内容就好, 关于json解析的那段其实是一开始我用wireshark软件提取了pacp的json格式,以为这玩意儿就是数据内容了,其实并不是,哈哈,尴尬.

//
// Created by tianNanYiHao on 2019-07-18.
//

#include "PcdWrite.h"
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 

using namespace std;
using namespace pcl;


/**
 * 数据写入pcd
 * @param filePath 读取数据的文件路径
 * @param savePath 写如数据的文件路径
 */
void PcdWrite::writePcdByFile(const string filePath, const string savePath) {



    /*创建点云并写入*/
    PointCloud<PointXYZ> myCloud;
    myCloud.width = 465/3;
    myCloud.height = 1;
    myCloud.is_dense = false;
    myCloud.points.resize(myCloud.width * myCloud.height);


    /*读取文件数据*/
    ifstream op;
    string filePathh = filePath;
    filePathh = "src/test.json";
    op.open(filePathh, ios::binary);
    if (!op.is_open()) {
        cout << "open json file failed." << endl;
        return;
    }
    cout << "打开" + filePathh + "成功" << endl;

    /*对流数据进行json解析*/
    JSONCPP_STRING errs;
    Json::Value root, data;
    Json::CharReaderBuilder readerBuilder;

    bool pares_ok = Json::parseFromStream(readerBuilder, op, &root, &errs);
    if (!pares_ok) {
        cout << "jsoncpp 读取json文件失败....." << endl;
        return;
    } else {
        cout << "jsoncpp 读取json文件成功....." << endl;
        try {
            for (int j = 0; j < root.size(); ++j) {
                data = root[j];
                cout << data << endl;
                for (int k = 0; k < data["data"].size(); ++k) {
//                    cout << data["data"][k] << endl;
                    if ((k+1)%3 ==0 && k>1){
//                        cout << "==---==" << endl;
//                        cout << k-2 << endl;
//                        cout << k-1 << endl;
//                        cout << k << endl;
//                        cout << "==***==" << endl;

                        myCloud.points[(k+1)/3].x = data["data"][k-2].asInt();
                        myCloud.points[(k+1)/3].y = data["data"][k-1].asInt();
                        myCloud.points[(k+1)/3].z = data["data"][k].asInt();
                    }
                }
            }
            /*输出pcd*/
            io::savePCDFileASCII(savePath, myCloud);
            cout << "pcd写入成功" << endl;

        } catch (const Json::LogicError &error) {
            cout << "json数据解析异常..." << endl;
        }
    }
}

读取pacp文件并可视化,同步生成pcd文件

1.这里说个概念, 上面我们看到的兔子点云,其实是一帧, 这里姑且不讨论一帧的概念是不是正确,就官网给出的pcap雷达数据包(100M大小), 其实是一个动态的数据记录,里面包含了几百上千个这样的一帧,所以说要么我们生成n多个pcd文件, 要么我们就得生成一个包含所有帧数的大的pcd文件
2. 这里代码我将pcl官网给的例子进行了改造, 其中有些报错或者官网照搬下来出现的问题,可以参考这个帖子 关于pcl官网velodyne激光雷达传感器获取点云的一些错误纠正
3. 再贴一下cmakelist.txt, 其中关于libpcl_visualization的路径问题也让我蛋疼,必须手动link.

cmake_minimum_required(VERSION 3.14)
project(ReadPcap)

set(CMAKE_CXX_STANDARD 14)

#[[pcl]]
find_package(PCL 1.9 REQUIRED COMPONENTS common io) # 至少最低支持1.3版本的pcl包

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

#[[boost]]
find_package(Boost REQUIRED COMPONENTS system thread)
include_directories(${Boost_INCLUDE_DIRS})

add_executable(ReadPcap main.cpp)

# 本工程
target_link_libraries(ReadPcap
        ${PCL_LIBRARIES}
        ${Boost_LIBRARIES}
        "/usr/local/lib/libpcl_visualization.dylib" # 必须手动link libpcl_visualization 否则报错找不到
        )

#include 
#include 
#include 
#include 
#include 
#include 
#include 

/*添加pcd_io头文件及point_types头文件支持savePCDFile()*/
#include 
#include 
#include 
#include 

using namespace std;
using namespace pcl::console;
using namespace pcl::visualization;

/*
 * 官网例子直接使用的问题, 需要额外增加如下代码
 * 1. 增加 using namespace pcl;
 * template；
 * 2.将 SimpleHDLViewer (Grabber& grabber,..    改为 SimpleHDLViewer (pcl::Grabber& grabber,...
    将SimpleHDLViewer v (grabber, color_handler); 改为SimpleHDLViewer v (grabber, color_handler); 
 * */
using namespace pcl;
//template;



class SimpleHDLViewer {
public:
    typedef pcl::PointCloud<pcl::PointXYZI> Cloud;
    typedef Cloud::ConstPtr CloudConstPtr;
    int pcdNum = 0;


    SimpleHDLViewer(pcl::Grabber &grabber,
                    pcl::visualization::PointCloudColorHandler<pcl::PointXYZI> &handler) :
            cloud_viewer_(new pcl::visualization::PCLVisualizer("PCL HDL Cloud")),
            grabber_(grabber),
            handler_(handler) {
    }

    /*点云-回调函数*/
    void cloud_callback(const CloudConstPtr &cloud) {
        cout << "执行点云回调" << endl;
        cout << cloud << endl;
        /*回调部分*/
        std::lock_guard<std::mutex> lock(cloud_mutex_);
        cloud_ = cloud;

        /*调用save方法,保存pcd文件, 仅观看pcl_viewer图像文件时,建议关闭保存函数,卡!*/
        savePcd_Files(cloud);

    }

    /**
     * 解析pcap文件的同时,保存pcd文件
     * @param cloud 入参
     */
    void savePcd_Files(const CloudConstPtr &cloud) {
        pcl::PointCloud<pcl::PointXYZ> cloud2; // 定义一个接收的点云数据,用于保存到写入文件pcd
        /*点云cloud数据写入pcd文件*/
        cloud2.width = cloud->points.size();
        cloud2.height = 1;
        cloud2.is_dense = false;  //不是稠密型的
        cloud2.points.resize(cloud2.width * cloud2.height);  //点云总数大小
        for (size_t i = 0; i < cloud->points.size(); ++i) {
            cout << "      " << cloud->points[i].x << "   " << cloud->points[i].y << "   " << cloud->points[i].z
                 << "   " << endl;
            cloud2.points[i].x = cloud->points[i].x;
            cloud2.points[i].y = cloud->points[i].y;
            cloud2.points[i].z = cloud->points[i].z;
        }
        pcdNum += 1;
        cout << pcdNum << endl;
        /*添加pcd保存函数*/
        string fontName = "input/part";
        string endName = ".pcd";
        ostringstream oss;
        oss << fontName << pcdNum << endName;
        io::savePCDFileASCII(oss.str(), cloud2);
    }


    void run() {
        cout << "run" << endl;
        cloud_viewer_->addCoordinateSystem(3.0);
        cloud_viewer_->setBackgroundColor(0, 0, 0);
        cloud_viewer_->initCameraParameters();
        cloud_viewer_->setCameraPosition(0.0, 0.0, 30.0, 0.0, 1.0, 0.0, 0);
        cloud_viewer_->setCameraClipDistances(0.0, 50.0);

        std::function<void(const CloudConstPtr &)> cloud_cb =
                [this](const CloudConstPtr &cloud) { cloud_callback(cloud); };
        boost::signals2::connection cloud_connection = grabber_.registerCallback(
                cloud_cb);

        grabber_.start();

        while (!cloud_viewer_->wasStopped()) {
            CloudConstPtr cloud;

            // See if we can get a cloud
            if (cloud_mutex_.try_lock()) {
                cloud_.swap(cloud);
                cloud_mutex_.unlock();
            }

            if (cloud) {
                handler_.setInputCloud(cloud);
                if (!cloud_viewer_->updatePointCloud(cloud, handler_, "HDL"))
                    cloud_viewer_->addPointCloud(cloud, handler_, "HDL");

                cloud_viewer_->spinOnce();
            }

            if (!grabber_.isRunning())
                cloud_viewer_->spin();

            boost::this_thread::sleep(boost::posix_time::microseconds(100));
        }

        grabber_.stop();

        cloud_connection.disconnect();
    }

    pcl::visualization::PCLVisualizer::Ptr cloud_viewer_;

    pcl::Grabber &grabber_;
    std::mutex cloud_mutex_;

    CloudConstPtr cloud_;
    pcl::visualization::PointCloudColorHandler<pcl::PointXYZI> &handler_;
};

int main(int argc, char **argv) {
    std::string hdlCalibration, pcapFile;

    parse_argument(argc, argv, "-calibrationFile", hdlCalibration);
    parse_argument(argc, argv, "-pcapFile", pcapFile);

    bool justRunIt = true;
    string pacpFile_def = "src/HDL32-V2_Tunnel.pcap";
//    string pacpFile_def = "src/HDL32-V2_R into Butterfield into Digital Drive.pcap";
//    string pacpFile_def = "src/4 intersecton w police car.pcap";
//    string pacpFile_def = "src/2014-11-10-11-32-17_Velodyne-VLP_10Hz_Monterey Highway.pcap";
//    string pacpFile_def = "src/data.pcap";

    pcl::HDLGrabber grabber(hdlCalibration, (justRunIt ? pacpFile_def : pcapFile));
    pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> color_handler("intensity");
    SimpleHDLViewer v(grabber, color_handler);
    v.run();
    return (0);
}

转换出的pcd文件, 我们可以用上面的PcdRead类来读取一下数据并验证一下

以上code源码在此

疑问以及补充

以上是针对pcl官网给出的pcap数据进行的转码, 但实际情况是我从别人那边拿到的雷达实测数据包, 直接解析还不行, 查阅资料显示 需要我不仅仅提供pcap文件, 还需要提供随雷达而来的CalibrationFile 文件,好像是xml格式的, 因为我自己没有这个雷达以及其他原因,我手头并没有CalibrationFile 校准文件, 也无法读取真实的pcap文件, 所以目前还没有真正解析手头上的pcap文件,谁可以提供一下就更好了,如果该文件可以通用的话…

补充2

这是近期我升级macOS到10.14以后再开启工程出现的错误, 解决办法也很粗暴,
直接重建一个新工程,文件通通迁移过去完成编译…