点云标注工具开发记录（五）之点云文件加载、视角转换

在Open3D中，通过read方法，我们可以读取不同格式的点云数据，那么，在不使用Open3D的相关接口时，我们就需要自己重写文件读入、加载、渲染展示方法，效果如下：

点云文件读入

首先，我们要绑定事件：

main.py中启动点云读入线程

def point_cloud_read_thread_start(self, file_path):
        if file_path.endswith('.las') or file_path.endswith('.ply') or file_path.endswith('.txt'):
            self.point_cloud_read_thread.set_file_path(file_path)   # 传递文件名
            self.point_cloud_read_thread.start()                    # 线程读取文件
        else:
            QtWidgets.QMessageBox.warning(self, 'Warning', "Only support file endwith '.txt', '.ply', '.las'")

在pointcloud.py文件中我们可以看到，其继承了QThread，并重写了run方法，当我们的线程启动时，其会执行run方法的内容，

from multiprocessing import Pool

class PointCloudReadThread(QThread):
    message = pyqtSignal(str, int)
    tag = pyqtSignal(bool)

    def __init__(self):
        super(PointCloudReadThread, self).__init__()
        self.file_path = None
        self.callback = None
        self.pointcloud = None

    def run(self):
    	#在状态栏加载这段话
        self.message.emit("Open file | Loading ...", 10000000)    # 一直显示

        pool = Pool()#一个线程池，其大小为20 <multiprocessing.pool.Pool state=RUN pool_size=20>
        #pool.apply_async：这是 Pool 类的一个方法，用于异步地执行一个函数。与 pool.apply 不同，apply_async 是非阻塞的，这意味着它会立即返回一个 AsyncResult 对象（在您的例子中是 p），而不会等待函数执行完成。其余的则是需要指定的传入参数，这里其要异步执行的是read方法，
        p = pool.apply_async(func=self.read, args=(self.file_path,), callback=self.callback)
        pool.close()#关闭线程池
        pool.join()

        self.pointcloud = p.get()
        self.message.emit("Open file | Load point cloud finished.", 1000)
        self.tag.emit(True)
	#read方法，根据传入的文件个数，选择不同的文件加载方法，这里我们使用的是ply格式的文件
    @staticmethod
    def read(file_path:str):
        if file_path.endswith('.las'):
            xyz, rgb, size, offset = las_read(file_path)
        elif file_path.endswith('.ply'):
            xyz, rgb, size, offset = ply_read(file_path)
        elif file_path.endswith('.txt'):
            xyz, rgb, size, offset = txt_read(file_path)
        else:
            return None
        pointcloud = PointCloud(file_path, xyz, rgb, size, offset)
        return pointcloud

ply文件读取代码如下：

def ply_read(file_path):
	#PlyData是提供的一种PLY文件格式读取接口
    ply_data = PlyData.read(file_path)
    #PlyData的值如下：((PlyElement('vertex', (PlyProperty('x', 'double'), PlyProperty('y', 'double'), PlyProperty('z', 'double'), PlyProperty('red', 'uchar'), PlyProperty('green', 'uchar'), PlyProperty('blue', 'uchar')), count=1178312, comments=[]),), text=False, byte_order='<', comments=['Created by Open3D'], obj_info=[])
    if 'vertex' not in ply_data:
        return np.array([]), np.array([]), np.array([0, 0, 0]), np.array([0, 0, 0])
    #下面这段代码则将ply_data的内容读取处理
	#ply_data['vertex']['x']
	#Out[5]: memmap([ 7.90625,  7.65625,  8.     , ..., 34.78125, 34.71875, 30.1875 ])
    vertices = np.vstack((ply_data['vertex']['x'],
                          ply_data['vertex']['y'],
                          ply_data['vertex']['z'])).transpose()
    if 'red' in ply_data['vertex']:
        colors = np.vstack((ply_data['vertex']['red'],
                            ply_data['vertex']['green'],
                            ply_data['vertex']['blue'])).transpose()
    else:
        colors = np.ones(vertices.shape)
	
    vertices = vertices.astype(np.float32)
    #接下来这个操作便是计算一些点云属性，入最值，大小、偏移等
    xmin, ymin, zmin = min(vertices[:, 0]), min(vertices[:, 1]), min(vertices[:, 2])
    xmax, ymax, zmax = max(vertices[:, 0]), max(vertices[:, 1]), max(vertices[:, 2])
    size = np.array((xmax - xmin, ymax - ymin, zmax - zmin))
    #偏移值即xyz的最小值
    offset = np.array([xmin, ymin, zmin])
    vertices -= offset
    colors = colors.astype(np.float32)
    #颜色归一化
    colors = colors / 255
    return vertices, colors, size, offset

使用PlyData读取的ply文件内容如下：

将其转换为numpy格式查看，我们看到其内容如下，即表示xyz、rgb。

生成点云对象

将点云信息获得后，生成点云对象（这个PointCloud是我们自己定义的）

#生成点云对象
pointcloud = PointCloud(file_path, xyz, rgb, size, offset)

