【AVP项目01】bag数据读取、深度图转融合点云、点云标注

最近做了一个有关AVP的项目，主要是提取停车场点云中的人形骨架，对深度图像和点云处理领域感悟颇多，于是想写下一系列的博客记录一下，有兴趣的可以看看。

本篇涉及bag数据读取、深度图转换为融合点云和点云标注。

该系列第二篇传送门：【AVP项目02】PointNet++语义分割，DBSCAN聚类实例化

1 情景介绍

自主代客泊车（AVP，Automated Valet Parking），是利用智能手段实现车辆在停车场内的自动泊车，从而将用户从繁琐的泊车过程中解放出来。

随着车路云一体化技术的发展，单车智能正向着车端场端融合智能的方向发展，车辆与停车场之间不再是独立的个体，而是构成了新的融合型的自主代客泊车系统。

该项目针对场端感知做出了一定的探索，具体来说是针对提供的分布于停车场的75个深度相机的感知数据，尝试出提取出场景中的人形骨架。

2 数据的获取与处理

2.1 原始数据概览与读取

提供的原始数据有以下4份：

名称	含义
camera_info_depth_list_K.txt	75个相机的内参
depth2global.txt	75个相机的外参（相机$\to$世界）
2024-06-22-18-59-05.bag	75个相机的深度图像
2024-06-25-21-47-20.bag	75个相机的点云

camera_info_depth_list_K.txt相机内参文件的尺寸为(75,9)，9列的含义如下：
$$[F_x,0,c_x,0,F_y,c_y,0,0,1]$$
其中$F_x,F_y$为等效的焦距，$c_x,c_y$为像元中心像素坐标。

depth2global.txt相机外参文件的尺寸为(75,7)，7列的含义如下：
$$[tx,ty,tz,rw,rx,ry,rz]$$
其中$tx,ty,tz$为平移向量，$rw,rx,ry,rz$为旋转四元数。

在针对bag数据时，需要先使用ros命令查看bag的信息，再针对性地设计读取程序，涉及的ros命令如下：

roscore
rosbag play <bag_path>
rosbag info <bag_path>
rostopic echo <topic_name>
rosmsg show <msg_name>

2024-06-22-18-59-05.bag文件存储的是75个相机在一段时间内的深度图像数据，使用以下程序即可读出并保存为*.png文件：

import numpy as np
import rosbag
from cv_bridge import CvBridge
import cv2
import os

print('Start')
bridge = CvBridge()
camera_num = 75
bag = rosbag.Bag('../data/2024-06-22-18-59-05.bag')

for i in range(1,camera_num+1):
    ts = []
    for j, (topic, msg, t) in enumerate(bag.read_messages(topics=['/realsense%03d/depth/image_rect_raw'%i])):
        img = bridge.imgmsg_to_cv2(msg,desired_encoding='passthrough')
        #保存为png,选择无损压缩0
        cv2.imwrite('../data/data_img/imgs_%03d/img_%04d.png'%(i,j), img, [int(cv2.IMWRITE_PNG_COMPRESSION),0])
        tp = (t.secs, t.nsecs)
        ts.append(tp)
    ts = np.array(ts)
    np.save('../data/data_img/imgs_%03d/ts_%03d.npy'%(i,i),ts)
bag.close()

print('End!')

2024-06-25-21-47-20.bag文件存储的是75个相机的点云数据，使用以下程序即可读出并保存为*.npy文件：

import numpy as np
import rosbag
import sensor_msgs.point_cloud2 as pc2
import os

print('Start')
camera_num = 75
bag = rosbag.Bag(r'../data/2024-06-25-21-47-20.bag')

for i in range(1,camera_num+1):
    ts = []
    for j, (topic, msg, t) in enumerate(bag.read_messages(topics=['/realsense%03d/depth/points/air'%i])):
        pcd = np.array(list((pc2.read_points(msg, field_names=("x","y","z")))))
        np.save('../data/data_pcd/pcds_%03d/pcd_%04d.npy'%(i,j), pcd)
        tp = (t.secs, t.nsecs)
        ts.append(tp)
    ts = np.array(ts)
    np.save('../data/data_pcd/pcds_%03d/ts_%03d.npy'%(i,i),ts)
bag.close()

print("End!")

2.2 深度图像融合点云

虽然已经有了各相机的点云数据，但为了加深对深度图与点云的理解，这里还是尝试将深度图数据转换为世界坐标系下的融合点云，与已提供的75个相机的融合点云做对比，并验证数据和处理方式的正确性。

各坐标系的符号设置如下：

符号	说明
$u$	图像像素坐标系
$x$	图像物理坐标系
$X_C$	相机坐标系
$X_W$	世界坐标系

