精选【Livox 教程】Livox 的 lvx 点云格式转 pcd

宇宙爆肝锦标赛冠军

个人专栏：PCL

发布时间 2023.11.27阅读数 1928 评论数 0

问题背景

Livox 激光雷达采集的数据可以通过 ROS 驱动进行读取，但是有的情况下我们并不在设备上安装 ROS ，其实 ROS 驱动只是在 Livox SDK 的上层进行了封装，将原始的数据转化为 ROS 下的点云格式。Livox SDK 可以将点云数据存储为其自己定义的 lvx 格式，但是常用的点云处理库 PCL 并不支持 lvx 的读入，所以这篇博客介绍如何将 lvx 格式转化为 pcd 格式的点云。

格式介绍

当涉及到三维数据处理，特别是在地理信息系统（GIS）、机器人技术、自动驾驶等领域中，点云格式的选择至关重要。在使用 Livox 激光雷达的过程中，经常需要将 LVX 格式的点云转化为 PCL 点云库常用的 PCD 的格式。

LVX点云格式

LVX格式是专为Livox激光雷达设计的点云格式。它主要用于存储这些设备捕获的原始点云数据，包括三维坐标和反射率等信息。

关键特点:

特定于Livox激光雷达设备。
包含时间戳，有利于数据分析。
需要特定软件或工具来处理。
应用于自动驾驶、机器人和测绘。

PCD 点云格式

PCD是Point Cloud Library（PCL）推广的格式，专门用于存储和处理三维点云数据。这种格式支持包括位置、颜色和法线在内的多种数据类型。

关键特点:

设计用于PCL，支持广泛的数据处理功能。
支持ASCII和二进制两种格式。
应用于计算机视觉、三维建模和机器人技术。
与PCL集成提供了数据处理的灵活性和强大功能。

FMT

简介 FMT：现代 C++ 格式化库

在现代 C++ 开发中，高效且安全的字符串处理至关重要。这就是 FMT 库发挥作用的地方。FMT 是一个现代 C++ 格式化库，旨在提供一种比传统 printf 或 iostream 更高效、更安全的字符串格式化方式。以下是 FMT 的一些核心特点：

类型安全：与 printf 不同，FMT 在编译时检查类型，减少运行时错误。
高性能：性能优于 iostream，有时甚至超过 printf。
简洁的格式化语法：基于 Python 的 str.format()，使字符串格式化更直观。
易于扩展：可以为自定义类型定义格式化规则。
现代 C++ 兼容性：支持 C++11 及更高版本。

FMT 在数据转换中的应用

在处理复杂的数据转换任务，如将 LVX 点云格式转换为 PCD 格式时，FMT 的应用显得尤为重要。下面是一些使用 FMT 的理由：

高效的格式化输出：转换点云数据时，需要生成结构化的输出（如 PCD 文件头）。FMT 提供了高效且易读的方式来处理这种格式化输出。
性能优势：处理大量点云数据时，FMT 的高效性能可以显著提高数据处理速度。
类型安全：转换点云数据涉及多种数据类型，FMT 的类型安全特性可以减少类型错误，增强代码的稳定性。
代码简洁性：FMT 的 API 设计简洁，有助于使代码更加清晰，特别是在处理复杂的数据转换逻辑时。
跨平台兼容：FMT 的跨平台特性使得开发的工具更具可移植性。

从源码编译安装 FMT

FMT 源码下载链接：Overview — fmt 10.1.0 documentation

cd fmt
mkdir build
cd build
cmake ..
make
sudo make install

CMake 引入 FMT 库

# Import FMT
find_package(FMT REQUIRED)
if(FMT_FOUND)
    message( STATUS "FMT_FOUND: " ${FMT_FOUND})
    message( STATUS "FMT_INCLUDE_DIRS: " ${FMT_INCLUDE_DIRS})
    message( STATUS "FMT_LIBRARY_DIRS: " ${FMT_LIBRARY_DIRS})
    include_directories(${FMT_INCLUDE_DIRS})
else()
    message(err: FMT not found)
endif()

target_link_libraries(${target_name}
    ${FMT_LIBRARIES} # 新引入
    ${EIGEN3_LIBRARIES}
    ${PCL_LIBRARIES}
    ${Boost_LIBRARIES}
    fmt::fmt # 新引入
    stdc++fs # 新引入
    )

LVX -> PCD 格式转换

代码解析

代码中定义了许多结构体，如下图所示：

下面是对上图中各个结构体的解释：

