Dynamic Detection
Seoul ADEX 2023 Project
교수님께서 서울공항에서 진행하는 Seoul ADEX 2023에서 LiDAR로 동적 객체 감지 구현을 제안하셨다. Jetson Orin NX에서 실시간 동작을 최종 목표로 다음과 같이 작업을 진행하였다.
- 데스크탑에서 작업한 코드를 깃허브에 push
- NX에서 jetson-containers(Jetson용 ROS Noetic을 제공)에 기반하여 컨테이너 빌드
- 소스 코드 pull
RS LiDAR SDK
연구실의 LiDAR는 반구 형태의 32채널 RoboSense-Bpearl이다.
수직 FoV는 지면에서 90까지이다. 수직 방향으로 32개의 레이저가 존재하며 한번에 발사된다. 수직 방향의 레이저 간의 각도는 90 / 32 = 2.8이다. 수평 FoV는 360이고 한 바퀴에 1800회 가량 발사된다. 수평 방향의 레이저 간의 각도는 360 / 1800 = 0.2이다. 돌면서 발사 횟수에 오차가 있는데 프레임 비교를 위해서는 오차를 없애는 작업이 필요하다.
결론적으로 한 바퀴 도는 동안 즉, 한 프레임 당 대략 32\(\times\)1800 = 57600개의 포인트가 이진 데이터로 날아온다.
C++로 구성된 SDK인 /rslidar_sdk_node에서 /rslidar_points라는 pcd를 담은 토픽을 publish하면 그걸 /filter_node로 subscribe해서 pcd를 필터링하는 방식으로 진행하기로 결정했다. /filter_node를 거치기 전(/rslidar_points)과 후(/filter_points)의 토픽을 RViz로 출력하여 필터링 전후를 확인 가능하게 하기로 계획하였다.
일단 테스트 삼아 /rslidar_points를 z축으로 10배 늘린 후 출력해보았다.
Encoding Decoding Optimization
토픽을 /filter_node를 통해 받아야 하는데 데이터가 이진화 형태라서 토픽 구조에 대한 이해가 필요했다. 데이터를 전달받아서 float32 타입으로 디코딩을 해주려면 4바이트 단위로 끊어서 읽어야했다. 토픽 메시지인 PointCloud2의 fields는 x, y, z, intensity로 구성되어 있고 이 순서로 이진화되어 있다. 또한 height, width, point step, row step 등의 정보가 담겨 있어 수식적 상관관계와 의미를 파악해보았다.
\(height = \displaystyle\frac{data}{point_{step} \times width} = \frac{data}{row_{step}}\)
- \(point_{step}\)은 16으로 fields의 메모리 공간을 의미
- \(width\)는 32로 채널 수를 의미
- \(row_{step}\)은 \(point_{step} \times width\)로 한 번에 수직 방향으로 발사되는 레이저
- \(height\)는 1800+-n으로 회전 당 발사 횟수
처음에 struct의 unpack를 이용해서 x, y, z, i를 바이트 단위로 끊어서 디코딩하고 인코딩 역시 따로 진행 후 pack하여 마지막에 extend로 이진화 시켰다.
Nvidia Orin NX로 컨테이너화 시켜서 넣으니까 연산이 더 느리다. 손을 흔들면 0.5초 가량 후에 반응이 온다.
여러가지 시도 끝에 결국에는 넘파이를 사용하여 해결을 보았다. 디코딩은 넘파이의 frombuffer를 이용하여 불러온 후 복사하여 reshape으로 (-1, 4) 형태로 바꾸었고, 인코딩은 넘파이 ravel을 이용하여 x, y, z, i, x, y .. 형태로 쫙 편 후에 이것을 전치시켜 tobytes로 이진화시켰다. 인코딩과 디코딩 개선과정에서 속도가 빨라지는 것이 확연히 나타났다. Callback함수 실행 시간이 0.13초에서 디코딩 최적화 후 0.07초로 줄었고 인코딩 최적화 후 0.001초까지 줄어들었다.
Frame Differencing
프레임간 지연 현상은 어느 정도 해소가 되었으니 프레임 간의 차를 이용해서 변화량이 많은 포인트만을 출력시켜보기로 했다. 현재 프레임과 이전 프레임의 포인트 클라우드끼리 유클리디안 거리를 구한 후 threshold를 넘는 것만 출력하도록 간단히 구성하였다.
연구실 내부가 직사각형으로 된 작은 방이라 그런지 결과가 잘나왔다. 그렇게 별 신경 안쓰고 있다가 교수님께서 시연 보이라고 하셔서 방 밖으로 가지고 나갔는데 노이즈가 너무 많이 발생해서 당황스러웠다. 방 안은 벽이 매끄러워서 오차가 발생해도 문제가 없었는데 방 밖은 그게 아니었다.
문제점
- 방 안에서와 달리 객체가 많으면 회전 당 수평 방향 오차가 발생하여 노이즈가 급증
- theshold를 높였을 때 오차가 간헐적으로 발생하며 출력이 깜빡거리는 현상 발생
개선방향
- Average Pooling 적용
Average Pooling
교수님께서 노이즈 해결방안으로 Average Pooling을 제안해 주셨다.
LiDAR의 형태 특성 상 32개의 레이저는 물리적으로 붙어서 한번에 발사되기 때문에 수직 방향으로 데이터가 엇갈리진 않을 것이다. 따라서 수평 방향의 엇갈림에 대한 average pooling만 진행하기로 결정하였다.
디코딩된 포인트 클라우드는 행이 포인트이고 열이 fields인 (56700, 4) 형태이다. 수평 방향끼리의 연산을 위해 transpose와 reshape을 이용해 (1800, 32, 4) 형태로 변경하였다. 행은 수평 방향 발사 횟수로 매 프레임마다 약간의 오차가 있지만 편의상 1800으로 두었다. 필터 윈도우의 크기를 kernel로 정하였고, kernel이 2일때 채널별 각 행끼리 평균값을 구하면 (900, 32, 4) 형태로 변경된다. 여기서 넘파이의 nanmean를 사용하였는데 포인트들 사이에 존재하는 nan값을 대체하여 노이즈 출력을 방지하기 위함이다. 예를 들어 6, 8의 평균은 7이지만 6, nan의 값은 6으로 연산한다.
거리를 구하기 전에 이전 프레임과 형태를 맞추기 위한 처리를 진행하였다. 이전 프레임보다 행이 작을 경우 이전 프레임의 행을 현재 프레임의 행에 맞게 자르고 클 경우 이전 프레임의 부족한 행만큼 nan값으로 채웠다.
그 후 fields 중 x, y, z값만 이전 프레임에서 구한 평균과 유클리디안 거리를 구하면 (900, 32) 형태로 변경된다. 그리고 repeat과 tile을 이용하여 (1800, 32, 4)로 변경 후 where을 이용해서 theshold가 넘는 값만 필터링 후 (54700, 4) 형태로 변경하여 반환하였다.
문제점
- 형태를 맞추는 과정에서 마지막 1~2행이 nan값이 되어 필터링에서 제외됨
개선방향
- 거리를 구하기 전 마지막 1~2행을 nan으로 설정하여 출력에서 제외
Average pooling 후 노이즈가 약간씩 발생하긴 하지만 동적 객체만 필터링하는 것을 확인하였다. 최종 Callback 연산 시간은 평균 0.003초이다.