class PointCloud:
    def __init__(self, file_path:str, xyz, rgb, size, offset):
        self.file_path:str = file_path
        self.xyz:np.ndarray = xyz if xyz.dtype == np.float32 else xyz.astype(np.float32)
        self.offset:np.ndarray = offset
        self.size:np.ndarray = size
        self.num_point = self.xyz.shape[0]
        self.rgb:np.ndarray = rgb if rgb.dtype == np.float32 else rgb.astype(np.float32)

    def __str__(self):
        return "<PointCloud num_point: {} | size: ({:.2f}, {:.2f}, {:.2f}) | offset: ({:.2f}, {:.2f}, {:.2f}) >".format(
            self.num_point, self.size[0], self.size[1], self.size[2], self.offset[0], self.offset[1], self.offset[2])

点云渲染生成

最终，我们也就得到了点云文件，接下来将送消息通知，调用point_cloud_read_thread_finished方法，这个方法主要是初始化一些点云信息，用于将来保存，在这里面，真正在屏幕上生成点云图像的是self.openGLWidget.load_vertices(pointcloud, categorys, instances)方法

#线程绑
self.point_cloud_read_thread.tag.connect(self.point_cloud_read_thread_finished)
def point_cloud_read_thread_finished(self, tag:bool):
        if tag:#self.tag.emit(True)这个tag是在这里
            pointcloud = self.point_cloud_read_thread.pointcloud#获得我们生成的PointCloud对象
            if pointcloud is None:
                return
            #
            label_file = '.'.join(self.current_file.split('.')[:-1]) + '.json'#当我们点击保存时，会生成json的label标签文件，目录与我们的点云文件相同，该文件设置为仅可读模式
            categorys = None
            instances = None
            #判断这个文件是否存在，继续写入，这个是在第二次打开时才会生效，然后判断点云文件与先前的点云是否相同，并且加载先前我们对点云的分类信息
            if os.path.exists(label_file):
                with open(label_file, 'r') as f:
                    datas = load(f)
                    info = datas.get('info', '')
                    if info == 'Laiease label file.':
                        categorys = datas.get('categorys', [])
                        instances = datas.get('instances', [])
                        categorys = np.array(categorys, dtype=np.int16)
                        instances = np.array(instances, dtype=np.int16)

                        if categorys.shape[0] != pointcloud.xyz.shape[0] or instances.shape[0] != pointcloud.xyz.shape[0]:
                            QtWidgets.QMessageBox.warning(self, 'Warning', 'Point cloud size does not match label size!')
                            if categorys.shape[0] != pointcloud.xyz.shape[0]:
                                categorys = None
                            if instances.shape[0] != pointcloud.xyz.shape[0]:
                                instances = None
            if pointcloud.num_point < 1:
                return
			#展示点云
            self.openGLWidget.load_vertices(pointcloud, categorys, instances)
            #侧边标签显示点云信息
            self.label_num_point.setText('{}'.format(pointcloud.num_point))
            self.label_size_x.setText('{:.2f}'.format(pointcloud.size[0]))
            self.label_size_y.setText('{:.2f}'.format(pointcloud.size[1]))
            self.label_size_z.setText('{:.2f}'.format(pointcloud.size[2]))
            self.label_offset_x.setText('{:.2f}'.format(pointcloud.offset[0]))
            self.label_offset_y.setText('{:.2f}'.format(pointcloud.offset[1]))
            self.label_offset_z.setText('{:.2f}'.format(pointcloud.offset[2]))

            self.setWindowTitle(pointcloud.file_path)
            #开启点云框选、取消框选按钮
            self.actionPick.setEnabled(True)
            self.actionCachePick.setEnabled(True)

展示点云调用的是load_vertices(pointcloud, categorys, instances)方法，该方法将计算点云的一些初始化参数

    def load_vertices(self, pointcloud, categorys:np.ndarray=None, instances:np.ndarray=None):
        self.reset()#清空先前的信息

        self.pointcloud = pointcloud
        #这里是对视角进行转换，如平移旋转等
        self.vertex_transform.setTranslation(-pointcloud.size[0]/2, -pointcloud.size[1]/2, -pointcloud.size[2]/2)
        #添加掩膜，用于根据标签展示点云
        self.mask = np.ones(pointcloud.num_point, dtype=bool)
        self.category_display_state_dict = {}
        self.instance_display_state_dict = {}
		#这里会读取categorys的内容，即在point_cloud_read_thread_finished方法中加载的json里面的类别信息
        self.categorys = categorys if categorys is not None else np.zeros(pointcloud.num_point, dtype=np.int16)#这个是不同展示模式，设计了rgb、categorys以及instances三种模式
        self.instances = instances if categorys is not None else np.zeros(pointcloud.num_point, dtype=np.int16)
        #计算缩放系数，这个值用于后面屏幕点击坐标转换
        self.ortho_change_scale = max(pointcloud.size[0] / (self.height() / 5 * 4),
                                      pointcloud.size[1] / (self.height() / 5 * 4))
        self.current_vertices = self.pointcloud.xyz#这个是要通过vob渲染展示到屏幕的点云
        self.current_colors = self.pointcloud.rgb
        self.init_vertex_vao()#渲染展示
        self.resizeGL(self.width(), self.height())
        self.update()

