Unity상에서 PCA를 사용해 LiDAR Pointcloud의 바닥을 없애보자

이전 글에서 다뤘던 PCA코드를 Pointcloud 전체에 적용하는 내용입니다. Voxel grid sampling과 같이 grid를 생성하여 진행했습니다.

 

하지만 차이가 있다면 바닥의 경우 Unity 기준 y축의 정보가 없고, Object가 있는 경우 y축의 정보가 있을 겁니다!! 따라서 grid를 정육면체가 아닌 아래 그림과 같이 직육면체(y축으로 긴)를 grid로 설정하고

grid 내부의 point들에 대해 PCA를 적용하고 Eigen Vector를 얻는다면

 

Object와 바닥을 효과적으로 구분할 수 있지 않을까??

 

하는 생각을 통해 코드를 제작했습니다. 

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

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


    public List<Vector3> segList = new List<Vector3>();


    public uint total_number_of_points;
    public bool check = false;
    // Voxel grid sampling을 위한 매개변수
    public float voxelSize = 0.05f; // 격자 크기

    public float pca_voxelSize = 0.3f; // 격자 크기

    private Dictionary<Vector3, Vector3> voxelGrid = new Dictionary<Vector3, Vector3>(); // 격자 내 포인트 저장
    private Dictionary<Vector3, List<Vector3>> pca_Grid = new Dictionary<Vector3, List<Vector3>>(); // 격자 내 포인트 저장

    // PCA 시각화를 위한 LineRenderer 프리팹
    public LineRenderer pcaLineRendererPrefab;



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

    }

    void Points(PointCloud2Msg popo)
    {
        pointList.Clear();
        voxelGrid.Clear();
        pca_Grid.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 수행
            // gridPosition은 방의 위치를 설정
            Vector3 gridPosition = new Vector3(
                Mathf.Floor(point.x / voxelSize),
                Mathf.Floor(point.y / voxelSize),
                Mathf.Floor(point.z / voxelSize)
            );

            // 격자 내 포인트 갱신
            if (!voxelGrid.ContainsKey(gridPosition)) //gridPosition에 key값이 없다면 
            {
                voxelGrid[gridPosition] = point; // 해당 position에 point값 넣기 voxelGrid[key] = value값
            }
            else //key값이 있다면
            {
               
                // 격자 내에 포인트가 이미 있는 경우, 격자 내에서 중앙에 가장 가까운 포인트 선택
                Vector3 existingPoint = voxelGrid[gridPosition]; // existingPoint에 gridPosition을 넣음
                voxelGrid[gridPosition] = (existingPoint + point) / 2f; //격자 내에서 중앙에 가까운 점 선택

            }

        }


        //voxel grid 내 포인트를 리스트에 추가
        foreach (var point in voxelGrid.Values)
        {
            if (point.y >= 0)
            {

            }
            else
            {

                pointList.Add(point);

            }
            
        }

        for(int i = 0; i < pointList.Count; i ++)
        {
            Vector3 data = pointList[i];
            // pca를 위한 grid 구분하기
            Vector3 pca_gri = new Vector3(
               Mathf.Floor(data.x / pca_voxelSize),
                Mathf.Floor(data.y / 5),
                Mathf.Floor(data.z / pca_voxelSize));

            // pca를 위한 격자 내 포인트 추가
            if (!pca_Grid.ContainsKey(pca_gri))
            {
                //// grid 키가 비어있을 때 (키 채우고 값 넣기)
                pca_Grid.Add(pca_gri, new List<Vector3>());
                pca_Grid[pca_gri].Add(data);
            }
            else
            {
                // 키가 채워져 있을 때 (조건에 맞는 값 추가하기)
                pca_Grid[pca_gri].Add(data);
            }
            

        }

        // 전체 대상 pca 진행

        foreach (var datas in pca_Grid.Values)
        {
            if (datas.Count > 2)
            {
                check = false;
                do_Pca(datas);
                if (check == true)
                {
                    List<Vector3> tempList = new List<Vector3>();

                    foreach (var data in pointList)
                    {
                        if (!datas.Contains(data))
                        {
                            tempList.Add(data);
                        }
                    }

                    pointList = tempList;
                }
            }
            
        }

    }

    private void do_Pca(List<Vector3> pointList)
    {
        // 데이터 중심화를 위한 평균 구하기
        Vector3 mean = Mean_points(pointList);

        // pca를 하기 위한 데이터 중심이동
        Mat Center_mat = new Mat(pointList.Count, 3, MatType.CV_32F);
        for (int j = 0; j < Center_mat.Rows; j++)
        {
            // Center_mat.Set<float>(row, column, value);
            Center_mat.Set<float>(j, 0, pointList[j].x - mean.x);
            Center_mat.Set<float>(j, 1, pointList[j].y - mean.y);
            Center_mat.Set<float>(j, 2, pointList[j].z - mean.z);
        }

        //PCA 수행 PCA(pac진행할 행렬, 저장할 행렬, 각 데이터 표현 방법)
        PCA pca = new PCA(Center_mat, new Mat(), PCA.Flags.DataAsRow);

        // 주성분 벡터 시각화
        check = VisualizePCA(mean, pca.Eigenvectors);
    }

    private bool VisualizePCA(Vector3 mean, Mat eigenVectors)
    {
        
        LineRenderer pcaLineRenderer = new LineRenderer();
        
        for (int i = 0; i < 1; i++)
        {
            // 주성분 벡터의 방향과 크기 계산
            Vector3 eigenVector = new Vector3(
                eigenVectors.At<float>(i, 0)/3, // 주성분 벡터의 x 성분
                eigenVectors.At<float>(i, 1)/3, // 주성분 벡터의 y 성분
                eigenVectors.At<float>(i, 2)/3  // 주성분 벡터의 z 성분
            );


            // 주성분 벡터의 끝점 좌표 계산
            Vector3 endPosition = mean + eigenVector;
            Vector3 Direction = endPosition - mean;
            //float angle = Mathf.Atan2(Direction.y, Direction.x) * Mathf.Rad2Deg;
       
            float angle = Vector3.Angle(Vector3.up, Direction);

            //Debug.DrawLine(mean, endPosition);

            if (Mathf.Abs(angle) > 60)
            {
                //Debug.DrawLine(mean, endPosition);
                check = true;

            }
            else
            {
                check = false;
            }


        }

        return check;
    }

    private Vector3 Mean_points(List<Vector3> pointList) 
    {
        float x = 0;
        float y = 0;
        float z = 0;
        //평균을 구하기 위한 반복문 실행 
        for (int i = 0; i < pointList.Count; i++)
        {
            x += pointList[i].x;
            y += pointList[i].y;
            z += pointList[i].z;
        }
        float mean_x = x / pointList.Count;
        float mean_y = y / pointList.Count;
        float mean_z = z / pointList.Count;

        Vector3 meanPoints = new Vector3(mean_x, mean_y, mean_z);
        return meanPoints;
    }
   

    // Update 함수에서 사용하지 않는 리소스 해제
    private void Update()
    {
        Resources.UnloadUnusedAssets();
    }
}