| from __future__ import annotations |
|
|
| from io import BytesIO |
| from typing import IO, Any, List, NamedTuple |
|
|
| import numpy as np |
| import numpy.typing as npt |
|
|
|
|
| class PointCloudHeader(NamedTuple): |
| """Class for Point Cloud header.""" |
|
|
| version: str |
| fields: List[str] |
| size: List[int] |
| type: List[str] |
| count: List[int] |
| width: int |
| height: int |
| viewpoint: List[int] |
| points: int |
| data: str |
|
|
|
|
| class PointCloud: |
| """ |
| Class for raw .pcd file. |
| """ |
|
|
| def __init__(self, header: PointCloudHeader, points: npt.NDArray[np.float64]) -> None: |
| """ |
| PointCloud. |
| :param header: Pointcloud header. |
| :param points: <np.ndarray, X, N>. X columns, N points. |
| """ |
| self._header = header |
| self._points = points |
|
|
| @property |
| def header(self) -> PointCloudHeader: |
| """ |
| Returns pointcloud header. |
| :return: A PointCloudHeader instance. |
| """ |
| return self._header |
|
|
| @property |
| def points(self) -> npt.NDArray[np.float64]: |
| """ |
| Returns points. |
| :return: <np.ndarray, X, N>. X columns, N points. |
| """ |
| return self._points |
|
|
| def save(self, file_path: str) -> None: |
| """ |
| Saves to .pcd file. |
| :param file_path: The path to the .pcd file. |
| """ |
| with open(file_path, 'wb') as fp: |
| fp.write('# .PCD v{} - Point Cloud Data file format\n'.format(self._header.version).encode('utf8')) |
| for field in self._header._fields: |
| value = getattr(self._header, field) |
| if isinstance(value, list): |
| text = ' '.join(map(str, value)) |
| else: |
| text = str(value) |
| fp.write('{} {}\n'.format(field.upper(), text).encode('utf8')) |
| fp.write(self._points.tobytes()) |
|
|
| @classmethod |
| def parse(cls, pcd_content: bytes) -> PointCloud: |
| """ |
| Parses the pointcloud from byte stream. |
| :param pcd_content: The byte stream that holds the pcd content. |
| :return: A PointCloud object. |
| """ |
| with BytesIO(pcd_content) as stream: |
| header = cls.parse_header(stream) |
| points = cls.parse_points(stream, header) |
| return cls(header, points) |
|
|
| @classmethod |
| def parse_from_file(cls, pcd_file: str) -> PointCloud: |
| """ |
| Parses the pointcloud from .pcd file on disk. |
| :param pcd_file: The path to the .pcd file. |
| :return: A PointCloud instance. |
| """ |
| with open(pcd_file, 'rb') as stream: |
| header = cls.parse_header(stream) |
| points = cls.parse_points(stream, header) |
| return cls(header, points) |
|
|
| @staticmethod |
| def parse_header(stream: IO[Any]) -> PointCloudHeader: |
| """ |
| Parses the header of a pointcloud from byte IO stream. |
| :param stream: Binary stream. |
| :return: A PointCloudHeader instance. |
| """ |
| headers_list = [] |
| while True: |
| line = stream.readline().decode('utf8').strip() |
| if line.startswith('#'): |
| continue |
| columns = line.split() |
| key = columns[0].lower() |
| val = columns[1:] if len(columns) > 2 else columns[1] |
| headers_list.append((key, val)) |
|
|
| if key == 'data': |
| break |
|
|
| headers = dict(headers_list) |
| headers['size'] = list(map(int, headers['size'])) |
| headers['count'] = list(map(int, headers['count'])) |
| headers['width'] = int(headers['width']) |
| headers['height'] = int(headers['height']) |
| headers['viewpoint'] = list(map(int, headers['viewpoint'])) |
| headers['points'] = int(headers['points']) |
| header = PointCloudHeader(**headers) |
|
|
| if any([c != 1 for c in header.count]): |
| raise RuntimeError('"count" has to be 1') |
|
|
| if not len(header.fields) == len(header.size) == len(header.type) == len(header.count): |
| raise RuntimeError('fields/size/type/count field number are inconsistent') |
|
|
| return header |
|
|
| @staticmethod |
| def parse_points(stream: IO[Any], header: PointCloudHeader) -> npt.NDArray[np.float64]: |
| """ |
| Parses points from byte IO stream. |
| :param stream: Byte stream that holds the points. |
| :param header: <np.ndarray, X, N>. A numpy array that has X columns(features), N points. |
| :return: Points of Point Cloud. |
| """ |
| if header.data != 'binary': |
| raise RuntimeError('Un-supported data foramt: {}. "binary" is expected.'.format(header.data)) |
|
|
| |
| row_type = PointCloud.np_type(header) |
| length = row_type.itemsize * header.points |
| buff = stream.read(length) |
| if len(buff) != length: |
| raise RuntimeError('Incomplete pointcloud stream: {} bytes expected, {} got'.format(length, len(buff))) |
|
|
| points = np.frombuffer(buff, row_type) |
|
|
| return points |
|
|
| @staticmethod |
| def np_type(header: PointCloudHeader) -> np.dtype: |
| """ |
| Helper function that translate column types in pointcloud to np types. |
| :param header: A PointCloudHeader object. |
| :return: np.dtype that holds the X features. |
| """ |
| type_mapping = {'I': 'int', 'U': 'uint', 'F': 'float'} |
| np_types = [type_mapping[t] + str(int(s) * 8) for t, s in zip(header.type, header.size)] |
|
|
| return np.dtype([(f, getattr(np, nt)) for f, nt in zip(header.fields, np_types)]) |
|
|
| def to_pcd_bin(self) -> npt.NDArray[np.float32]: |
| """ |
| Converts pointcloud to .pcd.bin format. |
| :return: <np.float32, 5, N>, the point cloud in .pcd.bin format. |
| """ |
| lidar_fields = ['x', 'y', 'z', 'intensity', 'ring'] |
| return np.array([np.array(self.points[f], dtype=np.float32) for f in lidar_fields]) |
|
|
| def to_pcd_bin2(self) -> npt.NDArray[np.float32]: |
| """ |
| Converts pointcloud to .pcd.bin2 format. |
| :return: <np.float32, 6, N>, the point cloud in .pcd.bin2 format. |
| """ |
| lidar_fields = ['x', 'y', 'z', 'intensity', 'ring', 'lidar_info'] |
| return np.array([np.array(self.points[f], dtype=np.float32) for f in lidar_fields]) |
|
|