视角转换

在点云标注过程中，我们经常要进行点云视角切换，如前视角、后视角等。
视角切换代码如下，我们以左视角为例：
首先，需要绑定视角切换按钮和事件

self.actionLeft_view.triggered.connect(self.openGLWidget.set_left_view)

在openglwidget.py文件中，定义了set_left_view方法

		self.vertex_transform = Transform()
        self.circle_transform = Transform()
        self.axes_transform = Transform()
        self.keep_transform = Transform()
        self.projection = QMatrix4x4()
        self.camera = Camera()
        
def set_left_view(self):
        self.vertex_transform.setRotation(QQuaternion.fromAxisAndAngle(QVector3D(1, 0, 0), 270))
        self.circle_transform.setRotation(QQuaternion.fromAxisAndAngle(QVector3D(1, 0, 0), 270))
        self.axes_transform.setRotation(QQuaternion.fromAxisAndAngle(QVector3D(1, 0, 0), 270))
        self.vertex_transform.rotate(QVector3D(0, 1, 0), 90)
        self.circle_transform.rotate(QVector3D(0, 1, 0), 90)
        self.axes_transform.rotate(QVector3D(0, 1, 0), 90)
        self.update()

Transform()类的定义如下：

from PyQt5.QtGui import QMatrix4x4, QVector3D, QQuaternion
#视角转换

class Transform(object):
    def __init__(self):
        self.m_translation = QVector3D()
        self.m_scale = QVector3D(1, 1, 1)
        self.m_rotation = QQuaternion()
        self.m_world = QMatrix4x4()

        self.localforward = QVector3D(0.0, 0.0, 1.0)
        self.localup = QVector3D(0.0, 1.0, 0.0)
        self.localright = QVector3D(1.0, 0.0, 0.0)

    def forward(self):
        return self.m_rotation.rotatedVector(self.localforward)

    def up(self):
        return self.m_rotation.rotatedVector(self.localup)

    def right(self):
        return self.m_rotation.rotatedVector(self.localright)

    def translate(self, dx, dy, dz):
        self.m_translation += QVector3D(dx, dy, dz)

    def scale(self, sx, sy, sz):
        self.m_scale *= QVector3D(sx, sy, sz)

    def rotate(self, axis, angle):
        dr = QQuaternion.fromAxisAndAngle(axis, angle)
        self.m_rotation = dr * self.m_rotation
        self.m_translation = dr.rotatedVector(self.m_translation)

    def rotate_in_place(self, axis, angle):
        dr = QQuaternion.fromAxisAndAngle(axis, angle)
        self.m_rotation = dr * self.m_rotation

    def setTranslation(self, dx, dy, dz):
        self.m_translation = QVector3D(dx, dy, dz)

    def setTranslationwithRotate(self, dx, dy, dz):
        self.m_translation = self.m_rotation.rotatedVector(QVector3D(dx, dy, dz))

    def setScale(self, sx, sy, sz):
        self.m_scale = QVector3D(sx, sy, sz)

    def setRotation(self, r:QQuaternion):
        dr = r * self.m_rotation.inverted()
        self.m_translation = dr.rotatedVector(self.m_translation)
        self.m_rotation = r

    def toMatrix(self):
        self.m_world.setToIdentity()
        self.m_world.translate(self.m_translation)
        self.m_world.scale(self.m_scale)
        self.m_world.rotate(self.m_rotation)
        return self.m_world

当然，这里的视角切换是切换到固定视角，当我们的鼠标在拖动时，点云也会随着切换视角，此时，其实现方法与之类似：

def mouse_rotate(self, xoffset, yoffset):
        # 点云旋转
        self.vertex_transform.rotate(self.vertex_transform.localup, xoffset * 0.5)
        self.vertex_transform.rotate(self.vertex_transform.localright, yoffset * 0.5)
        # 坐标旋转
        self.circle_transform.rotate_in_place(self.circle_transform.localup, xoffset * 0.5)
        self.circle_transform.rotate_in_place(self.circle_transform.localright, yoffset * 0.5)
        self.axes_transform.rotate_in_place(self.axes_transform.localup, xoffset * 0.5)
        self.axes_transform.rotate_in_place(self.axes_transform.localright, yoffset * 0.5)
        self.update()

为了将坐标从一个坐标系变换到另一个坐标系，需要用到几个变换矩阵，最重要的几个分别是模型(Model)、观察(View)、投影(Projection)三个矩阵。顶点坐标起始于局部空间(Local Space)，在这里它称为局部坐标(Local Coordinate)，它在之后会变为世界坐标(World Coordinate)，观察坐标(View Coordinate)，裁剪坐标(Clip Coordinate)，并最后以屏幕坐标(Screen Coordinate)的形式结束。下面的这张图展示了整个流程以及各个变换过程做了什么：