则相机坐标系与图像像素坐标系间有以下转换公式:
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{cccc}{F}_x& 0& c_x& 0\\ 0& F_y& c_y& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{X}_C\\ Y_C\\ Z_C\\ 1\end{array}\right]$$
深度图转为相机坐标系点云的程序Dep2PcdC如下：

def Dep2PcdC(img, K):
    # 这是把深度图像转换成点云(相机坐标系C)的python代码(单张，单张图像进行处理)
    #转换深度图为深度
    depth = np.asarray(img,dtype=np.uint16).T  #(W,H)
    Z = depth/1000  #单位变为m
    fx, fy, cx ,cy = K[[0,4,2,5]]
    #建立XY坐标
    X = np.zeros((W,H))
    Y = np.zeros((W,H))
    for k in range(W):
        X[k,:] =np.full(X.shape[1], k)
    X = ((X-cx)*Z) / fx
    for k in range(H):
        Y[:,k] =np.full(Y.shape[0], k)
    Y = ((Y-cy)*Z) / fy
    #转为点云列表
    data_c = np.zeros((3,W*H))
    data_c[0] = X.T.reshape(-1)
    data_c[1] = Y.T.reshape(-1)
    data_c[2] = Z.T.reshape(-1)
    #点云过滤
    mask = data_c[2,:] >= 0.001
    data_c = data_c[:,mask]
    return data_c

而相机坐标系与世界坐标系间有以下转换公式：
$$\left[\begin{array}{c}{X}_W\\ Y_W\\ Z_W\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}{X}_C\\ Y_C\\ Z_C\\ 1\end{array}\right]$$
其中$R,T$可分别由$tx,ty,tz$和$rw,rx,ry,rz$获得。
深度图像转为世界坐标系点云的程序Dep2PcdW如下：

def Dep2PcdW(img, K, RT):
    # 这是把深度图像转换成点云(世界坐标系W)的python代码(单张，单张图像进行处理)
    #外参矩阵R,T
    tx, ty, tz, rw, rx, ry, rz = RT
    R = Rotation.from_quat([rw, rx, ry, rz]).as_matrix()
    transformation_matrix = np.eye(4)
    transformation_matrix[:3, :3] = R
    transformation_matrix[:3, 3] = np.array([tx, ty, tz])
    #转换
    data_c = Dep2PcdC(img,K) #转为相机坐标系点云
    data_c_homo = np.vstack((data_c, np.ones((1, data_c.shape[1]))))
    data_w_homo = transformation_matrix @ data_c_homo #这里不需要逆
    data_w = data_w_homo[0:3,:]
    return data_w

现在我们获得了75个相机在世界坐标系下的点云数据，按照时间顺序，使用open3D库，合成各时间戳下75相机的融合点云，并经过地面过滤和孤点过滤优化，过滤程序如下：

import open3d as o3d
def PcdGroundFilter(pcd):
    #滤除地面
    pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)) #法向量
    plane_model, inliers = pcd.segment_plane(distance_threshold=0.3,  #分割地面
                                             ransac_n=3,
                                             num_iterations=1000)
    filtered_pcd  = pcd.select_by_index(inliers, invert=True) # 提取平面外的点（非地面点）
    return filtered_pcd
def PcdSingleFilter(pcd):
    #滤除孤点
    r = 0.5#r 半径
    n = 20#n 球体内的点数
    cl, ind = pcd.remove_radius_outlier(n, r)
    filtered_pcd = pcd.select_by_index(ind)
    return filtered_pcd

以0063号时间戳为代表，提供的融合点云可视化如下：
同时间戳下，深度图转换的融合点云图可视化如下:

两者比较类似，说明总体上，验证了数据和处理方法的正确性。最后在实际运用时还是使用的提供的点云数据，每个时间戳下的原始点云数据为$(N,3)$的形式，每个点仅有$[x,y,z]$三个维度，因而为了给神经网络提供监督信号，需要通过标注给每个点再提供一个$label$维度。

2.3 点云数据标注

因为点云数据比较多，人工标注比较麻烦，一开始我的想法是使用Kitti等含有人形点云的数据训练一个可以自动标注的 PointNet++ 网络，用这个网络来标注AVP的点云数据。

但是后来发现Kitti也没有对点云数据做标注，而且就算标注了，根据“没有免费午餐”原理，神经网络具有特异性，一般只能适应特定场景，用别的数据集训练的模型做的标注也不一定能够准确可靠地适应自动代客泊车场景。

因此，最后还是决定人工手动标记，这里我使用了CloudCompare作为标记工具，除此外还有labelcloud等标记软件，具体的标记过程见我写的另一篇博客：【点云标注与分割】CloudCompare基础入门教程与快捷操作（gif动图演示）。

3 结语

本篇主要介绍了bag数据的导入、深度图转融合点云、点云标注，下一篇介绍PointNet++语义分割和Dbscan聚类实例化。