Unity에서 Voxel grid sampling을 하자

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosMessageTypes.Sensor;

public class For_points : MonoBehaviour
{
    // 포인트 값 리스트에 저장
    public List<Vector3> pointList = new List<Vector3>();
    public uint total_number_of_points;

    // Voxel grid sampling을 위한 매개변수
    public float voxelSize = 0.05f; // 격자 크기
    private Dictionary<Vector3, Vector3> voxelGrid = new Dictionary<Vector3, Vector3>(); // 격자 내 포인트 저장

    void Start()
    {
        ROSConnection.GetOrCreateInstance().Subscribe<PointCloud2Msg>("/ouster/points", Points);
    }

    void Points(PointCloud2Msg popo)
    {
        pointList.Clear();
        voxelGrid.Clear();
        total_number_of_points = popo.height * popo.width;

        // 포인트 하나의 사이즈
        int pointSize = (int)popo.point_step;

        for (int i = 0; i < total_number_of_points; i++)
        {
            // 각 포인트의 시작 인덱스 계산
            int pointIndex = i * pointSize;

            // 포인트의 x, y, z 좌표 읽어오기
            float x = System.BitConverter.ToSingle(popo.data, pointIndex);
            float y = System.BitConverter.ToSingle(popo.data, pointIndex + 4); // 4 바이트씩 건너뛰어야 함
            float z = System.BitConverter.ToSingle(popo.data, pointIndex + 8); // 8 바이트씩 건너뛰어야 함

            Vector3 point = new Vector3(-y, z, x);


            // voxel grid sampling 수행
            Vector3 gridPosition = new Vector3(
                Mathf.Floor(point.x / voxelSize),
                Mathf.Floor(point.y / voxelSize),
                Mathf.Floor(point.z / voxelSize)
            );

            // 격자 내 포인트 갱신
            if (!voxelGrid.ContainsKey(gridPosition))
            {
                voxelGrid[gridPosition] = point;
            }
            else
            {
                // 격자 내에 포인트가 이미 있는 경우, 격자 내에서 중앙에 가장 가까운 포인트 선택
                Vector3 existingPoint = voxelGrid[gridPosition];
                voxelGrid[gridPosition] = (existingPoint + point) / 2f;
            }
        }
        // voxel grid 내 포인트를 리스트에 추가
        foreach (var point in voxelGrid.Values)
        {
            if (point.y >= 0)
            {

            }
            else
            {

                pointList.Add(point);


            }

        }
    }

    private void Update()
    {
        Resources.UnloadUnusedAssets();
    }
}



// popo.field[] 결과 0:x, 1:y, 2:z, 3:intensity, 4:t, 5:reflectivity, 6:ring, 7:ambient, 8:range