3d点云处理 opencv 3d点云入门

文章目录

• O*、NeRF数据与代码解读（相机参数与坐标系变换）

• 1.总体概览
• 2.相机的内外参数
• 3.如何获得相机参数（colmap可估计img位姿）
• 3.5 colmap使用技巧：
• 4.缩放图像需要修改什么相机参数？
• 5.3D空间射线怎么构造

• 一、KITTI数据集介绍（重点是lidar-图像坐标系转换）

• 1.数据格式

• 1.激光雷达数据（data_object_velodyne）
• 可视化
• 2.标注数据label_2.
• 3.图像数据image_2
• 4.标定校准数据calib

• 2.KITTI数据集中的三个坐标系：
• 3.点云数据转鸟瞰图（BEV）
• 4.将标注画在鸟瞰图（BEV）上
• 5. 将标注画在点云上

• 二、几种点云可视化方式

• **方案一**：mayavi可视化点云
• **方案二**、matplotlib可视化点云
• **方案三**、CloudCompare可视化点云
• **Mayavi常用函数**

• 三、点云数据变换

• 1.pcd点云可视化与移动（利用open3d）
• 2.pcd点云旋转

• 1.1 根据欧拉角计算旋转矩阵
• 1.2 根据旋转向量（轴角）计算旋转矩阵
• 1.3 根据四元数计算旋转矩阵

• 3.点云仿射变换
• 4.点云缩放
• 5. ply 与其他数据处理

• ply 数据读取（open3d、plyfile、 直接打开）
• 保存ply文件
• ply转pcd
• ply转bin，任意点云格式转ply
• semantic kitti数据

• 7.下采样

• 最远点采样FPS
• 体素下采样（open3d）
• 均匀下采样(open3d)
• 随机下采样(open3d 与 numpy )

• 8.点云离群点剔除 （open3d）
• 9、聚类

• 四、点云数据增强
• 五、分割可视化
• 六、逆透视变换
• 七、NeRF-Studio 与 meshroom

• 1.使用meshroom裁剪全景图像
• 2.NeRF-Studio 估计图像位姿态

---

O*、NeRF数据与代码解读（相机参数与坐标系变换）

NeRF: Representing Scenes as Neural Radiance Fields for View Synthesis 是一篇获得ECCV2020 Best Paper Honorable Mention的论文。给定一个场景的多视角的图像，神经辐射场（NeRF）通过图像重建误差优化一个神经场景表征。优化后可以实现逼真的新视角合成效果。被其逼真的图像合成效果所吸引，很多研究人员开始跟进该方向，并在最近的一两年时间里产生了大量的（好几百篇！）改进和拓展工作。

1.总体概览

NeRF的技术其实很简洁，并不复杂。但与2D视觉里考虑的2维图像不同，NeRF考虑的是一个3D空间。下面列的是NeRF实现的几个关键部分：

有一个3D空间，用一个连续的场表示空间里存在一个感兴趣的物体区域处于不同位置和朝向的相机拍摄多视角图像对于一张图像，根据相机中心和图像平面的一个像素点，两点确定一条射线穿过3D空间在射线上采样多个离散的3D点并利用体素渲染像素的颜色。

这里面涉及到3D空间、物体区域、相机位置和朝向、图像、射线、以及3D采样点等。要想优化NeRF，我们需要能够表达刚刚提到的这些东西。

坐标系定义： 为了唯一地描述每一个空间点的坐标，以及相机的位置和朝向，我们需要先定义一个世界坐标系。一个坐标系其实就是由原点的位置与XYZ轴的方向决定。接着，为了建立3D空间点到相机平面的映射关系以及多个相机之间的相对关系，我们会对每一个相机定义一个局部的相机坐标系。下图为常见的坐标系定义习惯。

常见的相机坐标系定义习惯（右手坐标系）。注意：在OpenCV/COLMAP的相机坐标系里相机朝向+z轴，在LLFF/NeRF的相机坐标系中里相机朝向-z轴。有时我们会按坐标系的xyz朝向描述坐标系，如OpenCV/COLMAP里使用的RDF表述X轴指向right，Y轴指向Down，Z轴指向Foward。

2.相机的内外参数

相机的位置和朝向由相机的外参（extrinsic matrix）决定，投影属性由相机的内参（intrinsic matrix）决定。

注意：接下来的介绍假设矩阵是列矩阵(column-major matrix)，变换矩阵左乘坐标向量实现坐标变换（这也是OpenCV/OpenGL/NeRF里使用的形式）。

1.相机外参

