[pcd] 도로 포인트클라우드를 평면으로 정렬하는 4가지 방법: 딥러닝 전처리를 위한 접근(3. DTM기반 CSF)

포인트클라우드는 자율주행, 디지털 트윈, 스마트시티 등에서 중요한 3D 데이터입니다. 특히 도로 주행 환경을 스캔한 포인트클라우드를 딥러닝에 활용하려면, 도로면을 평면으로 정렬하거나 2D 이미지처럼 표현하는 전처리 과정이 필요합니다.

 

이 글에서는 도로와 같은 경사 있고 굴곡진 포인트클라우드를 projection 하기 위한 4가지 대표적인 방법 중 세 번째를 소개합니다.

 

3. DTM(DIgital Terrain Model)

DTM은 지표면의 형상을 3D로 표현한 모델입니다.

LiDAR,  항공사진 등으로부터 얻은 포인트 클라우드를 분석해 지면만 분리하고, 그위에 있는 구조물(건물, 나무 등)은 제거합니다.

 

CSF(Cloth Simulation Filtering)란?

CSFsms DTM을 추출하는 대표적인 방법중의 하나입니다.

거꾸로 뒤집힌 천 을 포인트클라우드 아래로 떨어뜨린다고 상상해보았을때, 

- 천이 포인트들과 부딪히면, 가장 낮은 부분(땅)에 닿게 됩니다.

- 천의 형상이 지면을 근사합니다

- 이후 천에 닿은 점들만 남기면 ground point분리가 가능합니다.

 

CSF는 실제로 물리 기반 시뮬레이션에 기반해 있습니다:

 

1. 천은 이산격자(grid)로 구성됨 — 각 노드 \(v_i\)
2. 인접 노드 간의 거리와 응력(stiffness)을 고려하여 다음 조건을 만족하도록 힘의 평형을 계산:
3. 포인트클라우드와의 충돌 조건에 따라 천의 높이를 업데이트:- 천이 점에 닿으면 더 이상 떨어지지 않음(collision)
4. 충돌 지점을 기반으로 지면 추정

 

\(m \cdot \frac{d^2 v_i}{dt^2} = \sum_{j \in N(i)} k (v_j - v_i) + g\)

 

\( m: \) 질량

\( k: \) 스프링 상수 (천의 유연성)

\( g: \) 중력

\( N(i): \) 인접 노드 집합

 

말이 되나 이게

 

import numpy as np
from scipy.spatial import cKDTree
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import plotly.graph_objects as go

# 지형 데이터 생성 (평면 + 노이즈)
x = np.linspace(-10, 10, 100)
y = np.linspace(-10, 10, 100)
X, Y = np.meshgrid(x, y)
Z_ground = 0.1 * np.sin(X/2) + 0.1 * np.cos(Y/2) + np.random.normal(0, 0.05, X.shape)

# 비지형 데이터 생성 (건물, 나무 등)
num_objects = 5
objects = []
for _ in range(num_objects):
    x_obj = np.random.uniform(-8, 8)
    y_obj = np.random.uniform(-8, 8)
    height = np.random.uniform(1, 3)
    width = np.random.uniform(0.5, 1.5)
    
    # 간단한 직육면체 생성
    x_cube = np.random.uniform(x_obj - width/2, x_obj + width/2, 50)
    y_cube = np.random.uniform(y_obj - width/2, y_obj + width/2, 50)
    z_cube = np.random.uniform(0, height, 50)
    
    objects.append(np.column_stack((x_cube, y_cube, z_cube)))

# 모든 포인트 결합
ground_points = np.column_stack((X.flatten(), Y.flatten(), Z_ground.flatten()))
object_points = np.vstack(objects)
points = np.vstack((ground_points, object_points))

# 시각화
fig = go.Figure(data=[go.Scatter3d(
    x=points[:, 0],
    y=points[:, 1],
    z=points[:, 2],
    mode='markers',
    marker=dict(
        size=2,
        color=points[:, 2],
        colorscale='Viridis',
        opacity=0.8
    )
)])

fig.update_layout(
    title='Sample Point Cloud',
    scene=dict(aspectmode='data')
)
fig.show()

 

 

def create_cloth(points, resolution=1.0):
    # 포인트 클라우드의 경계 계산
    min_x, min_y = np.min(points[:, :2], axis=0)
    max_x, max_y = np.max(points[:, :2], axis=0)
    
    # 천의 크기 계산
    cloth_width = int(np.ceil((max_x - min_x) / resolution)) + 1
    cloth_height = int(np.ceil((max_y - min_y) / resolution)) + 1
    
    # 천의 초기 위치 계산
    x = np.linspace(min_x, max_x, cloth_width)
    y = np.linspace(min_y, max_y, cloth_height)
    X, Y = np.meshgrid(x, y)
    
    # 천의 초기 높이는 포인트 클라우드의 최대 높이보다 약간 높게 설정
    max_z = np.max(points[:, 2])
    Z = np.ones_like(X) * (max_z + 5)
    
    # 천의 각 점의 위치와 속도 초기화
    cloth_positions = np.stack([X.flatten(), Y.flatten(), Z.flatten()], axis=1)
    cloth_velocities = np.zeros_like(cloth_positions)
    
    # 천의 연결 구조 생성 (격자 구조)
    cloth_connections = []
    for i in range(cloth_height):
        for j in range(cloth_width):
            idx = i * cloth_width + j
            if j < cloth_width - 1:
                cloth_connections.append((idx, idx + 1))  # 가로 연결
            if i < cloth_height - 1:
                cloth_connections.append((idx, idx + cloth_width))  # 세로 연결
    
    return cloth_positions, cloth_velocities, np.array(cloth_connections)