类名	描述	属性
PcdPoint	表示点云中的一个点，包括其坐标和强度。	- float x - float y - float z - float intensity
PublicHeader	文件格式的公共头部结构。	- char file_signature[16] - uint8_t versionA - uint8_t versionB - uint8_t versionC - uint8_t versionD - char magic_code[4]
PrivateHeader	文件格式的私有头部结构。	- unsigned int frame_duration - unsigned char device_count
DeviceInfo	包含特定设备的信息。	- char lidar_sn[16] - char hub_sn[16] - unsigned char device_index - unsigned char device_type - unsigned char extrinsic_enable - float roll - float pitch - float yaw - float x - float y - float z
FrameHeader	表示文件中帧的头部结构。	- long long cur_offset - long long next_offset - long long frame_index
Package	表示文件中的数据包。	- unsigned char device_index - uint8_t version - uint8_t slot_id - uint8_t lidar_id - uint8_t reserved - uint32_t status_code - uint8_t timestamp_type - uint8_t data_type - uint64_t timestamp
Datatype_0	表示文件中的特定数据格式。	- int32_t x - int32_t y - int32_t z - int32_t reflectivity
Datatype_1	文件中的另一种特定数据格式。	- int32_t depth - int32_t theta - int32_t phi - int32_t reflectivity
Datatype_2	表示文件中另一种变体的数据格式。	- int32_t x - int32_t y - int32_t z - uint8_t reflectivity - uint8_t tag
…	其他数据类型（Datatype_3至Datatype_8）遵循类似的模式，每种都有自己的字段。	根据数据类型的不同而变化。

实现流程

下图是格式转换代码的流程图：

源码

#include <iostream>
#include <chrono>
#include <thread>
#include <experimental/filesystem>
#include <fstream>
#include <string>
#include <fmt/format.h>

#ifdef _DEBUG
#pragma comment(lib,"fmtd.lib")
#else
#pragma comment(lib,"fmt.lib")
#endif


#pragma pack(push,1)
struct PcdPoint
{
    float x;
    float y;
    float z;
    float intensity;
};

// Public Header结构体
struct PublicHeader {
    char file_signature[16];
    uint8_t versionA;
    uint8_t versionB;
    uint8_t versionC;
    uint8_t versionD;
    char magic_code[4];
    // filesignature前10个字节为livox_tech,后6个字节填充为0
    //versionA versionB为1，versionC versionD为0，表示此格式的版本
    //magiccode为0xAC0EA767
};
// Private Header结构体
struct PrivateHeader {
    unsigned int frame_duration;//一帧的持续时间，单位ms
    unsigned char device_count;//DeviceInfo的数量
};

// Device Info结构体
struct DeviceInfo {
    char lidar_sn[16];
    char hub_sn[16];
    unsigned char device_index; //Index in device info list
    unsigned char device_type; //设备类型 0：lidar hub  1:Mid-40.100 2:Tele-15 4:Horizon
    unsigned char extrinsic_enable;//0：禁用外部参数，应在没有外部参数的情况下计算点云；   1：启用外部参数，应使用外部参数计算云点；
    float roll;
    float pitch;
    float yaw;
    float x;
    float y;
    float z;
};

// Frame Header结构体
struct FrameHeader {
    long long cur_offset;
    long long next_offset;
    long long frame_index;
};
// Package结构体
struct Package {
    unsigned char device_index;//refer to deviceInfo
    uint8_t version;
    uint8_t slot_id;
    uint8_t lidar_id;
    uint8_t reserved;
    uint32_t status_code;
    uint8_t timestamp_type;
    uint8_t data_type;//点云坐标格式
    uint64_t timestamp; //ns
};

struct Datatype_0 {
    int32_t x;
    int32_t y;
    int32_t z;
    int32_t reflectivity;
};

struct Datatype_1 {
    int32_t depth;
    int32_t theta;
    int32_t phi;
    int32_t reflectivity;
};

struct Datatype_2 {
    int32_t x;
    int32_t y;
    int32_t z;
    uint8_t reflectivity;
    uint8_t tag;
};

struct Datatype_3 {
    int32_t depth;
    int32_t theta;
    int32_t phi;
    int32_t reflectivity;
    uint8_t tag;
};

struct Datatype_4 {
    int32_t x1;
    int32_t y1;
    int32_t z1;
    uint8_t reflectivity1;
    uint8_t tag1;
    int32_t x2;
    int32_t y2;
    int32_t z2;
    uint8_t reflectivity2;
    uint8_t tag2;
};

struct Datatype_5 {
    int32_t theta;
    int32_t phi;
    int32_t depth1;
    int32_t reflectivity1;
    uint8_t tag1;
    int32_t depth2;
    int32_t reflectivity2;
    uint8_t tag2;
};