相机外参是一个4x4的矩阵M，其作用是将世界坐标系的点 P_world=[x,y,z,1] 变换到相机坐标系P_camera=M P_world 下。我们也把相机外参叫做world-to-camera (w2c)矩阵。(注意用的是4维的齐次坐标）

相机外参的逆矩阵被称为camera-to-world (c2w)矩阵，其作用是把相机坐标系的点变换到世界坐标系。因为NeRF主要使用c2w，这里详细介绍一下c2w的含义。c2w矩阵是一个4x4的矩阵，左上角3x3是旋转矩阵R，又上角的3x1向量是平移向量T。有时写的时候可以忽略最后一行[0,0,0,1]。

Camera-to-world (c2w) 矩阵

具体的，旋转矩阵的第一列到第三列分别表示了相机坐标系的X, Y, Z轴在世界坐标系下对应的方向；平移向量表示的是相机原点在世界坐标系的对应位置。

如果这段描述还是有点抽象，可以尝试进行下列计算帮助自己理解。刚刚讲到c2w是将相机坐标系的向量变换到世界坐标系下，那我们如果将c2w作用到(即左乘)相机坐标系下的X轴[1,0,0,0]，Y轴[0,1,0,0], Z轴[0,0,1,0]，以及原点[0,0,0,1]（注意方向向量的齐次坐标第四维等于0，点坐标第四维等于1），我们会得到它们在世界坐标系的坐标表示：

[R, T][1, 0, 0, 0]^T = [r11, r21, r31]^T # X轴对应的是c2w矩阵的第一列
[R, T][0, 1, 0, 0]^T = [r12, r22, r32]^T # Y轴对应的是c2w矩阵的第二列
[R, T][0, 0, 1, 0]^T = [r13, r23, r33]^T # Y轴对应的是c2w矩阵的第三列
[R, T][0, 0, 0, 1]^T = [t1, t2, t3]^T    # 原点对应的是c2w矩阵的第四列

从上面可以看到可以看到，将c2w作用到相机坐标系下的X轴、Y轴、 Z轴、以及原点我们会依次得到c2w的四列向量。

2.相机内参

相机的内参矩阵将相机坐标系下的3D坐标映射到2D的图像平面，这里以针孔相机(Pinhole camera)为例介绍相机的内参矩阵K：

内参矩阵K包含4个值，其中fx和fy是相机的水平和垂直焦距（对于理想的针孔相机，fx=fy）。焦距的物理含义是相机中心到成像平面的距离，长度以像素为单位。cx和cy是图像原点相对于相机光心的水平和垂直偏移量。cx，cy有时候可以用图像宽和高的1/2近似:

#  NeRF run_nerf.py有这么一段构造K的代码
    if K is None:
        K = np.array([
            [focal, 0, 0.5*W],
            [0, focal, 0.5*H],
            [0, 0, 1]
        ])

3.如何获得相机参数（colmap可估计img位姿）

NeRF算法假设相机的内外参数是提供的，那么怎么得到所需要的相机参数呢？这里分合成数据集和真实数据集两种情况。

合成数据

对于合成数据集，我们需要通过指定相机参数来渲染图像，所以得到图像的时候已经知道对应的相机参数，比如像NeRF用到的Blender Lego数据集。常用的渲染软件还有Mitsuba、OpenGL、PyTorch3D、Pyrender等。渲染数据比较简单，但是把得到的相机数据转到NeRF代码坐标系牵扯到坐标系之间的变换，有时候会比较麻烦。

真实数据

对于真实场景，比如我们用手机拍摄了一组图像，怎么获得相机位姿？目前常用的方法是利用运动恢复结构(structure-from-motion, SFM)技术估计几个相机间的相对位姿。这个技术比较熟了，现在学术界里用的比较多的开源软件包是COLMAP： https://colmap.github.io/。输入多张图像，COLMAP可以估计出相机的内参和外参(也就是sparse model)。

安装colmap：

apt-get update
apt-get install colmap

下面是COLMAP官网教程给的三个命令行操作步骤，简单来说： 第一步是对所有的图像进行特征点检测与提取，第二步是进行特征点匹配，第三步是进行SFM恢复相机位姿和稀疏的3D特征点。具体的使用方法和原理还请阅读其官方文档。其实COLMAP也集成了multiview stereo (MVS)算法用于重建场景完整的三维结构(也称为dense model)。不过NeRF本身是一种新颖的场景表征和重建算法，我们只需要相机的位姿信息，所以我们不需要跑MVS进行dense重建。注意：如果没有标定信息，基于单目的SFM无法获得场景的绝对尺度。

# The project folder must contain a folder "images" with all the images.
$ DATASET_PATH=/path/to/dataset

$ colmap feature_extractor \
   --database_path $DATASET_PATH/database.db \
   --image_path $DATASET_PATH/images

$ colmap exhaustive_matcher \
   --database_path $DATASET_PATH/database.db

$ mkdir $DATASET_PATH/sparse

$ colmap mapper \
    --database_path $DATASET_PATH/database.db \
    --image_path $DATASET_PATH/images \
    --output_path $DATASET_PATH/sparse

使用COLMAP得到相机参数后只需要转成NeRF可以读取的格式即可以用于模型训练了。那这里面需要做什么操作？

LLFF真实数据格式

NeRF代码里用load_llff.py这个文件来读取真实的数据，第一次看到LLFF这个词可能不知道是什么意思。其实LLFF GitHub - Fyusion/LLFF: Code release for Local Light Field Fusion at SIGGRAPH 2019 是NeRF作者的上一篇做新视角合成的工作。为了和LLFF方法保持一致的数据格式，NeRF使用load_llff.py读取LLFF格式的真实数据，并建议大家使用LLFF提供的的imgs2poses.py文件获取所需相机参数。

COLMAP到LLFF数据格式

imgs2poses.py这个文件其实很简单，就干了两件事。

第一件事是调用colmap软件估计相机的参数，在sparse/0/文件夹下生成一些二进制文件：cameras.bin, images.bin, points3D.bin, project.ini。第二件事是读取上一步得到的二进制文件，保存成一个poses_bounds.npy文件。

这里有一个细节需要注意，就是在pose_utils.py文件里load_colmap_data()函数的倒数第二行，有一个操作将colmap得到的c2w旋转矩阵中的第一列和第二列互换，第三列乘以负号：

# LLFF/llff/poses/pose_utils.py
def load_colmap_data(realdir):
    ...   
    # must switch to [-u, r, -t] from [r, -u, t], NOT [r, u, -t]
    poses = np.concatenate([poses[:, 1:2, :], poses[:, 0:1, :], -poses[:, 2:3, :], poses[:, 3:4, :], poses[:, 4:5, :]], 1)
    return poses, pts3d, perm

还记得刚刚提到c2w旋转矩阵的三列向量分别代表XYZ轴的朝向，上述操作实际上就是把相机坐标系轴的朝向进行了变换：X和Y轴调换，Z轴取反，如下图所示：

poses_bounds.npy里有什么load_llff.py会直接读取poses_bounds.npy文件获得相机参数。poses_bounds.npy是一个Nx17的矩阵，其中N是图像的数量，即每一张图像有17个参数。其中前面15个参数可以重排成3x5的矩阵形式：

poses_bounds.npy的前15维参数。左边3x3矩阵是c2w的旋转矩阵，第四列是c2w的平移向量，第五列分别是图像的高H、宽W和相机的焦距f

后两个参数用于表示场景的范围Bounds (bds)，是该相机视角下场景点离相机中心最近(near)和最远(far)的距离，所以near/far肯定是大于0的。

这两个值是怎么得到的？是在imgs2poses.py中，计算colmap重建的3D稀疏点在各个相机视角下最近和最远的距离得到的。

这两个值有什么用？之前提到体素渲染需要在一条射线上采样3D点，这就需要一个采样区间，而near和far就是定义了采样区间的最近点和最远点。贴近场景边界的near/far可以使采样点分布更加密集，从而有效地提升收敛速度和渲染质量。

poses_bounds.npy里最后两个参数（near/far）的作用示意图

3.5 colmap使用技巧：

1. 航拍（像机焦距较大）时，像机参数如下：

“fl_x”: 7516.557593687107, # 焦距
“fl_y”: 7537.338324771301,
“cx”: 511.56096564540155,
“cy”: 268.4251482048674,
“w”: 1024, # 图像宽高
“h”: 540,
“camera_model”: “OPENCV”,
“k1”: -0.014968006153574012,
“k2”: -0.0005643351843003853,
“p1”: -0.0009081580152328081,
“p2”: 0.0001238689035023915,

wh: 图像宽高，fl_x ： 焦距 ；k1、p1是像机的径向畸变和切向畸变的参数。在使用如此长的焦距时，姿态优化可能不稳定。可以尝试禁用它：

–pipeline.datamanager.camera-optimizer.mode off

增加“near”平面的值（可能需要更大的平面），以及增加远平面，–pipeline.model.far-plane

–pipeline.model.near-plane 默认值为0.05
–pipeline.model.far-plane

–pipeline.datamanager.camera-optimizer.mode off nerfstudio-data
–center-method focus
–auto-scale-poses False
–scene-scale 0.1

relaunch training using all images, add --train-split-fraction 1.0

1e-2 threshold， sequential(更加依赖前后帧)

4.缩放图像需要修改什么相机参数？

在_load_data()函数里，有一个用于图像缩放的factor比例参数，将HxW的图像缩放成(H/factor)x(W/factor)。这里面有一个问题是如果缩放了图像尺寸，相机的参数需要相应的做什么变化？

做法是：外参（位置和朝向）不变，相机的焦距f，cx, 和cy等比例缩放。下图的示意图展示了当相机位置不变，相机视野(Field of view, FOV)不变的情况下，图像的高和焦距长短的关系。

viewmatrix()

view_matrix是一个构造相机矩阵的的函数，输入是相机的Z轴朝向、up轴的朝向(即相机平面朝上的方向Y)、以及相机中心。输出下图所示的camera-to-world (c2w)矩阵。因为Z轴朝向，Y轴朝向，和相机中心都已经给定，所以只需求X轴的方向即可。又由于X轴同时和Z轴和Y轴垂直，我们可以用Y轴与Z轴的叉乘得到X轴方向。

下面是load_llff.py里关于view_matrix()的定义，看起来复杂一些。其实就是比刚刚的描述比多了一步：在用Y轴与Z轴叉乘得到X轴后，再次用Z轴与X轴叉乘得到新的Y轴。为什么这么做呢？这是因为传入的up(Y)轴是通过一些计算得到的，不一定和Z轴垂直，所以多这么一步。

# load_llff.py
def viewmatrix(z, up, pos):
    vec2 = normalize(z)
    vec1_avg = up
    vec0 = normalize(np.cross(vec1_avg, vec2))
    vec1 = normalize(np.cross(vec2, vec0))
    m = np.stack([vec0, vec1, vec2, pos], 1)
    return m

poses_avg()

这个函数其实很简单，顾名思义就是多个相机的平均位姿（包括位置和朝向）。输入是多个相机的位姿。

1.第一步对多个相机的中心进行求均值得到center。
2.第二步对所有相机的Z轴求平均得到vec2向量（方向向量相加其实等效于平均方向向量）。
3.第三步对所有的相机的Y轴求平均得到up向量。
4.最后将vec2, up, 和center输入到刚刚介绍的viewmatrix()函数就可以得到平均的相机位姿了。

def poses_avg(poses):

    hwf = poses[0, :3, -1:]

    center = poses[:, :3, 3].mean(0)
    vec2 = normalize(poses[:, :3, 2].sum(0))
    up = poses[:, :3, 1].sum(0)
    c2w = np.concatenate([viewmatrix(vec2, up, center), hwf], 1)
    
    return c2w

下图展示了一个poses_avg()函数的例子。左边是多个输入相机的位姿，右边是返回的平均相机姿态。可以看出平均相机位姿的位置和朝向是之前所有相机的均值。

(中间大的坐标系是世界坐标系，每一个小的坐标系对应一个相机的局部坐标系)

recenter_poses()

recenter_poses()函数的名字听起来是中心化相机位姿（同样包括位置和朝向）的意思。输入N个相机位姿，会返回N个相机位姿。

具体的操作了解起来可能有点跳跃。第一步先用刚刚介绍的poses_avg(poses)得到多个输入相机的平均位姿c2w，接着用这个平均位姿c2w的逆左乘到输入的相机位姿上就完成了归一化。

def recenter_poses(poses):

    poses_ = poses+0
    bottom = np.reshape([0,0,0,1.], [1,4])
    c2w = poses_avg(poses)
    c2w = np.concatenate([c2w[:3,:4], bottom], -2)
    bottom = np.tile(np.reshape(bottom, [1,1,4]), [poses.shape[0],1,1])
    poses = np.concatenate([poses[:,:3,:4], bottom], -2)

    poses = np.linalg.inv(c2w) @ poses
    poses_[:,:3,:4] = poses[:,:3,:4]
    poses = poses_
    return poses

首先我们要知道利用同一个旋转平移变换矩阵左乘所有的相机位姿是对所有的相机位姿做一个全局的旋转平移变换，那下一个问题就是这些相机会被变到什么样的一个位置？我们可以用平均相机位姿作为支点理解，如果把平均位姿的逆c2w^-1左乘平均相机位姿c2w，返回的相机位姿中旋转矩阵为单位矩阵，平移量为零向量。也就是变换后的平均相机位姿的位置处在世界坐标系的原点，XYZ轴朝向和世界坐标系的向一致。

下图我们用一个例子帮助理解。左边和右边分别是输入和输出的相机位姿示意图。我们可以看到变换后的多个相机的平均位姿处在世界坐标系的原点，并且相机坐标系的XYZ轴与世界坐标系保持一致了。

render_path_spiral()

这个函数写的有点复杂，它和模型训练没有关系，主要是用来生成一个相机轨迹用于新视角的合成.下面只放了render_path_spiral()函数的定义，NeRF代码里还有一段是在准备输入参数，由于相关代码比较长就不贴出来。

def render_path_spiral(c2w, up, rads, focal, zdelta, zrate, rots, N):
    render_poses = []
    rads = np.array(list(rads) + [1.])
    hwf = c2w[:,4:5]
    
    for theta in np.linspace(0., 2. * np.pi * rots, N+1)[:-1]:
        c = np.dot(c2w[:3,:4], np.array([np.cos(theta), -np.sin(theta), -np.sin(theta*zrate), 1.]) * rads) 
        z = normalize(c - np.dot(c2w[:3,:4], np.array([0,0,-focal, 1.])))
        render_poses.append(np.concatenate([viewmatrix(z, up, c), hwf], 1))
    return render_poses

需要知道这个函数它是想生成一段螺旋式的相机轨迹，相机绕着一个轴旋转，其中相机始终注视着一个焦点，相机的up轴保持不变。简单说一下上面的代码：

首先是一个for循环，每一迭代生成一个新的相机位置。c是当前迭代的相机在世界坐标系的位置，np.dot(c2w[:3,:4], np.array([0,0,-focal, 1.])是焦点在世界坐标系的位置，z是相机z轴在世界坐标系的朝向。接着使用介绍的viewmatrix(z, up, c)构造当前相机的矩阵。

下面这个图可视化了 render_path_spiral()生成的轨迹。

(中间大的坐标系是世界坐标系，每一个小的坐标系对应一个相机的局部坐标系)

这个函数也比较复杂，前半部分是在将输入的相机参数进行归一化，后半部分是生成一段相机轨迹用于合成新视角。对输入相机参数进行归一化时，思路是：

用 pt_mindist = min_line_dist(rays_o, rays_d)找到离所有相机中心射线距离之和最短的点（可以先简单理解成场景的中心位置）

rays_d = poses[:,:3,2:3]
    rays_o = poses[:,:3,3:4]

    def min_line_dist(rays_o, rays_d):
        A_i = np.eye(3) - rays_d * np.transpose(rays_d, [0,2,1])
        b_i = -A_i @ rays_o
        pt_mindist = np.squeeze(-np.linalg.inv((np.transpose(A_i, [0,2,1]) @ A_i).mean(0)) @ (b_i).mean(0))
        return pt_mindist

    pt_mindist = min_line_dist(rays_o, rays_d)

将得到的场景中心位置移到世界坐标系的原点，同时将所有相机z轴的平均方向转到和世界坐标系的z轴相同

center = pt_mindist
    up = (poses[:,:3,3] - center).mean(0)

    vec0 = normalize(up)
    vec1 = normalize(np.cross([.1,.2,.3], vec0))
    vec2 = normalize(np.cross(vec0, vec1))
    pos = center
    c2w = np.stack([vec1, vec2, vec0, pos], 1)

    poses_reset = np.linalg.inv(p34_to_44(c2w[None])) @ p34_to_44(poses[:,:3,:4])

最后将相机的位置缩放到单位圆内

rad = np.sqrt(np.mean(np.sum(np.square(poses_reset[:,:3,3]), -1)))
sc = 1./rad
poses_reset[:,:3,3] *= sc

下面这个图可视化了spherify_poses()返回的结果。

5.3D空间射线怎么构造

最后我们看一下这个射线是怎么构造的。给定一张图像的一个像素点，我们的目标是构造以相机中心为起始点，经过相机中心和像素点的射线。

首先，明确两件事：

一条射线包括一个起始点和一个方向，起点的话就是相机中心。对于射线方向，我们都知道两点确定一条直线，所以除了相机中心我们还需另一个点，而这个点就是成像平面的像素点。

NeRF代码是在相机坐标系下构建射线，然后再通过camera-to-world (c2w)矩阵将射线变换到世界坐标系。

通过上述的讨论，我们第一步是要先写出相机中心和像素点在相机坐标系的3D坐标。下面我们以OpenCV/Colmap的相机坐标系为例介绍。相机中心的坐标很明显就是[0,0,0]了。像素点的坐标可能复杂一点：首先3D像素点的x和y坐标是2D的图像坐标 (i, j)减去光心坐标 (cx,cy)，然后z坐标其实就是焦距f (因为图像平面距离相机中心的距离就是焦距f)。

所以我们就可以得到射线的方向向量是(i-c_x,j-c_y,f) - (0,0,0)。因为是向量，我们可以把整个向量除以焦距f归一化z坐标

接着只需要用c2w矩阵把相机坐标系下的相机中心和射线方向变换到世界坐标系就搞定了。

(OpenCV/Colmap相机坐标系下射线的构造示意图)

下面是NeRF的实现代码

def get_rays_np(H, W, K, c2w):
    i, j = np.meshgrid(np.arange(W, dtype=np.float32), np.arange(H, dtype=np.float32), indexing='xy')
    dirs = np.stack([(i-K[0][2])/K[0][0], -(j-K[1][2])/K[1][1], -np.ones_like(i)], -1)
    # Rotate ray directions from camera frame to the world frame
    rays_d = np.sum(dirs[..., np.newaxis, :] * c2w[:3,:3], -1)  # dot product, equals to: [c2w.dot(dir) for dir in dirs]
    # Translate camera frame's origin to the world frame. It is the origin of all rays.
    rays_o = np.broadcast_to(c2w[:3,-1], np.shape(rays_d))
    return rays_o, rays_d

这是因为OpenCV/Colmap的相机坐标系里相机的Up/Y朝下, 相机光心朝向+Z轴，而NeRF/OpenGL相机坐标系里相机的Up/朝上，相机光心朝向-Z轴，所以这里代码在方向向量dir的第二和第三项乘了个负号。

一、KITTI数据集介绍（重点是lidar-图像坐标系转换）

1.数据格式

数据包含4个部分，即激光雷达数据velodyne、图像数据image_2、校准数据calib和标注数据label_2。

1.激光雷达数据（data_object_velodyne）

velodyne文件夹下存储了点云文件，以bin格式存储。velodyne文件是激光雷达的测量数据（绕其垂直轴（逆时针）连续旋转），激光雷达参数如下：

1 × Velodyne HDL-64E rotating 3D laser scanner, 
10 Hz, 64 beams, 0.09 degree angular resolution, 
2 cm distance accuracy, collecting ∼ 1.3 million points/second,
field of view: 360◦ horizontal, 26.8◦ vertical, range: 120 m

以“000000.bin”文件为例，点云数据以浮点二进制文件格式存储，每行包含8个数据，每个数据由四位十六进制数表示（浮点数），每个数据通过空格隔开。一个点云数据由四个浮点数数据构成，分别表示点云的x、y、z、r（强度 or 反射值）。

KITTI激光雷达文件夹下的训练点云数量有7481个，即7481个bin文件，共13.2GB大小。测试点云数量有7518个，即7518个bin文件，共13.4GB大小。

可视化

利用python中的open3d实现 点云数据可视化（.bin文件）

import numpy as np
import struct
import open3d

def read_bin_velodyne(path):
    '''read bin file and transfer to array data'''
    pc_list=[]
    with open(path,'rb') as f:
        content=f.read()
        pc_iter=struct.iter_unpack('ffff',content)
        for idx,point in enumerate(pc_iter):
            pc_list.append([point[0],point[1],point[2]])
    return np.asarray(pc_list,dtype=np.float32)

def main():
    pc_path='/KITTI_DATASET_ROOT/testing/velodyne/000045.bin'
    example = np.fromfile(pc_path, dtype=np.float32, count=-1).reshape(-1, 4)
    example_xyz=example[:,:3]
    example_xyz=example_xyz[example_xyz[:,2]>-3]

    # From numpy to Open3D
    pcd = open3d.open3d.geometry.PointCloud()
    pcd.points= open3d.open3d.utility.Vector3dVector(example_xyz)
    vis_ = open3d.visualization.Visualizer()
    vis_.create_window()
    vis_.add_geometry(pcd)
    render_options = vis_.get_render_option()
    render_options.point_size = 1
    render_options.background_color = np.array([0, 0, 0])
    vis_.run()
    vis_.destroy_window()


if __name__=="__main__":
    main()

2.标注数据label_2.

标签存储在data_object_label_2文件夹中，存储为txt文本文件，即data_object_label_2/training/label_2/xxxxxx.txt。标签中仅不含了7481个训练场景的标注数据，而没有测试场景的标注数据。

标注文件中16个属性，即16列。但我们只能够看到前15列数据，因为第16列是针对测试场景下目标的置信度得分，也可以认为训练场景中得分全部为1但是没有专门标注出来。下图是000001.txt的标注内容和对应属性介绍。

第1列

目标类比别（type），共有8种类别，分别是Car、Van、Truck、Pedestrian、Person_sitting、Cyclist、Tram、Misc或’DontCare。DontCare表示某些区域是有目标的，但是由于一些原因没有做标注，比如距离激光雷达过远。但实际算法可能会检测到该目标，但没有标注，这样会被当作false positive （FP）。这是不合理的。用DontCare标注后，评估时将会自动忽略这个区域的预测结果，相当于没有检测到目标，这样就不会增加FP的数量了。此外，在 2D 与 3D Detection Benchmark 中只针对 Car、Pedestrain、Cyclist 这三类。

第2列
截断程度（truncated），表示处于边缘目标的截断程度，取值范围为0~1，0表示没有截断，取值越大表示截断程度越大。处于边缘的目标可能只有部分出现在视野当中，这种情况被称为截断。

第3列
遮挡程度（occlude），取值为（0，1，2，3）。0表示完全可见，1表示小部分遮挡，2表示大部分遮挡，3表示未知（遮挡过大）。

第4列
观测角度（alpha）,取值范围为（-pi, pi）。是在相机坐标系下，以相机原点为中心，相机原点到物体中心的连线为半径，将物体绕相机y轴旋转至相机z轴，此时物体方向与相机x轴的夹角。这相当于将物体中心旋转到正前方后，计算其与车身方向的夹角。

第5-8列
二维检测框（bbox），目标二维矩形框坐标，分别对应left、top、right、bottom，即左上（xy）和右下的坐标（xy）。

第9-11列
三维物体的尺寸（dimensions），分别对应高度、宽度、长度，以米为单位。

第12-14列
中心坐标（location），三维物体中心在相机坐标系下的位置坐标（x，y，z），单位为米。

第15列
旋转角（rotation_y），取值范围为（-pi, pi）。表示车体朝向，绕相机坐标系y轴的弧度值，即物体前进方向与相机坐标系x轴的夹角。rolation_y与alpha的关系为alpha=rotation_y - theta，theta为物体中心与车体前进方向上的夹角。alpha的效果是从正前方看目标行驶方向与车身方向的夹角，如果物体不在正前方，那么旋转物体或者坐标系使得能从正前方看到目标，旋转的角度为theta。

第16列
置信度分数（score），仅在测试评估的时候才需要用到。置信度越高，表示目标越存在的概率越大。

3.图像数据image_2

KITTI数据集种共包含了4相机数据，2个灰度相机和2个彩色相机，其中image_2存储了左侧彩色相机采集的RGB图像数据（RGB）。

存储方式为png格式。KITTI相机的分辨率是1392x512，而image_2种存储的图像是矫正后的图像，分辨率为1242x375。训练集共7481张图片；测试集共7518张图片。

4.标定校准数据calib

标定校准文件主要作用是把 激光雷达坐标系 测得的点云坐标 转换到相机坐标中 去，相关参数存在data object calib中，共包含7481个训练标定文件和7518个测试标定文件。标定文件的存储方式为txt文本文件

以训练文件中的000000.txt标定校准文件为例，其内容如下图所示:

其中，0、1、2、3分别代表左边灰度相机、右边灰度相机、左边彩色相机和右边彩色相机。

1. 内参矩阵

P0-P3分别表示4个相机的内参矩阵，或投影矩阵, 大小为 3x4。相机内参矩阵是为了计算点云空间位置坐标在相机坐标系下的坐标，即把点云坐标投影到相机坐标系。将相机的内参矩阵乘以点云在世界坐标系中的坐标即可得到点云在相机坐标系中的坐标。

如果需要进一步将点云在相机坐标系下的坐标投影到像平面，还需要除以Z值，以及内参矩阵的推导请参考：。

2. 外参矩阵

根据上述介绍，我们知道存在三种坐标系世界坐标系、相机坐标系、激光雷达坐标系。世界坐标系反映了物体的真实位置坐标，也是作为相机坐标系和激光雷达坐标系之间相互变换的过渡坐标系。

点云位置坐标投影到相机坐标系前，需要转换到世界坐标系下，对应的矩阵为外参矩阵。外参矩阵为Tr_velo_to_cam ，大小为3x4，包含了旋转矩阵 R 和 平移向量 T。将相机的外参矩阵乘以点云坐标即可得到点云在世界坐标系中的坐标。

3.R0校准矩阵
R0_rect 为0号相机的修正矩阵，大小为3x3，目的是为了使4个相机成像达到共面的效果，保证4个相机光心在同一个xoy平面上。在进行外参矩阵变化之后，需要于R0_rect相乘得到相机坐标系下的坐标。

4.点云坐标到相机坐标
综上所述，点云坐标在相机坐标系中的坐标等于

内参矩阵 * 外参矩阵 * R0校准矩阵 * 点云坐标

P * R0_rect *Tr_velo_to_cam * x

例如，要将Velodyne激光雷达坐标系中的点x投影到左侧的彩色图像中y，使用公式：

y = P2 * R0_rect *Tr_velo_to_cam * x

当计算出z<0的时候表明该点在相机的后面 。

按照上述过程得到的结果是点云在相机坐标系中的坐标，如果需要将点云坐标投影到像平面还需要除以Z。参考2.1节。示例程序可以参考 ：。

2.KITTI数据集中的三个坐标系：

1. 激光雷达坐标系 (下图中的蓝色坐标系)
2. 相机坐标系 (下图中的红色坐标系)
3. 图像坐标系 (下图：相机采集的图像)

3.点云数据转鸟瞰图（BEV）

import numpy as np
from PIL import Image
import matplotlib.pyplot as plt
 
# -------------------------------1.点云读取----------------------------
pc_path= '/home/xzz/Downloads/mini_kitti/mini kitti/data object veloyne/training/000004.bin'
pointcloud = np.fromfile(pc_path, dtype=np.float32, count=-1).reshape([-1, 4])           # (115976, 4)


# -----------------------------2.设置鸟瞰图范围---------------------------
side_range = (-40, 40)  # 左右距离
fwd_range = (0, 70.4)  # 后前距离
 
x_points = pointcloud[:, 0]            # (115976)
y_points = pointcloud[:, 1]            # (115976)
z_points = pointcloud[:, 2]            # (115976)
 

# ------------------------------3.获得区域内的点----------------------------
f_filt = np.logical_and(x_points > fwd_range[0], x_points < fwd_range[1])      # (115976): [True, False, True...]
s_filt = np.logical_and(y_points > side_range[0], y_points < side_range[1])
filter = np.logical_and(f_filt, s_filt)                                        # (115976): [True, False, True...]
indices = np.argwhere(filter).flatten()                                        # (59732) : [0, 2, ...115976, 115975]
x_points = x_points[indices]
y_points = y_points[indices]
z_points = z_points[indices]
 
# ----------------------------4. 把坐标单位从米，调整到厘米-------------------------
res = 0.1  # 分辨率0.1m
x_img = (-y_points / res).astype(np.int32)
y_img = (-x_points / res).astype(np.int32)


# -----------------------------5.调整坐标原点(到（0，0）)----------------------------
x_img -= int(np.floor(side_range[0]) / res)
y_img += int(np.floor(fwd_range[1]) / res)
print(x_img.min(), x_img.max(), y_img.min(), x_img.max())
 

# -----------------------------6.填充像素值（用z值）-------------------------
height_range = (-2, 0.5)                                   # z值范围选取
pixel_value = np.clip(a=z_points, a_max=height_range[1], a_min=height_range[0])
 
 
def scale_to_255(a, min, max, dtype=np.uint8):
	return ((a - min) / float(max - min) * 255).astype(dtype)
 
 
pixel_value = scale_to_255(pixel_value, height_range[0], height_range[1])   # z值映射到到(0，255)
 
# -----------------------------7.创建图像数组-------------------------------------
x_max = 1 + int((side_range[1] - side_range[0]) / res)
y_max = 1 + int((fwd_range[1] - fwd_range[0]) / res)
im = np.zeros([y_max, x_max], dtype=np.uint8)
im[y_img, x_img] = pixel_value
 


# -------------------------8.鸟瞰图可视化（灰度/彩色）------------------------
# im2 = Image.fromarray(im)
# im2.show()
 
# imshow （彩色）
plt.imshow(im, cmap="nipy_spectral", vmin=0, vmax=255)
plt.show()

4.将标注画在鸟瞰图（BEV）上

先分析了主函数 test。实际跑代码，可以把主函数放最后

from __future__ import division
import os
import numpy as np
import cv2
import math

def test():
    lidar_file = '/home/xzz/Downloads/mini_kitti/mini kitti/data object veloyne/training/000004.bin'    # 点云
    calib_file = '/home/xzz/Downloads/mini_kitti/mini kitti/data object calib/training/000004.txt'      # 修正文件
    label_file = '/home/xzz/Downloads/mini_kitti/mini kitti/data_object_label_2/training/000004.txt'    # GroundTruth
 
    # 1.加载雷达数据------------------------------------------------------------------------------------------------
    print("Processing: ", lidar_file)
    lidar = np.fromfile(lidar_file, dtype=np.float32)
    lidar = lidar.reshape((-1, 4))                      # (115976, 4)   点云
 
    # 2.加载修正文件------------------------------------------------------------------------------------------------
    calib = load_kitti_calib(calib_file)                # return: P2:(3, 4), R0:(3, 3), Tr_velo2cam:(3, 4)
    
    # 3.标注转三维目标检测框----------------------------------------------------------------------------------------
    # 读入GT[8:]( 即h,w,l,x,y,z,r )和外参矩阵，将中心点修正，3D边框绕Z轴旋转，相加得到8个顶点坐标
    # 点云与外参矩阵相乘，得到世界坐标系中坐标（此时xy值，就是俯视图中的坐标了。对应的z值就是像素值）
    gt_box3d = load_kitti_label(label_file, calib['Tr_velo2cam'])    # (n, 8, 3)
 
    
    # 4.手动筛选（过滤指定范围之外的点和目标框）---------------------------------------------------------------------
    lidar, gt_box3d = get_filtered_lidar(lidar, gt_box3d)          # (58590, 4)  (2, 8, 3)
 
    # 5.得到素中最高点的高度、强度，体素中点的密度（即点的数量）------------------------------------------------------
    hight_image, height_r_image, density_image = lidar_to_bev(lidar)    
    
    # 6.可视化 （只取xy坐标，八个顶点只取上面4个或下面4个）----------------------------------------------------------
    hight_with_box = draw_polygons(hight_image,gt_box3d[:,:4,:2])
    height_r_with_box = draw_polygons(height_r_image,gt_box3d[:,:4,:2])
    density_with_box = draw_polygons(density_image,gt_box3d[:,:4,:2])
    cv2.imshow('hight', hight_with_box)
    cv2.imshow('height_r', height_r_with_box)
    cv2.imshow('density', density_with_box)
    cv2.imwrite('hight.png', hight_with_box)
    cv2.imwrite('height_r.png', height_r_with_box)
    cv2.imwrite('density.png', density_with_box)
    cv2.waitKey(0)



 
 # voxel size
vd = 0.4
vh = 0.2
vw = 0.2
 
# points cloud range
xrange = (0, 70.4)
yrange = (-40, 40)
zrange = (-3, 1)
 
# voxel grid
W = math.ceil((xrange[1] - xrange[0]) / vw)    # 352
H = math.ceil((yrange[1] - yrange[0]) / vh)    # 400
D = math.ceil((zrange[1] - zrange[0]) / vd)    # 10
 
def _quantize_coords(x, y):
    xx = H - int((y - yrange[0]) / vh)
    yy = W - int((x - xrange[0]) / vw)
    return xx, yy
 
#过滤指定范围之外的点和目标框
def get_filtered_lidar(lidar, boxes3d=None):
    # lidar:(115976, 4) boxes3d: (2, 8, 3)
    xrange = (0, 70.4)
    yrange = (-40, 40)
    zrange = (-3, 1)
    pxs = lidar[:, 0]
    pys = lidar[:, 1]
    pzs = lidar[:, 2]
    filter_x = np.where((pxs >= xrange[0]) & (pxs < xrange[1]))[0]      # (59820) : [0, 2, ...105689]
    filter_y = np.where((pys >= yrange[0]) & (pys < yrange[1]))[0]      # (115833)
    filter_z = np.where((pzs >= zrange[0]) & (pzs < zrange[1]))[0]      # (113897): [ 162, 165,...115976 ]
    filter_xy = np.intersect1d(filter_x, filter_y)                      # (59737)
    filter_xyz = np.intersect1d(filter_xy, filter_z)                    # (58590)
    if boxes3d is not None:
        box_x = (boxes3d[:, :, 0] >= xrange[0]) & (boxes3d[:, :, 0] < xrange[1])        # (2, 8): [True. F,..True]
        box_y = (boxes3d[:, :, 1] >= yrange[0]) & (boxes3d[:, :, 1] < yrange[1])
        box_z = (boxes3d[:, :, 2] >= zrange[0]) & (boxes3d[:, :, 2] < zrange[1])
        box_xyz = np.sum(box_x & box_y & box_z,axis=1)
        return lidar[filter_xyz], boxes3d[box_xyz>0]
    return lidar[filter_xyz]
 
def lidar_to_bev(lidar):
    pxs = lidar[:, 0]             # ( 58590 )
    pys = lidar[:, 1]
    pzs = lidar[:, 2]
    prs = lidar[:, 3]
 
    qxs=((pxs-xrange[0])/vw).astype(np.int32)
    qys=((pys-yrange[0])/vh).astype(np.int32)
    qzs=((pzs-zrange[0])/vd).astype(np.int32)
 
    print('height,width,channel=%d,%d,%d'%(W, H, D))
    top = np.zeros(shape=(W, H, D), dtype=np.float32)
    mask = np.ones(shape=(W, H, D), dtype=np.float32)* -5
    bev = np.zeros(shape=(W, H, 3), dtype=np.float32)
    bev[:, : ,0] = np.ones(shape=(W, H), dtype=np.float32)* -5
 
    for i in range(len(pxs)):
        #统计高度方向上每个体素的个数
        bev[-qxs[i], -qys[i], -1]= 1+ bev[-qxs[i], -qys[i], -1]
        if pzs[i]>mask[-qxs[i], -qys[i],qzs[i]]:
            #记录每个体素中点的最大高度值
            top[-qxs[i], -qys[i], qzs[i]] = max(0,pzs[i]-zrange[0])
            #更新最大高度值
            mask[-qxs[i], -qys[i],qzs[i]]=pzs[i]
        if pzs[i]>bev[-qxs[i], -qys[i], 0]:
            #记录高度方向上的最大高度值
            bev[-qxs[i], -qys[i], 0]=pzs[i]
            #记录高度方向上最高点的强度值
            bev[-qxs[i], -qys[i], 1]=prs[i]
 
    bev[:,:,-1] = np.log(bev[:,:,-1]+1)/math.log(64)              # 数值缩小
 
 
    bev_image = bev - np.min(bev.reshape(-1, 3), 0)
    bev_image_image = (bev_image/np.max(bev_image.reshape(-1, 3), 0)*255).astype(np.uint8)
    return  bev[:, :, 0], bev[:, :, 1], bev[:, :, 2]              # bev[:, :, 0]表示体素中最高点的高度值; 最高点的强度; 体素中点的密度即点的数量。
 
def  draw_polygons(image, polygons,color=(255,255,255), thickness=1, darken=1):
    img = image.copy() * darken        # polygons: (n,4,2) 
    for polygon in polygons:
        tup0, tup1, tup2, tup3 = [_quantize_coords(*tup) for tup in polygon]  # 选择3维点云（8，3）中的（4，2），可直接画在2D体素图上
        cv2.line(img, tup0, tup1, color, thickness, cv2.LINE_AA)
        cv2.line(img, tup1, tup2, color, thickness, cv2.LINE_AA)
        cv2.line(img, tup2, tup3, color, thickness, cv2.LINE_AA)
        cv2.line(img, tup3, tup0, color, thickness, cv2.LINE_AA)
    return img
 
def load_kitti_calib(calib_file):
    """
    load projection matrix
    """
    with open(calib_file) as fi:
        lines = fi.readlines()
        assert (len(lines) == 8)
    obj = lines[0].strip().split(' ')[1:]
    P0 = np.array(obj, dtype=np.float32)
    obj = lines[1].strip().split(' ')[1:]
    P1 = np.array(obj, dtype=np.float32)
    obj = lines[2].strip().split(' ')[1:]
    P2 = np.array(obj, dtype=np.float32)
    obj = lines[3].strip().split(' ')[1:]
    P3 = np.array(obj, dtype=np.float32)
    obj = lines[4].strip().split(' ')[1:]
    R0 = np.array(obj, dtype=np.float32)
    obj = lines[5].strip().split(' ')[1:]
    Tr_velo_to_cam = np.array(obj, dtype=np.float32)
    obj = lines[6].strip().split(' ')[1:]
    Tr_imu_to_velo = np.array(obj, dtype=np.float32)
    return {'P2': P2.reshape(3, 4),
            'R0': R0.reshape(3, 3),
            'Tr_velo2cam': Tr_velo_to_cam.reshape(3, 4)}
 
 
def box3d_cam_to_velo(box3d, Tr):
    # Tr: Tr_velo_to_cam(3, 4)
    def project_cam2velo(cam, Tr):
        T = np.zeros([4, 4], dtype=np.float32)
        T[:3, :] = Tr
        T[3, 3] = 1
        T_inv = np.linalg.inv(T)            # 算矩阵的(乘法)逆
        lidar_loc_ = np.dot(T_inv, cam)
        lidar_loc = lidar_loc_[:3]
        return lidar_loc.reshape(1, 3)
 
    def ry_to_rz(ry):
        angle = -ry - np.pi / 2
        if angle >= np.pi:
            angle -= np.pi
        if angle < -np.pi:
            angle = 2*np.pi + angle
        return angle
 
    h,w,l,tx,ty,tz,ry = [float(i) for i in box3d]
    cam = np.ones([4, 1])
    cam[0] = tx
    cam[1] = ty
    cam[2] = tz
    t_lidar = project_cam2velo(cam, Tr)               # 把目标中心点，转移到像机坐标系  Tr_velo_to_cam -> (1,3)
    Box = np.array([[-l / 2, -l / 2, l / 2, l / 2, -l / 2, -l / 2, l / 2, l / 2],
                    [w / 2, -w / 2, -w / 2, w / 2, w / 2, -w / 2, -w / 2, w / 2],
                    [0, 0, 0, 0, h, h, h, h]])
    rz = ry_to_rz(ry)
    rotMat = np.array([
        [np.cos(rz), -np.sin(rz), 0.0],
        [np.sin(rz), np.cos(rz), 0.0],
        [0.0, 0.0, 1.0]])
    velo_box = np.dot(rotMat, Box)                     # (3, 8)
    cornerPosInVelo = velo_box + np.tile(t_lidar, (8, 1)).T
    box3d_corner = cornerPosInVelo.transpose()
    return box3d_corner.astype(np.float32)             # (8, 3)
 
def load_kitti_label(label_file, Tr):
    # Tr: Tr_velo_to_cam(3, 4)
    with open(label_file,'r') as f:
        lines = f.readlines()
    gt_boxes3d_corner = []
    num_obj = len(lines)
    for j in range(num_obj):
        obj = lines[j].strip().split(' ')
        obj_class = obj[0].strip()
        if obj_class not in ['Car']:   
            continue                                      # 只顯示車輛目標
        box3d_corner = box3d_cam_to_velo(obj[8:], Tr)     # input:(h,w,l,x,y,z,r) (Tr)   out:(8, 3)
        gt_boxes3d_corner.append(box3d_corner)
    gt_boxes3d_corner = np.array(gt_boxes3d_corner).reshape(-1,8,3)
    return gt_boxes3d_corner                              # (2, 8, 3)
 

 
if __name__ == '__main__':
    test()

效果图：

5. 将标注画在点云上

# -*- coding: utf-8 -*-
"
 
from __future__ import division
import os
import numpy as np
import mayavi.mlab as mlab
 
#过滤指定范围之外的点和目标框
def get_filtered_lidar(lidar, boxes3d=None):
    xrange = (0, 70.4)
    yrange = (-40, 40)
    zrange = (-3, 1)
    pxs = lidar[:, 0]
    pys = lidar[:, 1]
    pzs = lidar[:, 2]
    filter_x = np.where((pxs >= xrange[0]) & (pxs < xrange[1]))[0]
    filter_y = np.where((pys >= yrange[0]) & (pys < yrange[1]))[0]
    filter_z = np.where((pzs >= zrange[0]) & (pzs < zrange[1]))[0]
    filter_xy = np.intersect1d(filter_x, filter_y)
    filter_xyz = np.intersect1d(filter_xy, filter_z)
    if boxes3d is not None:
        box_x = (boxes3d[:, :, 0] >= xrange[0]) & (boxes3d[:, :, 0] < xrange[1])
        box_y = (boxes3d[:, :, 1] >= yrange[0]) & (boxes3d[:, :, 1] < yrange[1])
        box_z = (boxes3d[:, :, 2] >= zrange[0]) & (boxes3d[:, :, 2] < zrange[1])
        box_xyz = np.sum(box_x & box_y & box_z,axis=1)
 
        return lidar[filter_xyz], boxes3d[box_xyz>0]
 
    return lidar[filter_xyz]
 
 
def draw_lidar(lidar, is_grid=False, is_axis = True, is_top_region=True, fig=None):
    pxs=lidar[:,0]
    pys=lidar[:,1]
    pzs=lidar[:,2]
    prs=lidar[:,3]
    if fig is None: fig = mlab.figure(figure=None, bgcolor=(0,0,0), fgcolor=None, engine=None, size=(1000, 500))
    mlab.points3d(
        pxs, pys, pzs, prs,
        mode='point',  # 'point'  'sphere'
        colormap='gnuplot',  #'bone',  #'spectral',  #'copper',
        scale_factor=1,
        figure=fig)
    #draw grid
    if is_grid:
        mlab.points3d(0, 0, 0, color=(1,1,1), mode='sphere', scale_factor=0.2)
        for y in np.arange(-50,50,1):
            x1,y1,z1 = -50, y, 0
            x2,y2,z2 =  50, y, 0
            mlab.plot3d([x1, x2], [y1, y2], [z1,z2], color=(0.5,0.5,0.5), tube_radius=None, line_width=1, figure=fig)
        for x in np.arange(-50,50,1):
            x1,y1,z1 = x,-50, 0
            x2,y2,z2 = x, 50, 0
            mlab.plot3d([x1, x2], [y1, y2], [z1,z2], color=(0.5,0.5,0.5), tube_radius=None, line_width=1, figure=fig)
 
    #draw axis
    if is_grid:
        mlab.points3d(0, 0, 0, color=(1,1,1), mode='sphere', scale_factor=0.2)
        axes=np.array([
            [2.,0.,0.,0.],
            [0.,2.,0.,0.],
            [0.,0.,2.,0.],
        ],dtype=np.float64)
        fov=np.array([  ##<todo> : now is 45 deg. use actual setting later ...
            [20., 20., 0.,0.],
            [20.,-20., 0.,0.],
        ],dtype=np.float64)
 
        mlab.plot3d([0, axes[0,0]], [0, axes[0,1]], [0, axes[0,2]], color=(1,0,0), tube_radius=None, figure=fig)
        mlab.plot3d([0, axes[1,0]], [0, axes[1,1]], [0, axes[1,2]], color=(0,1,0), tube_radius=None, figure=fig)
        mlab.plot3d([0, axes[2,0]], [0, axes[2,1]], [0, axes[2,2]], color=(0,0,1), tube_radius=None, figure=fig)
        mlab.plot3d([0, fov[0,0]], [0, fov[0,1]], [0, fov[0,2]], color=(1,1,1), tube_radius=None, line_width=1, figure=fig)
        mlab.plot3d([0, fov[1,0]], [0, fov[1,1]], [0, fov[1,2]], color=(1,1,1), tube_radius=None, line_width=1, figure=fig)
 
    #draw top_image feature area
    if is_top_region:
        #关注指定范围内的点云
        x1 = 0
        x2 = 70.4
        y1 = -40
        y2 = 40
        mlab.plot3d([x1, x1], [y1, y2], [0,0], color=(0.5,0.5,0.5), tube_radius=None, line_width=1, figure=fig)
        mlab.plot3d([x2, x2], [y1, y2], [0,0], color=(0.5,0.5,0.5), tube_radius=None, line_width=1, figure=fig)
        mlab.plot3d([x1, x2], [y1, y1], [0,0], color=(0.5,0.5,0.5), tube_radius=None, line_width=1, figure=fig)
        mlab.plot3d([x1, x2], [y2, y2], [0,0], color=(0.5,0.5,0.5), tube_radius=None, line_width=1, figure=fig)
    mlab.orientation_axes()
    mlab.view(azimuth=180,elevation=None,distance=50,focalpoint=[ 12.0909996 , -1.04700089, -2.03249991])#2.0909996 , -1.04700089, -2.03249991
    return fig
 
def draw_gt_boxes3d(gt_boxes3d, fig, color=(1,0,0), line_width=2):
    num = len(gt_boxes3d)
    for n in range(num):
        b = gt_boxes3d[n]
        for k in range(0,4):
            i,j=k,(k+1)%4
            mlab.plot3d([b[i,0], b[j,0]], [b[i,1], b[j,1]], [b[i,2], b[j,2]], color=color, tube_radius=None, line_width=line_width, figure=fig)
            i,j=k+4,(k+3)%4 + 4
            mlab.plot3d([b[i,0], b[j,0]], [b[i,1], b[j,1]], [b[i,2], b[j,2]], color=color, tube_radius=None, line_width=line_width, figure=fig)
            i,j=k,k+4
            mlab.plot3d([b[i,0], b[j,0]], [b[i,1], b[j,1]], [b[i,2], b[j,2]], color=color, tube_radius=None, line_width=line_width, figure=fig)
    mlab.view(azimuth=180,elevation=None,distance=50,focalpoint=[ 12.0909996 , -1.04700089, -2.03249991])#2.0909996 , -1.04700089, -2.03249991
 
def load_kitti_calib(calib_file):
    """
    load projection matrix
    """
    with open(calib_file) as fi:
        lines = fi.readlines()
        assert (len(lines) == 8)
    obj = lines[0].strip().split(' ')[1:]
    P0 = np.array(obj, dtype=np.float32)
    obj = lines[1].strip().split(' ')[1:]
    P1 = np.array(obj, dtype=np.float32)
    obj = lines[2].strip().split(' ')[1:]
    P2 = np.array(obj, dtype=np.float32)
    obj = lines[3].strip().split(' ')[1:]
    P3 = np.array(obj, dtype=np.float32)
    obj = lines[4].strip().split(' ')[1:]
    R0 = np.array(obj, dtype=np.float32)
    obj = lines[5].strip().split(' ')[1:]
    Tr_velo_to_cam = np.array(obj, dtype=np.float32)
    obj = lines[6].strip().split(' ')[1:]
    Tr_imu_to_velo = np.array(obj, dtype=np.float32)
    return {'P2': P2.reshape(3, 4),
            'R0': R0.reshape(3, 3),
            'Tr_velo2cam': Tr_velo_to_cam.reshape(3, 4)}
 
 
def box3d_cam_to_velo(box3d, Tr):
    def project_cam2velo(cam, Tr):
        T = np.zeros([4, 4], dtype=np.float32)
        T[:3, :] = Tr
        T[3, 3] = 1
        T_inv = np.linalg.inv(T)
        lidar_loc_ = np.dot(T_inv, cam)
        lidar_loc = lidar_loc_[:3]
        return lidar_loc.reshape(1, 3)
 
    def ry_to_rz(ry):
        angle = -ry - np.pi / 2
        if angle >= np.pi:
            angle -= np.pi
        if angle < -np.pi:
            angle = 2*np.pi + angle
        return angle
 
    h,w,l,tx,ty,tz,ry = [float(i) for i in box3d]
    cam = np.ones([4, 1])
    cam[0] = tx
    cam[1] = ty
    cam[2] = tz
    t_lidar = project_cam2velo(cam, Tr)
    Box = np.array([[-l / 2, -l / 2, l / 2, l / 2, -l / 2, -l / 2, l / 2, l / 2],
                    [w / 2, -w / 2, -w / 2, w / 2, w / 2, -w / 2, -w / 2, w / 2],
                    [0, 0, 0, 0, h, h, h, h]])
    rz = ry_to_rz(ry)
    rotMat = np.array([
        [np.cos(rz), -np.sin(rz), 0.0],
        [np.sin(rz), np.cos(rz), 0.0],
        [0.0, 0.0, 1.0]])
    velo_box = np.dot(rotMat, Box)
    cornerPosInVelo = velo_box + np.tile(t_lidar, (8, 1)).T
    box3d_corner = cornerPosInVelo.transpose()
    return box3d_corner.astype(np.float32)
 
def load_kitti_label(label_file, Tr):
    with open(label_file,'r') as f:
        lines = f.readlines()
    gt_boxes3d_corner = []
    num_obj = len(lines)
    for j in range(num_obj):
        obj = lines[j].strip().split(' ')
        obj_class = obj[0].strip()
        if obj_class not in ['Car']:
            continue
        box3d_corner = box3d_cam_to_velo(obj[8:], Tr)
        gt_boxes3d_corner.append(box3d_corner)
    gt_boxes3d_corner = np.array(gt_boxes3d_corner).reshape(-1,8,3)
    return gt_boxes3d_corner
 
def test():
    lidar_file = '/home/xzz/Desktop/mini_kitti/data object veloyne/training/000016.bin' 
    calib_file = '/home/xzz/Desktop/mini_kitti/data object calib/training/calib/000016.txt' 
    label_file = '/home/xzz/Desktop/mini_kitti/data_object_label_2/training/label_2/000016.txt' 
 
    #加载雷达数据
    print("Processing: ", lidar_file)
    lidar = np.fromfile(lidar_file, dtype=np.float32)
    lidar = lidar.reshape((-1, 4))                                         # (113070, 4)
 
    #加载标注文件
    calib = load_kitti_calib(calib_file)
    #标注转三维目标检测框
    gt_box3d = load_kitti_label(label_file, calib['Tr_velo2cam'])          # (4,8,3)
 
    #过滤指定范围之外的点和目标框
    lidar, gt_box3d = get_filtered_lidar(lidar, gt_box3d)                  # (55356, 4) 
 
    # view in point cloud，可视化
    fig = draw_lidar(lidar, is_grid=True, is_top_region=True)
    draw_gt_boxes3d(gt_boxes3d=gt_box3d, fig=fig)
    mlab.show()
 
if __name__ == '__main__':
    test()

二、几种点云可视化方式

常用点云格式如 ply、obj ，可直接安装软件 meshlab 直接可视化，也可实现格式转换。
其它如bin格式可见以下方法：

方案一：mayavi可视化点云

from mayavi import mlab
import numpy as np
 
def viz_mayavi(points):
    x = points[:, 0]  # x position of point
    y = points[:, 1]  # y position of point
    z = points[:, 2]  # z position of point
    fig = mlab.figure(bgcolor=(0, 0, 0), size=(640, 360)) #指定图片背景和尺寸
    mlab.points3d(x, y, z,
                         z,          # Values used for Color，指定颜色变化依据
                         mode="point",
                          colormap='spectral', # 'bone', 'copper', 'gnuplot'
                         # color=(0, 1, 0),   # 也可以使用固定的RGB值
                         )
    mlab.show()
points = np.loadtxt('airplane_0001.txt', delimiter=',')

方案二、matplotlib可视化点云

import matplotlib.pyplot as plt
import numpy as np
 
def viz_matplot(points):
    x = points[:, 0]  # x position of point
    y = points[:, 1]  # y position of point
    z = points[:, 2]  # z position of point
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
    ax.scatter(x,   # x
               y,   # y
               z,   # z
               c=z, # height data for color
               cmap='rainbow',
               marker="x")
    ax.axis()
    plt.show()
 
    
points = np.loadtxt('airplane_0001.txt', delimiter=',')

方案三、CloudCompare可视化点云

Mayavi常用函数

from mayavi import mlab
 
fig = mlab.figure(bgcolor=(0, 0, 0), size=(640, 360)) #指定图片背景和尺寸
 
mlab.points3d(x, y, z,
              z,          # Values used for Color，指定颜色变化依据
              mode="point",
              colormap='spectral', # 'bone', 'copper', 'gnuplot'
              # color=(0, 1, 0),   # 也可以使用固定的RGB值
               )
 
opacity=1.0  # 不透明度，取值范围0-1。0.0表示完全透明，1.0表示完全不透明
color=(1, 1, 1)  # RGB数值，每个数的取值范围均为0-1。例：(1, 1, 1)表示白色。
colormap='spectral'  #  不同的配色方案
 
mlab.show()#显示结果
 
# 2D data
img = xxxx  # img is a 2D nunmpy array
mlab.imshow(img) #显示二维结果
mlab.surf()
mlab.contour_surf()
mlab.mesh()             #将物体表面以网格（mesh）的形式展示出来，即坐标空间的网格化。# 参数:representation = 'wireframe' 可以仅绘制线框。
# 参数:representation = 'surface' 为default值，绘制完整曲面。mlab.mesh(x, y, z, representation='wireframe', line_width=1.0 )

三、点云数据变换

1.pcd点云可视化与移动（利用open3d）

点云的平移函数为translate。其函数原型如下所示：

pcd.translate((tx,ty,tz),relative=True)

1.当relative为True时，（tx, ty, tz）表示点云平移的相对尺度，也就是平移了多少距离。
2.当relative为False时，（tx, ty, tz）表示点云中心（质心）平移到的指定位置。
3.质心可以坐标可以通过 pcd.get_center( ) 得到。

from copy import deepcopy
import  open3d as o3d
 
if __name__ == '__main__':
    file_path = 'rabbit.pcd'
    pcd = o3d.io.read_point_cloud(file_path)
    print(pcd)

    # 点云转为 numpy矩阵
    points = np.array(pcd.points)
    
    # x方向平移
    pcd1 = deepcopy(pcd)
    pcd1.translate((20,0,0), relative=True)
    
    # y方向平移
    pcd2 = deepcopy(pcd)
    pcd2.translate((0,20,0), relative=True)
    
    # z方向平移
    pcd3 = deepcopy(pcd)
    pcd3.translate((0,0,20), relative=True)
    pcd4 = deepcopy(pcd)
    pcd4.translate((20,20,20), relative=True)
    
    # 点云显示
    o3d.visualization.draw_geometries([pcd, pcd1, pcd2, pcd3, pcd4], #点云列表
                                      window_name="点云平移",
                                      point_show_normal=False,
                                      width=800,  # 窗口宽度
                                      height=600)  # 窗口高度

2.pcd点云旋转

pcd.rotate(R, center=(20, 0, 0))

1.第一个参数R是旋转矩阵。open3d中点云的旋转仍然是通过矩阵运算来完成的，因而需要先获取旋转矩阵。旋转矩阵可以自己进行定义，也可以根据欧拉角、旋转向量和四元数计算得到。
2.第二个参数是旋转中心，即围绕哪个点进行旋转。如果不指定center的值，默认为点云质心 (pcd.get_center())。

1.1 根据欧拉角计算旋转矩阵

pcd.get_rotation_matrix_from_xyz(α, β, γ)。
   # 欧拉角旋转与旋转轴的先后顺序有关。除xyz外:xzy、yxz、yzx、等。

   R = pcd.get_rotation_matrix_from_xyz((0, np.pi/2, 0))#绕y轴旋转90°

1.2 根据旋转向量（轴角）计算旋转矩阵

旋转向量用3行1列的列向量(x, y, z).T来表示。那么旋转轴为向量方向，旋转角度为向量模长。

R = pcd.get_rotation_matrix_from_axis_angle(np.array([0, -np.pi/2, 0]).T)
#向量方向为旋转轴，模长等于旋转角度，绕y轴旋转-90°

1.3 根据四元数计算旋转矩阵

四元数用4行1列的列向量(w, x, y, z).T来表示。

R = pcd.get_rotation_matrix_from_quaternion(np.array([0, 0, 0, 1]).T)
# 绕x轴旋转180°

import open3d as o3d
from copy import deepcopy
import numpy as np

if __name__ == '__main__':
    file_path = 'rabbit.pcd'
    pcd = o3d.io.read_point_cloud(file_path)
    pcd.paint_uniform_color([0.5, 0.5, 0.5])#指定显示为灰色
    print(pcd)
    print(pcd.get_center())
 
    pcd1 = deepcopy(pcd)
    #采用欧拉角进行旋转
    R = pcd.get_rotation_matrix_from_xyz((0, np.pi/2, 0))#绕y轴旋转90°
    pcd1.rotate(R, center=(20, 0, 0))#旋转点位于x=20处，若不指定则默认为原始点云质心。
    pcd1.paint_uniform_color([0, 0, 1])#指定显示为蓝色
    print(pcd1.get_center())
    print(R)
 
    #采用旋转向量（轴角）进行旋转
    pcd2 = deepcopy(pcd)
    R = pcd.get_rotation_matrix_from_axis_angle(np.array([0, -np.pi/2, 0]).T)#向量方向为旋转轴，模长等于旋转角度，绕y轴旋转-90°
    pcd2.paint_uniform_color([0, 1, 0])#指定显示为绿色
    pcd2.rotate(R, center=(20, 0, 0))#旋转点位于x=20处，若不指定则默认为原始点云质心。
    print(pcd2.get_center())
    print(R)
   
    #采用四元数进行旋转
    pcd3 = deepcopy(pcd)
    R = pcd.get_rotation_matrix_from_quaternion(np.array([0, 0, 0, 1]).T)#绕x轴旋转180°
    pcd3.paint_uniform_color([1, 0, 0])#指定显示为红色
    pcd3.rotate(R, center=(0, 10, 0))#旋转点位于y=10处，若不指定则默认为原始点云质心。
    print(pcd3.get_center())
    print(R)
    # 点云显示
    o3d.visualization.draw_geometries([pcd, pcd1, pcd2, pcd3], #点云列表
                                      window_name="点云旋转",
                                      point_show_normal=False,
                                      width=800,  # 窗口宽度
                                      height=600)  # 窗口高度

3.点云仿射变换

仿射变换包含了一组线性变换和一个平移变换。其中，线性变换可以用矩阵左乘来表示。因此，仿射变换可以用矩阵和向量的方式来表达。

pen3d中的投影变换为函数为transform，参数为投影变换矩阵T。需要注意的是，open3d中的投影变换不仅仅包括仿射变换，还包括透视投影变换。仿射变换是线性的投影变换，而透视变换是非线性的。因此。open3d中的变换矩阵是4x4大小，而不是3x4。即：

矩阵T前3行对应仿射变换，最后一行对应透视变换。其中，s可以用来控制缩放系数，表示缩小的倍数。

import open3d as o3d
from copy import deepcopy
import numpy as np
 
if __name__ == '__main__':
    file_path = 'rabbit.pcd'
    pcd = o3d.io.read_point_cloud(file_path)
    pcd.paint_uniform_color([0.5, 0.5, 0.5])#指定显示为灰色
    print(pcd)
 
    
    #采用欧拉角进行旋转
    R = pcd.get_rotation_matrix_from_xyz((0, np.pi/2, 0))#绕y轴旋转90°
    #旋转矩阵
    R = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])
   
    # 仿射变换
    T = np.array([[1, 0, 0, 20], [0, 1, 1, 20], [0, 0, 1, 0], [0, 0, 0, 1]])
    pcd1 = deepcopy(pcd)
    pcd1.transform(T)
    pcd1.paint_uniform_color([0, 0, 1])#指定显示为蓝色
 
    # 旋转矩阵R+x方向平移20个单位
    T = np.array([[0, 0, 1, 20], [0, 1, 0, 0], [-1, 0, 0, 0], [0, 0, 0, 1]]) #旋转矩阵R+x方向平移20个单位
    pcd2 = deepcopy(pcd)
    pcd2.transform(T)
    pcd2.paint_uniform_color([0, 1, 0])#指定显示为绿色
 
 
    # y方向平移40个单位，并且缩小3倍
    T = np.array([[1, 0, 0, 0], [0, 1, 0, 40], [0, 0, 1, 0], [0, 0, 0, 3]]) #y方向平移40个单位，并且缩小3倍
    pcd3 = deepcopy(pcd)
    pcd3.transform(T)
    pcd3.paint_uniform_color([1, 0, 0])#指定显示为红色
 
    # 点云显示
    o3d.visualization.draw_geometries([pcd, pcd1, pcd2, pcd3], #点云列表
                                      window_name="投影变换",
                                      point_show_normal=False,
                                      width=800,  # 窗口宽度
                                      height=600)  # 窗口高度

