Velodyne LiDAR를 살짝 기울여 보자

해당 포스팅은 VLP16을 물리적으로 기울인 후, velodyne_driver에서 pointcloud 각도를 보정하는 방법을 포함하고 있습니다.

[환경]
ros: ros2 foxy ~
os: ubuntu 20.04 ~

0. 왜 기울여야 하지?

16채널 LiDAR를 사용할 때 카메라와 달리 데이터가 상대적으로 제한적으로 느껴지는 경우가 있었다.

이미지 같은 경우에는 해상도(640x480)에 맞게 촘촘하게 데이터를 가지고 있는 반면, LiDAR의 포인트클라우드는 채널에 해당하는 정보만을 가지고 있어 희소하게 느껴졌다.  그래서 내 기준 LiDAR의 부족한 정보를 보완하고자 여러 방법을 찾아보다 다음 2가지 방법을 시도해 보았다.

 

1. LiDAR 보간 (아직 기울이기 아님)

먼저 LiDAR 보간이란, 간단하게 말해서 수집된 포인트클라우드를 기반으로 채널 사이사이를 잘 예측해서 메꿔주는 방식이다.

깃허브에서 ros2로 작성된 보간 코드를 찾아봤지만 없는 것 같아서 친구랑 같이 ros1 코드를 ros2로 바꾸고 실험해 보았다. 동작은 잘 됐지만 뭔가 처리 속도가 느려서 패스하고 다른 방법을 찾아보았다.

 

[결과]

보간 전(좌측) 보간 후(우측)

 

아래는 참고한 코드이다.

https://github.com/EPVelasco/lidar-camera-fusion

GitHub - EPVelasco/lidar-camera-fusion: The code implemented in ROS projects a point cloud obtained by a Velodyne VLP16 3D-Lidar

 

2. 기울이기 첫 번째 시도 (데이터를 sub한 다음 수정)

몇 번의 보간 방식을 시도한 이후, 쿨하게 포기하고 LiDAR를 우리가 찾고자 하는 물체에 맞게 살짝 기울이는 방식을 시도하였다.

기울이는 방식을 채택한 이유는 우리가 뭔가 쉽게 시도해 볼 수 있을 거라 생각했고, 다른 프로젝트에서도 기울여서 하는 경우가 있어 시도해 보게 되었다. 그리고 검색하다가 라이다에 거울 같은 걸 달아서 반사시키는 재밌는 방식도 보았다. 

거울을 활용한 방법

 

다음 사이트에서 자세한 내용을 확인할 수 있다.

https://velodynelidar.com/blog/how-to-change-the-laser-angle-and-fov-on-a-velodyne-vlp-16/

How to Change the Laser Angle and FoV on a Velodyne VLP-16 | Velodyne Lidar

 

그럼 본격적으로 어떻게 기울였는지 알아보자면, /velodyne_points로 부터 받은 데이터를 다음 코드를 통해 각도를 보정하였다.

// 각도를 라디안으로
float degree = -15.0; //각도
float radian = degree * M_PI / 180.0;
float rotation_angle = radian;

// 회전 변환 행렬 수정 y축
Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
transform(0, 0) = cos(rotation_angle);
transform(0, 2) = sin(rotation_angle);
transform(2, 0) = -sin(rotation_angle);
transform(2, 2) = cos(rotation_angle);

// 포인트 클라우드 회전
pcl::transformPointCloud(*cloud, *cloud_filtered, transform);

 

하지만 보간 방식과 마찬가지로 모든 포인트클라우드에 각도를 보정하다 보니 연산량이 많아지며, 처리 속도가 느려졌고, pcl라이브러리의 VoxelGrid, CropBox 등을 이용해 최대한 데이터를 줄여 사용해 보았지만, 우리 기준에 적합하지 않아 또 다른 방법을 찾아보게 되었다.

 

 

3. 기울이기 두 번째 시도 (velodyne driver에서 수정)

2가지 방식의 실패를 겪고 우리는 포인트클라우드가 어떻게 우리한테 도달하는지 한번 고민해 보게 되었다. rqt_graph를 통해 다음과 같이 노드 간 연결되는 것을 확인했고, 저 중 어딘가에 각도를 보정하면 되겠다는 결론을 내리게 되었다.

rqt_graph로 본 velodyne_driver

 

그렇게 친구와 velodyne_driver를 분석한 결과 다음 위치에서 최종적으로 각도를 정해준다는 사실을 확인하였고 해당 부분에 2번째 시도에서 사용하였던 코드를 활용하여 각도를 보정한 결과 딜레이 없이 사용할 수 있었다.

사용한 패키지의 링크는 다음과 같고

https://github.com/ros-drivers/velodyne/tree/master

GitHub - ros-drivers/velodyne: ROS support for Velodyne 3D LIDARs

 

방법은 다음과 같다.

 

[Step1]

velodyne 폴더 내, velodyne_pointcloud/src/lib/rawdata.cpp에 존재하는 함수 unpack_vlp16을 찾아

790번째 줄 근처 존재하는 아래 부분에 이어

/** Use standard ROS coordinate system (right-hand rule) */
float x_coord = y;
float y_coord = -x;
float z_coord = z;

 

[Step2]

step1 코드 이후에 다음 코드를 추가하면 된다. (14도 기울인다고 가정)

//각도를 라디안으로
float degree = 14.0; // 각도
float radian = degree * M_PI / 180.0;
float rotation_angle = radian;

float temp_x = x_coord;
float temp_y = y_coord;
float temp_z = z_coord;

x_coord = temp_x * cos(rotation_angle) + temp_z * sin(rotation_angle);
y_coord = temp_y;
z_coord = -temp_x * sin(rotation_angle) + temp_z * cos(rotation_angle);

 

4. 결론

이때부터였는지 모르겠다. 이때 이후로 라이브러리며, 패키지를 살펴보는 게 두렵지 않았고, 무작정 한 번씩은 열어봤던 것 같다.

이 같은 경험이 이후 많은 도움을 주었다. 한 예로, 23년도 말 교내 라인트레이싱 대회에서 준 제어 라이브러리가 실시간성이 떨어져 사용하기 힘들었는데, 라이브러리를 분석하여 최소한 필요한 기능만 떼내어(?) 제어했던 재밌는 경험이 있다.