# 카메라, LIDAR, IMU 데이터 수집
#### 카메라 데이터 수집
카메라 데이터 수집은 주로 시뮬레이션에서 환경의 이미지를 얻어 후처리하거나 분석하는 데 사용된다. 시뮬레이션에서 카메라는 3D 공간에서 위치와 방향을 가진다. 카메라의 주요 매개변수로는 초점 거리, 센서 크기, 해상도 등이 있다. 이를 URDF나 SDF 파일에서 정의하고 Gazebo와 같은 시뮬레이션 환경에서 데이터를 수집할 수 있다.
카메라 모델을 정의할 때 중요한 요소는 다음과 같다:
**카메라 파라미터**
카메라 모델의 기본 방정식은 **핀홀 카메라 모델**로 설명할 수 있으며, 이는 3D 세계 좌표에서 2D 이미지 평면으로의 변환을 나타낸다. 이를 수학적으로 나타내면:
$$
\mathbf{p}*{\text{image}} = \mathbf{K} \mathbf{T} \mathbf{P}*{\text{world}}
$$
여기서:
* $\mathbf{p}\_{\text{image}}$는 2D 이미지 좌표이다.
* $\mathbf{K}$는 **카메라 내부 파라미터 행렬**이다.
$$
\mathbf{K} = \begin{bmatrix} f\_x & 0 & c\_x \ 0 & f\_y & c\_y \ 0 & 0 & 1 \end{bmatrix}
$$
여기서 $f\_x$와 $f\_y$는 이미지 평면의 초점 거리, $c\_x$와 $c\_y$는 이미지 센터 좌표이다.
* $\mathbf{T}$는 **카메라의 외부 파라미터 행렬**로, 회전 및 변환을 포함한다.
* $\mathbf{P}\_{\text{world}}$는 3D 세계 좌표에서의 포인트이다.
#### LIDAR 데이터 수집
LIDAR는 레이저를 사용하여 주변 환경의 거리를 측정하고, 이를 통해 3D 점군 데이터를 생성한다. 시뮬레이션에서는 LIDAR 센서를 정의하여 환경의 구조를 모델링할 수 있다. LIDAR의 주요 매개변수로는 레이저 스캔 각도, 레이저 거리 범위, 해상도 등이 있다.
LIDAR 센서가 제공하는 데이터는 주로 거리 값으로, 이를 수학적으로는 다음과 같이 나타낼 수 있다:
$$
d\_i = \sqrt{(x\_i - x\_0)^2 + (y\_i - y\_0)^2 + (z\_i - z\_0)^2}
$$
여기서:
* $d\_i$는 LIDAR 센서에서 측정한 거리이다.
* $(x\_0, y\_0, z\_0)$는 LIDAR 센서의 위치이다.
* $(x\_i, y\_i, z\_i)$는 측정된 물체의 좌표이다.
#### IMU 데이터 수집
IMU(Inertial Measurement Unit)는 가속도계와 자이로스코프를 사용하여 로봇의 자세와 속도를 측정한다. IMU 센서에서 제공되는 데이터는 주로 3축 가속도와 3축 각속도이다.
IMU 데이터는 다음과 같은 수식으로 표현할 수 있다:
1. **가속도 데이터**:
$$
\mathbf{a}*{\text{measured}} = \mathbf{a}*{\text{world}} + \mathbf{g}
$$
여기서:
* $\mathbf{a}\_{\text{measured}}$는 IMU에서 측정한 가속도이다.
* $\mathbf{a}\_{\text{world}}$는 월드 좌표계에서의 가속도이다.
* $\mathbf{g}$는 중력 가속도 벡터이다.
2. **각속도 데이터**:
$$
\boldsymbol{\omega}*{\text{measured}} = \boldsymbol{\omega}*{\text{world}}
$$
여기서:
* $\boldsymbol{\omega}\_{\text{measured}}$는 IMU에서 측정한 각속도이다.
* $\boldsymbol{\omega}\_{\text{world}}$는 월드 좌표계에서의 각속도이다.
IMU 센서는 자주 Kalman 필터나 다른 필터링 기법을 사용하여 데이터에서 노이즈를 제거하고 정확한 자세 추정을 수행한다.
#### 카메라, LIDAR, IMU 데이터 수집을 위한 URDF/SDF 설정
시뮬레이션에서 센서를 정의하고 데이터 수집을 수행하기 위해서는 URDF 또는 SDF 파일에 각 센서의 특성과 파라미터를 설정해야 한다. 여기서는 각 센서의 URDF/SDF 설정 방법에 대해 다룬다.
**카메라 센서의 URDF/SDF 설정**
URDF와 SDF에서 카메라 센서를 정의하려면 카메라의 위치, 방향, 해상도, 초점 거리 등의 파라미터를 설정해야 한다. 예를 들어, 카메라의 해상도는 SDF 파일에서 다음과 같이 설정할 수 있다:
```xml
1.3962634
640
480
R8G8B8
0.1
100
```
여기서 `horizontal_fov`는 카메라의 수평 시야각이며, `width`와 `height`는 이미지의 해상도를 정의한다. `clip`은 카메라의 클리핑 평면을 정의하여 카메라가 볼 수 있는 최소 및 최대 거리를 설정한다.
**LIDAR 센서의 URDF/SDF 설정**
LIDAR 센서는 URDF나 SDF에서 설정할 수 있으며, 특히 레이저의 범위, 해상도, 스캔 각도 등을 정의해야 한다. SDF 파일에서 LIDAR 센서를 설정하는 예시는 다음과 같다:
```xml
640
1
-1.5708
1.5708
1
1
0
0
0.1
30.0
0.01
```
여기서 `samples`는 레이저 빔의 수를 나타내고, `min_angle`과 `max_angle`은 레이저가 스캔하는 각도를 정의한다. `min`과 `max`는 LIDAR의 거리 범위를 나타내며, `resolution`은 거리 해상도를 설정한다.
**IMU 센서의 URDF/SDF 설정**
IMU는 가속도계와 자이로스코프의 데이터를 수집하는 센서로, URDF나 SDF에서 설정할 수 있다. 다음은 SDF에서 IMU 센서를 정의하는 예시이다:
```xml
0
0
0
0
0
0
```
여기서 `angular_velocity`는 자이로스코프 데이터를 나타내고, `linear_acceleration`은 가속도계 데이터를 나타낸다. IMU 센서는 로봇의 회전과 가속도를 추적하여 자세 추정에 중요한 역할을 한다.
#### 센서 데이터 필터링
센서 데이터는 노이즈가 포함될 수 있으며, 이를 처리하기 위해 필터링 기법이 사용된다. 일반적으로 Kalman 필터, 확장 칼만 필터(EKF), 또는 복합 센서 필터링 기법이 활용된다. 특히 IMU 데이터를 처리할 때 가속도와 각속도 데이터를 동시에 처리하여 자세 추정의 정확성을 높일 수 있다.
**확장 칼만 필터(EKF)**
IMU 데이터는 확장 칼만 필터(EKF)를 통해 처리될 수 있다. EKF는 비선형 시스템을 다루는 데 적합하며, 센서 데이터를 결합하여 상태를 추정하는 데 사용된다. EKF에서 상태 벡터 $\mathbf{x}$는 다음과 같이 정의된다:
$$
\mathbf{x} = \begin{bmatrix} \mathbf{p} \ \mathbf{v} \ \mathbf{q} \ \boldsymbol{\omega} \end{bmatrix}
$$
여기서:
* $\mathbf{p}$는 위치 벡터이다.
* $\mathbf{v}$는 속도 벡터이다.
* $\mathbf{q}$는 쿼터니언으로 나타낸 자세 벡터이다.
* $\boldsymbol{\omega}$는 각속도 벡터이다.
EKF의 업데이트 단계는 다음과 같이 표현된다:
$$
\mathbf{x}\_{k+1} = f(\mathbf{x}\_k, \mathbf{u}\_k) + \mathbf{w}\_k
$$
여기서:
* $\mathbf{x}\_{k+1}$는 다음 상태 벡터이다.
* $f(\mathbf{x}\_k, \mathbf{u}\_k)$는 상태 변환 함수이다.
* $\mathbf{u}\_k$는 제어 입력 벡터이다.
* $\mathbf{w}\_k$는 시스템 노이즈이다.
관측 벡터 $\mathbf{z}$는 센서 데이터로부터 추정되며, 다음과 같이 표현된다:
$$
\mathbf{z}\_k = h(\mathbf{x}\_k) + \mathbf{v}\_k
$$
여기서:
* $h(\mathbf{x}\_k)$는 관측 모델이다.
* $\mathbf{v}\_k$는 측정 노이즈이다.
EKF의 필터링 과정은 예측과 업데이트 단계로 구성된다. 이를 통해 센서 데이터의 노이즈를 제거하고 보다 정확한 상태 추정을 수행할 수 있다.