4.点云缩放

import open3d as o3d
from copy import deepcopy
import numpy as np
 
if __name__ == '__main__':
    file_path = 'rabbit.pcd'
    pcd = o3d.io.read_point_cloud(file_path)
    pcd.paint_uniform_color([0.5, 0.5, 0.5])#指定显示为灰色
    print(pcd)
    print('原始点云质心：', pcd.get_center())
    
    # 采用numpy计算
    points = np.array(pcd.points)
    points = points/2.0#缩小到原来的一半
    points[:, 0] = points[:, 0] + 20#质心平移到x=20处
    pcd1 = o3d.geometry.PointCloud()
    pcd1.points = o3d.utility.Vector3dVector(points)
    pcd1.paint_uniform_color([0, 0, 1])#指定显示为蓝色
    print('数组平移后点云质心：', pcd1.get_center())
 
 
    # 采用scale函数
    pcd2 = deepcopy(pcd)
    pcd2.scale(2.0, (40, 0, 0))#点云放大两倍，质心平移至(-40, 0, 0)
    pcd2.paint_uniform_color([0, 1, 0])#指定显示为绿色
    print('scale缩放后点云质心：', pcd2.get_center())
   
    # 采用仿射变换
    T = np.array([[1, 0, 0, 0], [0, 1, 0, 80], [0, 0, 1, 0], [0, 0, 0, 3]])#点云缩小到1/3，质心平移到(0, 80, 0)
    pcd3 = deepcopy(pcd)
    pcd3.transform(T)
    pcd3.paint_uniform_color([1, 0, 0])#指定显示为红色
    print('仿射变换缩放后点云质心：', pcd3.get_center())
 
   
    # 点云显示
    o3d.visualization.draw_geometries([pcd, pcd1, pcd2, pcd3], #点云列表
                                      window_name="点云缩放",
                                      point_show_normal=False,
                                      width=800,  # 窗口宽度
                                      height=600)  # 窗口高度

