目标检测与位姿估计（十五）：将深度图转换为pcd文件

最新推荐文章于 2024-10-11 16:09:56 发布

小白 AI 日记

最新推荐文章于 2024-10-11 16:09:56 发布

阅读量1.3k

点赞数 1

分类专栏：目标检测与位姿估计

本文链接：https://blog.csdn.net/qq_37662375/article/details/108351787

版权

目标检测与位姿估计专栏收录该内容

22 篇文章 23 订阅

订阅专栏

关于pcl工具我们之前介绍了很多种使用它的方法：python语言接口以及C++接口，甚至于ROS的版本。最后我选择了ROS版本，原因吗当然是方（lan）便（duo）。于是就需要自己写一个深度转化为pcd格式的接口。pcd文件很简单，只需要一个固定格式的header，以及每个点的xyz信息即可。当然，点云不止这一种形式，也可以是xyzi或者xyzrgb，可以根据需要定义。代码如下：

import numpy as np
import cv2 as cv2

# 定义header，这里给出了两种形式的点云
HEADERxyzi = '''\
# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z intensity
SIZE 4 4 4 4
TYPE F F F F
COUNT 1 1 1 1
WIDTH {}
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS {}
DATA ascii
'''

HEADERxyz = '''\
# .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 {}
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS {}
DATA ascii
'''

# 读取图片之后将xy和depth放进一个矩阵（每一个行的形式为（x， y， depth））中
def ReadPictureandTansPcl(imgpath):
    img = cv2.imread(imgpath, -1)
    imgsize = img.shape
    rows = imgsize[0]
    cols = imgsize[1]

    # 容器：盛放pcl的点云位置
    container = np.zeros(shape=(rows*cols, 3))
    # 这是计数器，可表示容器的行数量
    jishu=0
    # 均匀降采样
    ### 点云的生成相对较为浪费时间，根据目标检测box位置删除点云是第一种
    ### 提升速度方式，均匀降采样时第二种提升速度的方式
    HandDownSample = 1

    # 存放
    for i in range(0, rows, HandDownSample):
        for j in range(0, cols, HandDownSample):
            container[jishu,0]= i
            container[jishu,1]= j
            container[jishu,2]= img[i, j]
            jishu = jishu + 1
    return  container

# 写入pcl
def write_pcd(points, save_pcd_path):
    with open(save_pcd_path, 'w') as f:
        f.write(HEADERxyz.format(len(points), len(points)) + '\n')
        #np.savetxt(f, points, delimiter = ' ', fmt = '%f %f %f %d')
        np.savetxt(f, points, delimiter = ' ')


# 主函数
imgpath = '/root/cyse/RGBaddDEPYH/data/depth.png'
depth = ReadPictureandTansPcl(imgpath)
#print(depth)
write_pcd(depth,'/root/cyse/Piont2PCL/tete.pcd')