# Deskewing in Point Cloud Processing: C++ Implementation

#### Deskewing의 개요

Deskewing은 주로 LiDAR와 같은 센서로부터 수집된 포인트 클라우드 데이터에서 발생하는 왜곡(skew)을 보정하는 과정이다. 이 왜곡은 주로 센서의 움직임이나 회전 속도에 따라 발생하며, 시간 차이로 인해 각 포인트가 비정확한 위치에 기록되는 문제를 초래한다. 특히, 자율주행 자동차나 로봇과 같이 실시간으로 움직이며 데이터를 수집하는 시스템에서는 이러한 왜곡을 제거하는 것이 매우 중요하다. Deskewing 알고리즘은 이 왜곡을 보정하여 포인트 클라우드 데이터를 정확하게 정렬된 상태로 만드는 역할을 한다.

#### 포인트 클라우드의 시간적 비동기화 문제

LiDAR 센서는 회전하면서 포인트 클라우드를 생성하므로, 각 포인트는 서로 다른 시간에 캡처된다. 만약 센서가 이동 중이라면, 시간 차이에 따라 포인트가 캡처되는 위치가 다르게 나타나게 된다. 이로 인해 정적(Static) 환경에서는 문제가 적을 수 있지만, 동적(Dynamic) 환경에서는 심각한 왜곡이 발생할 수 있다. 이러한 문제를 해결하기 위해서는 각 포인트가 캡처된 시간과 센서의 위치를 정확히 계산하여 포인트의 위치를 재정렬해야 한다.

#### Deskewing의 수학적 모델링

Deskewing의 핵심은 각 포인트의 정확한 위치를 계산하는 것이다. 이를 위해서는 다음과 같은 수학적 모델링이 필요하다.

1. **시간 보정(Time Correction)**: 각 포인트가 캡처된 시간을 기반으로 센서의 이동을 보정한다. 이를 위해 포인트 $ P\_i $의 시간 $ t\_i $와 센서의 위치 및 회전 각도를 고려하여 보정된 위치 $ P\_i' $를 계산한다.
2. **좌표 변환(Coordinate Transformation)**: 보정된 시간에 따른 센서의 위치와 자세(orientation)를 기반으로, 포인트 클라우드의 원래 좌표를 새 좌표계로 변환한다.
3. **보간(Interpolation)**: 센서의 위치와 자세는 연속적으로 변하기 때문에, 연속적인 시간 사이에 존재하는 포인트들은 보간을 통해 정확한 위치로 이동해야 한다.

#### C++에서의 Deskewing 구현

C++에서 Deskewing 알고리즘을 구현하는 과정은 매우 세심한 계산과 최적화가 필요하다. 이 과정에서는 주로 고성능 컴퓨팅을 염두에 두고 C++의 장점을 최대한 활용한다.

**시간 보정 알고리즘**

C++에서는 각 포인트의 타임스탬프를 기준으로 보정된 위치를 계산할 수 있다. 이를 위해, 다음과 같은 순서로 구현이 이루어진다.

* **타임스탬프 읽기**: 포인트 클라우드 데이터의 각 포인트에는 타임스탬프가 포함되어 있다. 이 타임스탬프를 읽어들여 저장한다.

  ```cpp
  double pointTime = point.timestamp;
  ```
* **센서 위치 및 자세 계산**: 주어진 시간에서 센서의 위치와 자세를 계산한다. 이 계산은 주로 선형 보간(linear interpolation)이나, 더 정확한 경우 쿼터니언(Quaternion)을 사용한 회전 보간을 통해 이루어진다.

  ```cpp
  Eigen::Vector3d correctedPosition = interpolatePosition(sensorStartPos, sensorEndPos, pointTime);
  Eigen::Quaterniond correctedOrientation = interpolateOrientation(sensorStartOrientation, sensorEndOrientation, pointTime);
  ```
* **포인트 위치 보정**: 계산된 센서의 위치와 자세를 기반으로, 각 포인트의 원래 좌표를 변환한다.

  ```cpp
  Eigen::Vector3d correctedPoint = correctedOrientation * point.position + correctedPosition;
  ```

**좌표 변환과 보간**

포인트 클라우드의 좌표를 변환하는 과정은 센서의 움직임에 따라 달라진다. 이는 보간된 센서의 자세와 위치를 활용하여 각 포인트의 최종 좌표를 계산하는 단계로 이어진다.

* **선형 보간**: 포인트가 기록된 시간에 따라 센서의 위치를 선형적으로 보간한다.

  ```cpp
  Eigen::Vector3d interpolatePosition(const Eigen::Vector3d& startPos, const Eigen::Vector3d& endPos, double alpha) {
      return startPos + alpha * (endPos - startPos);
  }
  ```
* **쿼터니언을 이용한 회전 보간**: 센서의 회전은 쿼터니언을 사용하여 보간한다. 이 방식은 회전의 지속성을 유지하는 데 유리한다.

  ```cpp
  Eigen::Quaterniond interpolateOrientation(const Eigen::Quaterniond& startOrientation, const Eigen::Quaterniond& endOrientation, double alpha) {
      return startOrientation.slerp(alpha, endOrientation);
  }
  ```

#### C++ 최적화 기법

C++에서 Deskewing을 구현할 때 성능 최적화는 필수적이다. 특히 대규모 포인트 클라우드를 처리할 때, 다음과 같은 최적화 기법을 적용할 수 있다.

* **병렬 처리**: OpenMP나 C++11의 스레드를 이용하여 포인트 클라우드의 각 포인트를 병렬로 처리하여 속도를 향상시킬 수 있다.

  ```cpp
  #pragma omp parallel for
  for (int i = 0; i < pointCloud.size(); ++i) {
      processPoint(pointCloud[i]);
  }
  ```
* **Eigen 라이브러리 사용**: Eigen은 벡터 및 행렬 연산을 효율적으로 수행할 수 있는 C++ 라이브러리로, Deskewing 과정에서 자주 사용되는 수학적 연산을 최적화할 수 있다.
* **메모리 관리 최적화**: 불필요한 메모리 할당을 최소화하고, 메모리 접근 패턴을 최적화하여 캐시 히트를 증가시키는 방식으로 성능을 개선할 수 있다.

  ```cpp
  std::vector<Eigen::Vector3d> correctedPoints;
  correctedPoints.reserve(pointCloud.size());
  ```

***

관련 자료:

* Zhang, Ji, and Sanjiv Singh. LOAM: Lidar Odometry and Mapping in Real-time. *Robotics: Science and Systems* (2014).
* Pomerleau, François, et al. Comparing ICP variants on real-world data sets. *Autonomous Robots* 34.3 (2013): 133-148.
* Engel, Jakob, Vladyslav Usenko, and Daniel Cremers. A photometrically calibrated benchmark for monocular visual odometry. *arXiv preprint arXiv:1607.02555* (2016).