5. ply 与其他数据处理

PLY是一种电脑档案格式，全名为 多边形档案（Polygon File Format）或 斯坦福三角形档案（Stanford Triangle
Format）。

格式组成：
头：声明数据格式，规定和点和面片的数据内容
点：点的数据内容（坐标x,y,z 颜色r,g,b等）
线：线的数据内容（组成线的点序号索引，颜色等）
面片：面片的数据内容（组成面的点序号索引，颜色等）

举例：

ply
format ascii 1.0
//数据保存格式，三类
//format ascii 1.0
//format binary_little_endian 1.0
//format binary_big_endian 1.0
element vertex 8 //元素：顶点 个数为8
property float x
property float y
property float z //顶点格式：依次为x,yz坐标
element edge 6 //元素：边 6条
property int vertex1
property int vertex2
property uchar red
property uchar green
property uchar blue //边存储格式为： 顶点id 1，2，颜色r,g,b
end_header //头，以end_header结束
0.194585 0.0202505 -0.654565
0.393574 0.0181872 -0.634588
0.196413 0.220227 -0.652125
0.174584 0.0180056 -0.455581
0.811062 -0.0294865 -0.551833
0.991697 -0.0650619 -0.473697
0.845413 0.167279 -0.541659
0.73238 -0.0252545 -0.368009 //点内容，8个顶点（x,y,z）坐标
0 1 255 0 0
0 2 0 255 0
0 3 0 0 255
4 5 255 0 0
4 6 0 255 0
4 7 0 0 255 //6条边，（id1,id2,r,g,b）

