feat: initial commit - massive-cloud point cloud processing system

alg/点云测距v1.1/train_measurement_system/src/pointcloud_loader.py

@@ -0,0 +1,103 @@
+"""点云加载模块"""
+import numpy as np
+from pathlib import Path
+import struct
+
+
+class PointCloudLoader:
+    """加载点云文件"""
+
+    @staticmethod
+    def load_ply(file_path: str) -> np.ndarray:
+        """
+        加载PLY格式点云
+
+        Args:
+            file_path: PLY文件路径
+
+        Returns:
+            np.ndarray: 点云数组 (N, 3)
+        """
+        path = Path(file_path)
+        if not path.exists():
+            raise FileNotFoundError(f"点云文件不存在: {file_path}")
+
+        print(f"正在加载点云: {file_path}")
+
+        with open(path, 'rb') as f:
+            # 读取头部
+            header_lines = []
+            while True:
+                line = f.readline().decode('ascii').strip()
+                header_lines.append(line)
+                if line == 'end_header':
+                    break
+
+            # 解析头部
+            vertex_count = 0
+            format_type = None
+            properties = []
+
+            for line in header_lines:
+                if line.startswith('element vertex'):
+                    vertex_count = int(line.split()[-1])
+                elif line.startswith('format'):
+                    format_type = line.split()[1]
+                elif line.startswith('property'):
+                    parts = line.split()
+                    properties.append({
+                        'type': parts[1],
+                        'name': parts[2]
+                    })
+
+            if vertex_count == 0:
+                raise ValueError("PLY文件中没有顶点数据")
+
+            # 读取顶点数据
+            if format_type == 'binary_little_endian':
+                points = PointCloudLoader._read_binary_ply(f, vertex_count, properties)
+            elif format_type == 'ascii':
+                points = PointCloudLoader._read_ascii_ply(f, vertex_count)
+            else:
+                raise ValueError(f"不支持的PLY格式: {format_type}")
+
+        print(f"  加载完成: {len(points)} 个点")
+        return points
+
+    @staticmethod
+    def _read_binary_ply(f, vertex_count: int, properties: list) -> np.ndarray:
+        """读取二进制PLY数据"""
+        # 构建struct格式字符串
+        type_map = {
+            'float': 'f',
+            'double': 'd',
+            'uchar': 'B',
+            'uint': 'I',
+            'int': 'i'
+        }
+
+        fmt = '<'  # 小端序
+        for prop in properties:
+            fmt += type_map.get(prop['type'], 'f')
+
+        struct_size = struct.calcsize(fmt)
+        points = []
+
+        for _ in range(vertex_count):
+            data = struct.unpack(fmt, f.read(struct_size))
+            # 只取前3个值作为xyz坐标
+            points.append(data[:3])
+
+        return np.array(points, dtype=np.float64)
+
+    @staticmethod
+    def _read_ascii_ply(f, vertex_count: int) -> np.ndarray:
+        """读取ASCII PLY数据"""
+        points = []
+        for _ in range(vertex_count):
+            line = f.readline().decode('ascii').strip()
+            values = line.split()
+            # 只取前3个值作为xyz坐标
+            points.append([float(values[0]), float(values[1]), float(values[2])])
+
+        return np.array(points, dtype=np.float64)