struct Datatype_6 {
    float gyro_x;
    float gyro_y;
    float gyro_z;
    float acc_x;
    float acc_y;
    float acc_z;
};

struct Datatype_7 {
    int32_t x1;            /**< X axis, Unit:mm */
    int32_t y1;            /**< Y axis, Unit:mm */
    int32_t z1;            /**< Z axis, Unit:mm */
    uint8_t reflectivity1; /**< Reflectivity */
    uint8_t tag1;          /**< Tag */
    int32_t x2;            /**< X axis, Unit:mm */
    int32_t y2;            /**< Y axis, Unit:mm */
    int32_t z2;            /**< Z axis, Unit:mm */
    uint8_t reflectivity2; /**< Reflectivity */
    uint8_t tag2;          /**< Tag */
    int32_t x3;            /**< X axis, Unit:mm */
    int32_t y3;            /**< Y axis, Unit:mm */
    int32_t z3;            /**< Z axis, Unit:mm */
    uint8_t reflectivity3; /**< Reflectivity */
    uint8_t tag3;          /**< Tag */
};

/** Triple extend spherical coordinate format. */
struct Datatype_8 {
    uint16_t theta;        /**< Zenith angle[0, 18000], Unit: 0.01 degree */
    uint16_t phi;          /**< Azimuth[0, 36000], Unit: 0.01 degree */
    uint32_t depth1;       /**< Depth, Unit: mm */
    uint8_t reflectivity1; /**< Reflectivity */
    uint8_t tag1;          /**< Tag */
    uint32_t depth2;       /**< Depth, Unit: mm */
    uint8_t reflectivity2; /**< Reflectivity */
    uint8_t tag2;          /**< Tag */
    uint32_t depth3;       /**< Depth, Unit: mm */
    uint8_t reflectivity3; /**< Reflectivity */
    uint8_t tag3;          /**< Tag */
};


#pragma pack(pop)  // 恢复默认的字节对齐方式


void Convert(std::experimental::filesystem::path filepath)
{
    std::cout << filepath.string() << std::endl;

    auto root = filepath.parent_path();
    auto name = filepath.filename().string();

    auto dir = root / (name + "_output");
    std::experimental::filesystem::create_directories(dir);

    int frame_index = 0;
    int file_index = 0;
    std::vector<PcdPoint> cloud_points;

    std::ifstream file(filepath, std::ios::binary);  // 打开二进制文件
    if (!file) {
        std::cout << "无法打开文件." << std::endl;
        return;
    }

    // 读取Public Header
    PublicHeader publicHeader;
    file.read(reinterpret_cast<char*>(&publicHeader), sizeof(PublicHeader));

    // 读取Private Header
    PrivateHeader privateHeader;
    file.read(reinterpret_cast<char*>(&privateHeader), sizeof(PrivateHeader));
    // 读取DeviceInfo
    DeviceInfo deviceInfo;
    file.read(reinterpret_cast<char*>(&deviceInfo), sizeof(DeviceInfo));

    while (file.peek() != EOF) {
        long long bytes = 24;//frameheader字节24
        // 读取Frame Header
        FrameHeader frame_header;
        file.read(reinterpret_cast<char*>(&frame_header), sizeof(FrameHeader));
        // 计算每个Frame的字节数
        uint32_t frame_size = frame_header.next_offset - frame_header.cur_offset;

        frame_index++;
        while (bytes < frame_size) {
            Package package;
            file.read(reinterpret_cast<char*>(&package), sizeof(Package));
            bytes += 19;
            if (package.data_type == 7)
            {
                for (int i = 0; i < 30; i++)
                {
                    Datatype_7 pointdata;
                    file.read(reinterpret_cast<char*>(&pointdata), sizeof(Datatype_7));
                    bytes += sizeof(Datatype_7);

                    PcdPoint pt;
                    pt.x = double((pointdata.x1) / 1000.f);
                    pt.y = double((pointdata.y1) / 1000.f);
                    pt.z = double((pointdata.z1) / 1000.f);
                    pt.intensity = pointdata.reflectivity1;

                    cloud_points.push_back(pt);
                }
            }
            else if (package.data_type == 6)
            {
                Datatype_6 pointdata;
                file.read(reinterpret_cast<char*>(&pointdata), sizeof(Datatype_6));
                bytes += sizeof(Datatype_6);
            }
            else if (package.data_type == 0)
            {
                for (int i = 0; i < 100; i++)
                {
                    Datatype_0 pointdata;
                    file.read(reinterpret_cast<char*>(&pointdata), sizeof(Datatype_0));
                    bytes += sizeof(Datatype_0);

                    PcdPoint pt;
                    pt.x = double((pointdata.x) / 1000.f);
                    pt.y = double((pointdata.y) / 1000.f);
                    pt.z = double((pointdata.z) / 1000.f);
                    pt.intensity = pointdata.reflectivity;

                    cloud_points.push_back(pt);
                }
            }
            else if (package.data_type == 2)
            {
                for (int i = 0; i < 96; i++)
                {
                    Datatype_2 pointdata;
                    file.read(reinterpret_cast<char*>(&pointdata), sizeof(Datatype_2));
                    bytes += sizeof(Datatype_2);

                    PcdPoint pt;
                    pt.x = double((pointdata.x) / 1000.f);
                    pt.y = double((pointdata.y) / 1000.f);
                    pt.z = double((pointdata.z) / 1000.f);
                    pt.intensity = pointdata.reflectivity;

                    cloud_points.push_back(pt);
                }
            }
        }
        std::cout << frame_index << std::endl;
        if (frame_index == 5)
        {
            auto filename = fmt::format("{}.pcd", ++file_index);
            auto filepath = dir / filename;

            std::string pcd_header =
                "# .PCD v.7 - Point Cloud Data file format\n"
                "FIELDS "
                "x y z intensity\n"
                "SIZE 4 4 4 4\n"
                "TYPE F F F F\n"
                "COUNT 1 1 1 1\n"
                "WIDTH {}\n"
                "HEIGHT 1\n"
                "VIEWPOINT 0 0 0 1 0 0 0\n"
                "POINTS {}\n"
                "DATA binary\n";

            std::ofstream fs(filepath, std::ios::binary);
            auto text = fmt::format(pcd_header, (int)cloud_points.size(), (int)cloud_points.size());
            fs.write(text.c_str(), text.length());
            fs.write((char*)cloud_points.data(), cloud_points.size() * sizeof(PcdPoint));
            fs.close();

            frame_index = 0;
            cloud_points.clear();
        }
    }

    file.close();
}