ply 数据读取（open3d、plyfile、 直接打开）

法1：转换为TriangleMesh格式才能被Open3d处理，包括存储和点云处理等。

import open3d as o3d
import numpy as np
ply = o3d.geometry.TriangleMesh()
ply.vertices = o3d.utility.Vector3dVector(points_array)

path = 'L004.ply'
# 1.读入mesh
ply = o3d.io.read_triangle_mesh(path)   

# 2.读入点云
pcd = o3d.io.read_point_cloud(path)

# 3.体素下采样
pcd1 = deepcopy(pcd)
# pcd1.paint_uniform_color([0, 0, 1])#指定显示为蓝色
pcd1.translate((20, 0, 0)) #整体进行x轴方向平移
pcd1 = pcd1.voxel_down_sample(voxel_size=0.1)

# 4.保存下采样的点云
o3d.io.write_point_cloud( '/home/xzz/L004_down.ply' , pcd1)

# 5.可视化
o3d.visualization.draw_geometries([pcd1 ], #点云列表
                                      window_name="体素下采样",
                                      point_show_normal=False,
                                      width=800,  # 窗口宽度
                                      height=600)  # 窗口高度

points = np.array(ply.vertices) #转为矩阵

法2：利用plyfile 等数据包打开，需要提前 pip install这个包