# 천 생성
cloth_positions, cloth_velocities, cloth_connections = create_cloth(points)

# 시각화
fig = go.Figure()

# 포인트 클라우드
fig.add_trace(go.Scatter3d(
    x=points[:, 0],
    y=points[:, 1],
    z=points[:, 2],
    mode='markers',
    marker=dict(size=2, color='gray', opacity=0.5),
    name='Point Cloud'
))

# 천
fig.add_trace(go.Scatter3d(
    x=cloth_positions[:, 0],
    y=cloth_positions[:, 1],
    z=cloth_positions[:, 2],
    mode='markers',
    marker=dict(size=3, color='red'),
    name='Cloth'
))

# 천의 연결선
for i, j in cloth_connections:
    fig.add_trace(go.Scatter3d(
        x=[cloth_positions[i, 0], cloth_positions[j, 0]],
        y=[cloth_positions[i, 1], cloth_positions[j, 1]],
        z=[cloth_positions[i, 2], cloth_positions[j, 2]],
        mode='lines',
        line=dict(color='red', width=1),
        showlegend=False
    ))

fig.update_layout(
    title='Initial Cloth and Point Cloud',
    scene=dict(aspectmode='data')
)
fig.show()

 

 

def update_cloth(points, cloth_positions, cloth_velocities, cloth_connections, 
                 time_step=0.65, gravity=9.8, damping=0.5):
    # KDTree 생성
    kdtree = cKDTree(points[:, :2])
    
    # 중력 적용
    cloth_velocities[:, 2] -= gravity * time_step
    
    # 포인트와의 상호작용
    distances, indices = kdtree.query(cloth_positions[:, :2])
    for i, (dist, idx) in enumerate(zip(distances, indices)):
        if dist < 3.0:  # cloth_resolution
            point_z = points[idx, 2]
            if cloth_positions[i, 2] > point_z+0.1:
                # 반발력 적용
                cloth_velocities[i, 2] = 0
                cloth_positions[i, 2] = point_z
    
    # 천의 연결 구조에 따른 제약 조건 적용
    for i, j in cloth_connections:
        pos_i = cloth_positions[i]
        pos_j = cloth_positions[j]
        vel_i = cloth_velocities[i]
        vel_j = cloth_velocities[j]
        
        # 연결 길이 제약
        rest_length = np.linalg.norm(pos_i[:2] - pos_j[:2])
        current_length = np.linalg.norm(pos_i - pos_j)
        
        if current_length > rest_length:
            # 반발력 적용
            direction = (pos_j - pos_i) / current_length
            spring_k = 1
            force = spring_k*(current_length - rest_length) * time_step
            
            cloth_velocities[i] += direction * force
            cloth_velocities[j] -= direction * force
    
    # 속도 감쇠
    cloth_velocities *= (1 - damping)
    
    # 위치 업데이트
    cloth_positions += cloth_velocities * time_step
    
    return cloth_positions, cloth_velocities
    
    def simulate_and_visualize(points, cloth_positions, cloth_velocities, cloth_connections, 
                          num_iterations=100, visualize_every=20):
    print(f"Starting simulation with {num_iterations} iterations, visualizing every {visualize_every} iterations")
    
    for i in range(num_iterations):
        cloth_positions, cloth_velocities = update_cloth(
            points, cloth_positions, cloth_velocities, cloth_connections
        )
        
        if i % visualize_every == 0:
            print(f"Visualizing iteration {i}")
            fig = go.Figure()
            
            # 포인트 클라우드
            fig.add_trace(go.Scatter3d(
                x=points[:, 0],
                y=points[:, 1],
                z=points[:, 2],
                mode='markers',
                marker=dict(size=2, color='gray', opacity=0.5),
                name='Point Cloud'
            ))
            
            # 천
            fig.add_trace(go.Scatter3d(
                x=cloth_positions[:, 0],
                y=cloth_positions[:, 1],
                z=cloth_positions[:, 2],
                mode='markers',
                marker=dict(size=3, color='red'),
                name='Cloth'
            ))
            
            # 천의 연결선
            for m, n in cloth_connections:
                fig.add_trace(go.Scatter3d(
                    x=[cloth_positions[m, 0], cloth_positions[n, 0]],
                    y=[cloth_positions[m, 1], cloth_positions[n, 1]],
                    z=[cloth_positions[m, 2], cloth_positions[n, 2]],
                    mode='lines',
                    line=dict(color='red', width=1),
                    showlegend=False
                ))
            title_str = f'Cloth Simulation (Iteration {i}/{num_iterations})'
            fig.update_layout(
                title=title_str,
                scene=dict(aspectmode='data')
            )
            fig.show()
    
    print("Simulation completed")
    return cloth_positions
    
    # 시뮬레이션 실행
print("Starting simulation...")
final_cloth_positions = simulate_and_visualize(
    points, cloth_positions, cloth_velocities, cloth_connections,
    num_iterations=5,  # 반복 횟수
    visualize_every=1   # 시각화 간격
)

 

 

 

 

 

Initial cloth -> iteration 0은 꽤 괜찮은것같은데 iteration이 반복될수록 이상합니다