int main(int argc, char** argv) {

    Convert(argv[1]);

    return 0;
}

点云激光雷达 PCL C++

原创文章作者：宇宙爆肝锦标赛冠军。如若转载，请注明出处：古月居 https://www.guyuehome.com/45263

打赏 0

上一篇：【Livox 教程】Livox 的 lvx 点云格式转 las

下一篇：【PX4 教程（三）】无人机搭载 Livox 激光雷达使用 FAST-LIO 建图

精选【Livox 教程】Livox 的 lvx 点云格式转 pcd

宇宙爆肝锦标赛冠军

目录

问题背景

格式介绍

LVX点云格式

PCD 点云格式

FMT

简介 FMT：现代 C++ 格式化库

FMT 在数据转换中的应用

从源码编译安装 FMT

CMake 引入 FMT 库

LVX -> PCD 格式转换

代码解析

实现流程

源码

为你推荐

《HALCON数字图像处理》第六章笔记

BEVDepth: Acquisition of Reliable Depth for Multi-view 3D Object Detection 论文笔记

halcon：读取摄像头并二值化

RoboMaster机甲大师——视觉组——摄像头的VideoCapture读取中cvGetCaptureProperty参数大全

[OpenCV]超分放大

Canny算子边缘检测原理及实现

评论（0）

关于作者

宇宙爆肝锦标赛冠军

80

0

8

5

精选【PCL点云库教程（六）】2D-3D 融合的目标检测之视锥滤波

精选【基于 QT5 的 ROS2 GUI 开发教程（二）】基于 librviz 可视化点云（附 Livox ROS2 驱动和功能包配置）

【Ubuntu16.04_Bug】打开Jupyter notebook报错：UnicodeDecodeError: ‘ascii‘ codec can‘t decode byte……

相关推荐

基于opencv,openpose及caffe模型库实现手势识别

暗通道实验结果与后记

Roberts算子边缘检测

精选基于树莓派4B的YOLOv5-Lite目标检测的移植与部署（含训练教程）

Ubuntu 18.04/20.04 CV环境配置（下）--手势识别TRTpose+Kinect DK人体骨骼识别

大津法（最大类间方差法OTSU）

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

TA的专栏

边缘计算

ChatGPT-for-Robotics

3D视觉

Pybind11

mmdetection

实例分割

Webots

移动机器人

coppeliaSim

PX4

目标检测

Coppeliasim｜轨迹规划

图像与点云融合教程

三维轨迹规划与控制系统

PCL

工业机器人

基于 QT5 的 ROS2 GUI 开发教程

Unity-Robotic-Hub 入门教程

专栏导航

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板