import numpy as np
import pandas as pd
from plyfile import PlyData, PlyElement
# ply = o3d.geometry.TriangleMesh()
# ply.vertices = o3d.utility.Vector3dVector(points_array)

path = '/media/xzz/Elements/L002.ply' 
plydata = PlyData.read(path)

# plydata.comments : ['Created by CloudComp...(Zephyrus)', 'Created 2020-04-21 4:33 PM']
# plydata.header : 'ply\nformat binary_little_endian 1.0\ ncomment Created by CloudCompare v2.10.2 (Zephyrus)\
#                   ncomment Created 2020-04-21 4:33 PM\ nobj_info Generated by CloudCompare!\
#                   nelement vertex 10283800\nproperty double x\ nproperty double y\ nproperty double z\
#                   nproperty uchar red\ nproperty uchar green\ nproperty uchar blue\ nproperty float scalar_Intensity\
#                   nproperty float scalar_GPSTime\ nproperty float scalar_ScanAngleRank\ nproperty float scalar_Label\nend_header'
# plydata.obj_info : ['Generated by CloudCompare!']
data = plydata.elements[0].data
# data[0].dtype.names : ('x', 'y', 'z', 'red', 'green', 'blue', 'scalar_Intensity', 'scalar_GPSTime', 'scalar_ScanAngleRank', 'scalar_Label')
data_pd = pd.DataFrame(data)                        # 转换成DataFrame, 因为DataFrame可以解析结构化的数据
data_np = np.zeros(data_pd.shape, dtype=np.float)   # 初始化储存数据的array
property_names = data[0].dtype.names                # 读取property的名字
for i, name in enumerate(property_names):           # 按property读取数据，这样可以保证读出的数据是同样的数据类型。
    data_np[:, i] = data_pd[name]                   # 最后转成array
print(data_np.shape)

法3：用 open 直接打开（适合纯数据，不包含头文件的ply）

def pcd_read(file_path):
    lines = []
    with open(file_path, 'r') as f:
        lines = f.readlines()
    return lines
 
#将每一行数据分割后转为数字
def ls2n(line):
    line = line.strip().split(' ')
    return list(map(float, line))

if __name__ == '__main__':
    file_path = 'bun_zipper.ply'
    points = pcd_read(file_path)[12:(12+35947)]
    points = list(map(lambda x: ls2n(x), points))

保存ply文件

# -----------------保存ply文件 法1 --------------------
def write_ply(save_path, points, text=True):
    points = [(points[i,0],points[i,1],points[i,2]) for i in range(points.shape[0])]
    vertex = np.array(points, dtype=[('x','f4'),('y','f4'),('z','f4')])
    el = PlyElement.describe(vertex,'vertex',comments=['vertices'])
    # PlyData([el],text=text).write(save_path)
    face = PlyElement.describe(np.array(dace_data,dtype=[]),'face')
    color = PlyElement.describe(np.array(clolr_data,dtype=[]),'clolr')
    normals = PlyElement.describe(np.array(normals_data,dtype=[]),'normals')
    PlyData([point,face,color,normals]).write(save_path)


# -----------------保存ply文件 法2 --------------------
output_file = 'Andre_Agassi_0015.ply'
a = np.load("Andre_Agassi_0015.npy")
points = np.float32(a)

one = np.ones((43867,3))                              # 43867是我的点云的数量，用的时候记得改成自己的
colors = np.float32(one)*255
# colors = np.array([[0, 255, 255], [0, 255, 255]])   # 给每个点添加rgb
create_output(points, colors, output_file)

def create_output(vertices, colors, filename):
    colors = colors.reshape(-1, 3)
    vertices = np.hstack([vertices.reshape(-1, 3), colors])
    np.savetxt(filename, vertices, fmt='%f %f %f %d %d %d')     # 必须先写入，然后利用write()在头部插入ply header
    ply_header = '''ply
    		format ascii 1.0
    		element vertex %(vert_num)d
    		property float x
    		property float y
    		property float z
    		property uchar red
    		property uchar green
    		property uchar blue
    		end_header
    		\n
    		'''
    with open(filename, 'r+') as f:
        old = f.read()
        f.seek(0)
        f.write(ply_header % dict(vert_num=len(vertices)))
        f.write(old)

ply转pcd

pip install open3d 或conda 安装该库，conda install -c open3d-admin open3d

import open3d as o3d

pcd = o3d.io.read_point_cloud("source_pointcloud.ply")
o3d.io.write_point_cloud("sink_pointcloud.pcd", pcd)

ply转bin，任意点云格式转ply

首先安装 pip install plyfile
读取ply中的点云数据为numpy矩阵，直接保存为bin格式。不一定需要plyfile库，也可以是open3d库，或直接用with open打开文件读取即可。

import numpy as np
import pandas as pd
from plyfile import PlyData

def convert_ply(input_path, output_path):
    plydata = PlyData.read(input_path)  # read file
    data = plydata.elements[0].data  # read data
    data_pd = pd.DataFrame(data)  # convert to DataFrame
    data_np = np.zeros(data_pd.shape, dtype=np.float)  # initialize array to store data
    property_names = data[0].dtype.names  # read names of properties
    for i, name in enumerate(
            property_names):  # read data by property
        data_np[:, i] = data_pd[name]
    data_np.astype(np.float32).tofile(output_path)
 
if __name__ == '__main__':
    convert_ply('bun_zipper.ply', 'bun_zipper.bin')

semantic kitti数据

点云（bin）与标注（label）处理

import numpy as np
scan = np.fromfile('.bin',dtype=float32)
scan = san.reshape((-1,4))

label = np.fromfile('.bin',dtype=float32)

7.下采样

最远点采样FPS

点云最远点采样FPS(Farthest Point Sampling)方法的优势是可以尽可能多地覆盖到全部点云，但是需要多次计算全部距离，因而属于复杂度较高的、耗时较多的采样方法。

FPS采样步骤

（1）选择一个初始点：可以随机选择，也可以按一定的规则来选。随机选取,每次得到的结果都不一样;反之每次得到的结果就是一致的。

    （2）计算所有点与（1）中点的距离，选择距离最大的值作为新的初始点。

    （3）重复前两步过程，直到选择的点数量满足要求。

    由于（2）中每次选择的距离都是最大的，所以迭代的过程距离最大值会逐渐减少。这也就是下面代码中mask选取的依据。如果不加这一个限制，那么点会被来回重复选到。

import numpy as np
 
def farthest_point_sample(point, npoint):
    """
    Input:
        xyz: pointcloud data, [N, D]
        npoint: number of samples
    Return:
        centroids: sampled pointcloud index, [npoint, D]
    """
    N, D = point.shape
    xyz = point[:,:3]
    centroids = np.zeros((npoint,))
    distance = np.ones((N,)) * 1e10
    farthest = np.random.randint(0, N)
    for i in range(npoint):
        centroids[i] = farthest
        centroid = xyz[farthest, :]
        dist = np.sum((xyz - centroid) ** 2, -1)
        mask = dist < distance
        distance[mask] = dist[mask]
        farthest = np.argmax(distance, -1)
    point = point[centroids.astype(np.int32)]
    return point

体素下采样（open3d）

1. open3d中体素下采样的函数为： voxel_down_sample(voxel_size=0.1)。
   其参数为体素尺寸。尺寸越大，下采样的倍数越大，点云更稀疏。
2. voxel_down_sample_and_trace(self, voxel_size, min_bound, max_bound, approximate_class=False)
   参数voxel_size体素尺寸外，还有min_bound、max_bound体素边界的最小最大坐标；
   approximate_class=True时，体素采样后的点的颜色由体素中大多数点的颜色决定。当approximate_class=False时，体素采样后的点的颜色由体素中所有点的平均颜色决定。

输出包含如下两部分：
（1）稀疏后点云坐标
（2）稀疏后点云中各个点在原点云中的索引

import open3d as o3d
from copy import deepcopy
 
if __name__ == '__main__':
    file_path = '/home/Downloads/rabbit.pcd' 
    pcd = o3d.io.read_point_cloud(file_path)
    pcd.paint_uniform_color([0.5, 0.5, 0.5])#指定显示为灰色
    print(pcd)
 
    pcd1 = deepcopy(pcd)
    pcd1.paint_uniform_color([0, 0, 1])#指定显示为蓝色
    pcd1.translate((20, 0, 0)) #整体进行x轴方向平移
    pcd1 = pcd1.voxel_down_sample(voxel_size=1)
    print(pcd1)
 
    pcd2 = deepcopy(pcd)
    pcd2.paint_uniform_color([0, 1, 0])#指定显示为绿色
    pcd2.translate((0, 20, 0)) #整体进行y轴方向平移
    res = pcd2.voxel_down_sample_and_trace(1, min_bound=pcd2.get_min_bound()-0.5, max_bound=pcd2.get_max_bound()+0.5, approximate_class=True)
    pcd2 = res[0]
    print(pcd2)
 
    
    # 点云显示
    o3d.visualization.draw_geometries([pcd, pcd1, pcd2], #点云列表
                                      window_name="体素下采样",
                                      point_show_normal=False,
                                      width=800,  # 窗口宽度
                                      height=600)  # 窗口高度

均匀下采样(open3d)

是指每隔固定的点数采样一次。样本按点的顺序执行，始终选择从第 1 个点开始，而不是随机选择。显然点存储的顺序不同，得到的结果也会不一样（适合有序点云），适合均匀采集的点云，如果点云本身不均匀，那么有可能造成某一部分的点云没被采样到。相比于体素的采样方法，点云均匀采样后的点数是固定可控的，而体素采样后的点云数量是不可控的。

pcd1 = deepcopy(pcd)
pcd1.paint_uniform_color([0, 0, 1])#指定显示为蓝色
pcd1.translate((20, 0, 0)) #整体进行x轴方向平移
pcd1 = pcd1.uniform_down_sample(100)#每100个点采样一次

随机下采样(open3d 与 numpy )

# open3d方法
pcd2 = deepcopy(pcd)
pcd2.paint_uniform_color([0, 1, 0])#指定显示为绿色
pcd2.translate((0, 20, 0)) #整体进行y轴方向平移
pcd2 = pcd2.random_down_sample(0.1)#采1/10的点云

# numpy方法
points = np.array(pcd3.points)
n = np.random.choice(len(points), 500, replace=False) #s随机采500个数据
pcd3.points = o3d.utility.Vector3dVector(points[n])
pcd3.paint_uniform_color([1, 0, 0])#指定显示为红色

# 点云显示
o3d.visualization.draw_geometries([pcd, pcd1, pcd2, pcd3], #点云列表
                                      window_name="均匀随机采样",
                                      point_show_normal=False,
                                      width=800,  # 窗口宽度
                                      height=600)  # 窗口高度

8.点云离群点剔除 （open3d）

离群点一般是指偏离大部分数据的点，可能是由于随机误差造成异常点。
离群点剔除方法: 基于统计、邻近度、密度、方差等方法。
open3d中三种剔除方法: 无效值剔除、统计方法、半径滤波法。

1. 无效值剔除

无效值包括空值和无限值。空值一般用NaN表示。

remove_non_finite_points(self, remove_nan=True, remove_infinite=True)

当remove_nan为True时，剔除空值。当remove_infinite为True时表示去除无限值。

2. 统计方式剔除(邻域滤波)

在一个点周围选择若干个点，计算它们距离的统计参数，如果某个点偏离平均值超过stdio_ratio倍的方差则认为是离群点，并进行删除。std_ratio实际上是指偏离标准差的倍数。

remove_statistical_outlier（nb_neighbors，std_ratio，print_progress = False）

第一个参数: nb_neighbors ( int ) – 目标点周围的邻居数。
第二参数: std_ratio ( float ) – 标准偏差比率。
第三个参数: print_progress ( bool , optional , default=False ) – 设置为 True 以打印进度条。

3. 半径滤波方式剔除

在指在目标点周围指定半径内统计点的数量，如果小于某一阈值则认为目标点是离群点并进行删除。两个主要参数：半径和点云数量阈值。

remove_radius_outlier（self，nb_points，半径，print_progress = False）

import open3d as o3d
from copy import deepcopy
import numpy as np
 
 
if __name__ == '__main__':
    file_path = 'rabbit.pcd'
    # 均匀采样
    pcd = o3d.io.read_point_cloud(file_path)
    pcd = pcd.uniform_down_sample(50)#每50个点采样一次
    pcd.paint_uniform_color([0.5, 0.5, 0.5])#指定显示为灰色
    print(pcd)
 
    # 剔除无效值
    pcd1 = deepcopy(pcd)
    pcd1.paint_uniform_color([0, 0, 1])#指定显示为蓝色
    pcd1.translate((20, 0, 0)) #整体进行x轴方向平移
    pcd1 = pcd1.remove_non_finite_points(True, True)#剔除无效值
    print(pcd1)
 
    # 统计方法剔除
    pcd2 = deepcopy(pcd)
    pcd2.paint_uniform_color([0, 1, 0])#指定显示为绿色
    pcd2.translate((-20, 0, 0)) #整体进行x轴方向平移
    res = pcd2.remove_statistical_outlier(20, 0.5)#统计方法剔除
    pcd2 = res[0]#返回点云，和点云索引
    print(pcd2)
 
    # 半径方法剔除
    pcd3 = deepcopy(pcd)
    pcd3.paint_uniform_color([1, 0, 0])#指定显示为红色
    pcd3.translate((0, 20, 0)) #整体进行y轴方向平移
    res = pcd3.remove_radius_outlier(nb_points=20, radius=2)#半径方法剔除
    pcd3 = res[0]#返回点云，和点云索引
    print(pcd3)
 
    # 点云显示
    o3d.visualization.draw_geometries([pcd, pcd1, pcd2, pcd3], #点云列表
                                      window_name="离群点剔除",
                                      point_show_normal=False,
                                      width=800,  # 窗口宽度
                                      height=600)  # 窗口高度

9、聚类

1、DBSCAN点云聚类

是一种基于密度的聚类算法，大体思想是根据样本点的密度和连通性，将密度满足要求且密度可达的点设置为同一类。

函数cluster_dbscan:
第1个参数eps表示DBSCAN算法确定点密度时和邻近点的距离大小，即考虑eps距离范围内的点进行密度计算。
第2个参数min_points表示组成一类最少需要多少个点。print_progress可以用来显示运行的进度。
labels返回聚类成功的类别，-1表示没有分到任何类中的点，原始点云中每个点都会分别得到一个类别标签。

labels=pcd.cluster_dbscan(eps, min_points, print_progress=False)
#labels返回聚类成功的类别，-1表示没有分到任何类中的点

file_path = '/home/xzz/Downloads/rabbit.pcd' 

pcd = o3d.io.read_point_cloud(file_path)
pcd.paint_uniform_color([0.5, 0.5, 0.5])#指定显示为灰色
print(pcd)                              # (35947, 3)

# labels返回聚类成功的类别，-1表示没有分到任何类中的点 -> (37947): [ -1, 0 ,..145,93,-1 ]
labels = np.array(pcd.cluster_dbscan(eps=0.25, min_points=16, print_progress=True))        

# 最大值相当于共有多少个类别
max_label = np.max(labels)

# 生成n+1个类别的颜色，n表示聚类成功的类别，1表示没有分类成功的类别
colors = np.random.randint(255, size=(max_label+1, 3))/255.                     # (145, 3)
colors = colors[labels] 
                                                        # (35947, 3)
# 没有分类成功的点设置为黑色
colors[labels < 0] = 0 
pcd.colors = o3d.utility.Vector3dVector(colors[:, :3])
 
# 点云显示
o3d.visualization.draw_geometries([pcd], #点云列表
                                  window_name="DBSCAN聚类",
                                  point_show_normal=False,
                                  width=800,  # 窗口宽度
                                  height=600)  # 窗口高

2. KMeans点云聚类

1.先确定k个分类中心，使得各个分类中的点到分类中心的距离总和最小。最直观的效果是将距离相近的点聚为同一类。Kmeans聚类的总数需要提前设置，即假定K个类别，也就是聚类后的类别是确定的。而DBSCAN方法聚类的类别是不确定的。

2.KMeans通常第一步是随机选择K个点作为初始化的类别中心，然后通过不断迭代进行中心坐标更新直到中心点更新距离变化小于阈值或者迭代次数达到上限。
3.KMeans++是在第一步上进行了改进，在初始化过程中尽可能选择距离相隔较远的点作为初始化中心。Skit-learn的Kmeans默认采用的初始化方式为KMeans++。

Skit-learn的Kmeans函数为sklearn.cluster.KMeans

result= KMeans(n_clusters=8,init='k-means++',n_init=10,max_iter=300,tol=0.0001,
       precompute_distances='auto',verbose=0,random_state=None,
       copy_x=True,n_jobs=1,algorithm='auto').fit(points)

（共11个参数）其中三个参数：n_clusters定义类别数量；max_iter定义最大迭代次数。
函数返回值中：返回结果用result表示，可以用 result.__dict__查看其包含的结果数据。n_iter_为算法实际迭代的次数，cluster_centers_为类别中心，labels_返回各个点的类别标签，从0开始。

#返回结果的属性
dict_keys(['n_clusters', 'init', 'max_iter', 'tol', 'precompute_distances', 'n_init', 'verbose', 'random_state', 'copy_x', 'n_jobs', 'algorithm', '_n_threads', 'n_features_in_', 'cluster_centers_', 'labels_', 'inertia_', 'n_iter_'])

from sklearn.cluster import KMeans 
 
if __name__ == '__main__':
    file_path = '/home/xzz/Downloads/rabbit.pcd' 

    pcd = o3d.io.read_point_cloud(file_path)
    # pcd = pcd.uniform_down_sample(50)#每50个点采样一次
    pcd.paint_uniform_color([0.5, 0.5, 0.5])#指定显示为灰色
    print(pcd)
    
    points = np.array(pcd.points)           # (35947, 3)
    result = KMeans(n_clusters=8).fit(points)
    # 各个类别中心
    center = result.cluster_centers_        # (8, 3)
    # labels返回聚类成功的类别，从0开始，每个数据表示一个类别
    labels = result.labels_                 # (35947):[5,1,3,...]
     
    # 最大值相当于共有多少个类别
    max_label = np.max(labels) + 1 #从0开始计算标签
    print(max(labels))
    # 生成k个类别的颜色，k表示聚类成功的类别
    colors = np.random.randint(255, size=(max_label, 3))/255.
    colors = colors[labels]
    pcd.colors = o3d.utility.Vector3dVector(colors[:, :3])
 
    # 点云显示
    o3d.visualization.draw_geometries([pcd], #点云列表
                                      window_name="Kmeans点云聚类",
                                      point_show_normal=False,
                                      width=800,  # 窗口宽度
                                      height=600)  # 窗口高度

此外还有OPTICS、Spectral Clustering(SC,即谱聚类)、 Hierarchical Clustering(层次聚类)、Mean-shift（即：均值迁移）、BIRCH、Affinity Propagation等聚类算法在点云聚类上的简单应用效果。具体可见：

/article/details/124519378

四、点云数据增强

1 中心归一化

减去均值，并除以点距原点的最大距离。

def pc_normalize(pc):
    centroid = np.mean(pc, axis=0)
    pc = pc - centroid
    m = np.max(np.sqrt(np.sum(pc ** 2, axis=1)))
    pc = pc / m
    return pc

2 打乱点云顺序

def shuffle_data(data, labels):
    """ Shuffle data and labels.
        Input:
          data: B,N,... numpy array
          label: B,... numpy array
        Return:
          shuffled data, label and shuffle indices
    """
    idx = np.arange(len(labels))
    np.random.shuffle(idx)
    return data[idx, ...], labels[idx], idx
 
 
def shuffle_points(batch_data):
    """ Shuffle orders of points in each point cloud -- changes FPS behavior.
        Use the same shuffling idx for the entire batch.
        Input:
            BxNxC array
        Output:
            BxNxC array
    """
    idx = np.arange(batch_data.shape[1])
    np.random.shuffle(idx)
    return batch_data[:,idx,:]

3 点云随机旋转

def rotate_point_cloud(batch_data):
    """ Randomly rotate the point clouds to augument the dataset
        rotation is per shape based along up direction
        Input:
          BxNx3 array, original batch of point clouds
        Return:
          BxNx3 array, rotated batch of point clouds
    """
    rotated_data = np.zeros(batch_data.shape, dtype=np.float32)
    for k in range(batch_data.shape[0]):
        rotation_angle = np.random.uniform() * 2 * np.pi
        cosval = np.cos(rotation_angle)
        sinval = np.sin(rotation_angle)
        rotation_matrix = np.array([[cosval, 0, sinval],
                                    [0, 1, 0],
                                    [-sinval, 0, cosval]])
        shape_pc = batch_data[k, ...]
        rotated_data[k, ...] = np.dot(shape_pc.reshape((-1, 3)), rotation_matrix)
    return rotated_data
 
#含法向量
def rotate_point_cloud_with_normal(batch_xyz_normal):
    ''' Randomly rotate XYZ, normal point cloud.
        Input:
            batch_xyz_normal: B,N,6, first three channels are XYZ, last 3 all normal
        Output:
            B,N,6, rotated XYZ, normal point cloud
    '''
    for k in range(batch_xyz_normal.shape[0]):
        rotation_angle = np.random.uniform() * 2 * np.pi
        cosval = np.cos(rotation_angle)
        sinval = np.sin(rotation_angle)
        rotation_matrix = np.array([[cosval, 0, sinval],
                                    [0, 1, 0],
                                    [-sinval, 0, cosval]])
        shape_pc = batch_xyz_normal[k,:,0:3]
        shape_normal = batch_xyz_normal[k,:,3:6]
        batch_xyz_normal[k,:,0:3] = np.dot(shape_pc.reshape((-1, 3)), rotation_matrix)
        batch_xyz_normal[k,:,3:6] = np.dot(shape_normal.reshape((-1, 3)), rotation_matrix)
    return batch_xyz_normal

4 z方向点云随机旋转

def rotate_point_cloud_z(batch_data):
    """ Randomly rotate the point clouds to augument the dataset
        rotation is per shape based along up direction
        Input:
          BxNx3 array, original batch of point clouds
        Return:
          BxNx3 array, rotated batch of point clouds
    """
    rotated_data = np.zeros(batch_data.shape, dtype=np.float32)
    for k in range(batch_data.shape[0]):
        rotation_angle = np.random.uniform() * 2 * np.pi
        cosval = np.cos(rotation_angle)
        sinval = np.sin(rotation_angle)
        rotation_matrix = np.array([[cosval, sinval, 0],
                                    [-sinval, cosval, 0],
                                    [0, 0, 1]])
        shape_pc = batch_data[k, ...]
        rotated_data[k, ...] = np.dot(shape_pc.reshape((-1, 3)), rotation_matrix)
    return rotated_data

5 欧拉角随机旋转
随机生成三个角度，代表x、y、z方向的旋转角。

def rotate_perturbation_point_cloud(batch_data, angle_sigma=0.06, angle_clip=0.18):
    """ Randomly perturb the point clouds by small rotations
        Input:
          BxNx3 array, original batch of point clouds
        Return:
          BxNx3 array, rotated batch of point clouds
    """
    rotated_data = np.zeros(batch_data.shape, dtype=np.float32)
    for k in range(batch_data.shape[0]):
        angles = np.clip(angle_sigma*np.random.randn(3), -angle_clip, angle_clip)
        Rx = np.array([[1,0,0],
                       [0,np.cos(angles[0]),-np.sin(angles[0])],
                       [0,np.sin(angles[0]),np.cos(angles[0])]])
        Ry = np.array([[np.cos(angles[1]),0,np.sin(angles[1])],
                       [0,1,0],
                       [-np.sin(angles[1]),0,np.cos(angles[1])]])
        Rz = np.array([[np.cos(angles[2]),-np.sin(angles[2]),0],
                       [np.sin(angles[2]),np.cos(angles[2]),0],
                       [0,0,1]])
        R = np.dot(Rz, np.dot(Ry,Rx))
        shape_pc = batch_data[k, ...]
        rotated_data[k, ...] = np.dot(shape_pc.reshape((-1, 3)), R)
    return rotated_data
 
#含法向量
def rotate_perturbation_point_cloud_with_normal(batch_data, angle_sigma=0.06, angle_clip=0.18):
    """ Randomly perturb the point clouds by small rotations
        Input:
          BxNx6 array, original batch of point clouds and point normals
        Return:
          BxNx3 array, rotated batch of point clouds
    """
    rotated_data = np.zeros(batch_data.shape, dtype=np.float32)
    for k in range(batch_data.shape[0]):
        angles = np.clip(angle_sigma*np.random.randn(3), -angle_clip, angle_clip)
        Rx = np.array([[1,0,0],
                       [0,np.cos(angles[0]),-np.sin(angles[0])],
                       [0,np.sin(angles[0]),np.cos(angles[0])]])
        Ry = np.array([[np.cos(angles[1]),0,np.sin(angles[1])],
                       [0,1,0],
                       [-np.sin(angles[1]),0,np.cos(angles[1])]])
        Rz = np.array([[np.cos(angles[2]),-np.sin(angles[2]),0],
                       [np.sin(angles[2]),np.cos(angles[2]),0],
                       [0,0,1]])
        R = np.dot(Rz, np.dot(Ry,Rx))
        shape_pc = batch_data[k,:,0:3]
        shape_normal = batch_data[k,:,3:6]
        rotated_data[k,:,0:3] = np.dot(shape_pc.reshape((-1, 3)), R)
        rotated_data[k,:,3:6] = np.dot(shape_normal.reshape((-1, 3)), R)
    return rotated_data

6 指定角度旋转点云

def rotate_point_cloud_by_angle(batch_data, rotation_angle):
    """ Rotate the point cloud along up direction with certain angle.
        Input:
          BxNx3 array, original batch of point clouds
        Return:
          BxNx3 array, rotated batch of point clouds
    """
    rotated_data = np.zeros(batch_data.shape, dtype=np.float32)
    for k in range(batch_data.shape[0]):
        #rotation_angle = np.random.uniform() * 2 * np.pi
        cosval = np.cos(rotation_angle)
        sinval = np.sin(rotation_angle)
        rotation_matrix = np.array([[cosval, 0, sinval],
                                    [0, 1, 0],
                                    [-sinval, 0, cosval]])
        shape_pc = batch_data[k,:,0:3]
        rotated_data[k,:,0:3] = np.dot(shape_pc.reshape((-1, 3)), rotation_matrix)
    return rotated_data
 
def rotate_point_cloud_by_angle_with_normal(batch_data, rotation_angle):
    """ Rotate the point cloud along up direction with certain angle.
        Input:
          BxNx6 array, original batch of point clouds with normal
          scalar, angle of rotation
        Return:
          BxNx6 array, rotated batch of point clouds iwth normal
    """
    rotated_data = np.zeros(batch_data.shape, dtype=np.float32)
    for k in range(batch_data.shape[0]):
        #rotation_angle = np.random.uniform() * 2 * np.pi
        cosval = np.cos(rotation_angle)
        sinval = np.sin(rotation_angle)
        rotation_matrix = np.array([[cosval, 0, sinval],
                                    [0, 1, 0],
                                    [-sinval, 0, cosval]])
        shape_pc = batch_data[k,:,0:3]
        shape_normal = batch_data[k,:,3:6]
        rotated_data[k,:,0:3] = np.dot(shape_pc.reshape((-1, 3)), rotation_matrix)
        rotated_data[k,:,3:6] = np.dot(shape_normal.reshape((-1,3)), rotation_matrix)
    return rotated_data

7.随机扰动

def jitter_point_cloud(batch_data, sigma=0.01, clip=0.05):
    """ Randomly jitter points. jittering is per point.
        Input:
          BxNx3 array, original batch of point clouds
        Return:
          BxNx3 array, jittered batch of point clouds
    """
    B, N, C = batch_data.shape
    assert(clip > 0)
    jittered_data = np.clip(sigma * np.random.randn(B, N, C), -1*clip, clip)
    jittered_data += batch_data
    return jittered_data

8.随机平移

def shift_point_cloud(batch_data, shift_range=0.1):
    """ Randomly shift point cloud. Shift is per point cloud.
        Input:
          BxNx3 array, original batch of point clouds
        Return:
          BxNx3 array, shifted batch of point clouds
    """
    B, N, C = batch_data.shape
    shifts = np.random.uniform(-shift_range, shift_range, (B,3))
    for batch_index in range(B):
        batch_data[batch_index,:,:] += shifts[batch_index,:]
    return batch_data

9.随机缩放

def random_scale_point_cloud(batch_data, scale_low=0.8, scale_high=1.25):
    """ Randomly scale the point cloud. Scale is per point cloud.
        Input:
            BxNx3 array, original batch of point clouds
        Return:
            BxNx3 array, scaled batch of point clouds
    """
    B, N, C = batch_data.shape
    scales = np.random.uniform(scale_low, scale_high, B)
    for batch_index in range(B):
        batch_data[batch_index,:,:] *= scales[batch_index]
    return batch_data

10.随机丢弃

def random_point_dropout(batch_pc, max_dropout_ratio=0.875):
    ''' batch_pc: BxNx3 '''
    for b in range(batch_pc.shape[0]):
        dropout_ratio =  np.random.random()*max_dropout_ratio # 0~0.875
        drop_idx = np.where(np.random.random((batch_pc.shape[1]))<=dropout_ratio)[0]
        if len(drop_idx)>0:
            batch_pc[b,drop_idx,:] = batch_pc[b,0,:] # set to the first point
    return batch_pc

五、分割可视化

if __name__ == '__main__':
    preds = np.loadtxt('Area_5_office_33.txt')    # 预测结果（xyzrgbl）
    points = np.load('Area_5_office_33.npy')      # 标签 （xyzrgbl）
    print(preds.shape, points.shape)
    print(set(preds))
    
    # 随机生成13个类别的颜色
    colors_0 = np.random.randint(255, size=(13, 3))/255.
 
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points[:, :3])
   
    # 为各个真实标签指定颜色
    colors = colors_0[points[:, -1].astype(np.uint8)]
    pcd.colors = o3d.utility.Vector3dVector(colors[:, :3])
    
    # 显示预测结果
    pcd1 = deepcopy(pcd)
    pcd1.translate((0, 5, 0)) #整体进行y轴方向平移5
    #为各个预测标签指定颜色
    colors = colors_0[preds.astype(np.uint8)]
    pcd1.colors = o3d.utility.Vector3dVector(colors[:, :3])
 
 
    # 显示预测结果和真实结果对比
    pcd2 = deepcopy(pcd)
    pcd2.translate((0, -5, 0)) #整体进行y轴方向平移-5
    preds = preds.astype(np.uint8) == points[:, -1].astype(np.uint8)
    # 为各个预测标签指定颜色
    colors = colors_0[preds.astype(np.uint8)]
    pcd2.colors = o3d.utility.Vector3dVector(colors[:, :3])
 
 
    # 点云显示
    o3d.visualization.draw_geometries([pcd, pcd1, pcd2], window_name="PointNet++语义分割结果",
                                      point_show_normal=False,
                                      width=800,  # 窗口宽度
                                      height=600)  # 窗口高度

六、逆透视变换

详情请见：

变换结果为：

七、NeRF-Studio 与 meshroom

1.使用meshroom裁剪全景图像

应用Meshroom中的aliceVision_utils_split360Images.exe将图片进行分裂

在路径Meshroom/aliceVision/bin路径cmd输入以下代码：

aliceVision_utils_split360Images.exe -i C:\Users\49659\Desktop\testdata\fVc65WKZDDo-001.jpg  -o  C:\Users\49659\Desktop\split\images  --equirectangularNbSplits 20

-i“”双引号中输入你想分裂的照片的路径，-o“”双引号中输入你想输出的分裂后照片的输出路径，–equirectangularNbSplits 8中，8是你想选择将照片分裂的数量

2.NeRF-Studio 估计图像位姿态

1. 安装 NeRF-Studio

2.命令行估计照片位姿：

ns-process-data images --sfm-tool hloc --feature-type superpoint --matcher-type superglue --data '/path/to/IMG'  --output-dir '/path/to/IMG'

该命令采用 superpoint 和 superglue替代 sift提取特征