BA(Bundle Adjustment)를 LiDAR Mapping에 적용한 BALM의 저자가 2022년에 발표한 논문을 정리한 포스트입니다.
Github에는 BALM 2.0으로 소개되고 있습니다.
앞선 BALM 1.0 (github 상에서는 BALM-old)에 대한 논문을 읽었다고 가정한 포스트입니다.
LiDAR BA 관련 논문에 대한 리뷰는 아래를 참고해주세요.
BALM 1.0 (2021)
Hierarchical LiDAR BA (2023)
본 포스트에서 다룰 논문은 아카이브에만 올라와 있습니다.
1. LiDAR Bundle Adjustment
2. BALM 2.0 Contribution
3. Related Work
A. Multi-view registration
B. Bundle or plane adjustment
4. Bundle Adjustment Formulation and Optimization
1. LiDAR Bundle Adjustment
기존에 주로 사용되는 ICP, GICP, NDT, surfel registration은 두 포인트 클라우드 간의 정합만이 가능하다. 이러한 pairwise registration은 odometry 시스템에서 스캔 정합 과정이 길어져 drift가 축적된다. 또한 3D mapping 이나 multi-lidar calibration에서 반복된 정합 과정으로 연산 비용이 높다. 반면 BA는 효율적으로 여러 스캔을 정합할 수 있다.
라이다 BA는 모든 라이다 포즈들과 scene geometry를 동시에 결정한다. 모든 카메라 포즈들과 3D 포인트 위치를 동시에 결정하는 visual BA와 개념상으로 매우 비슷하다. BA는 vision 에서 매우 주요하게 사용되는 반면 라이다 분야에서는 그렇지 않은데, 라이다에서 BA를 적용하기 어려운 이유는 다음과 같다.
1. long measuring range, but low resolution between scanning lines
측정된 포인트 클라우드는 sparse 하고 반복적이지 않아 동일 featrue point를 여러 스캔에서 찾는 것이 거의 불가능하다. 따라서 고해상도 이미지로부터 각각의 point feature들을 얻을 수 있는 visual BA 공식을 곧바로 적용할 수 없다.
2. large number of raw points (1만~100만개)
라이다 BA에서 라이다의 모든 포인트들을 처리하는 것은 연산량이 매우 크다.
3. 파라미터 수
projection을 계산할때 최적화 해야하는 파라미터가 매우 많은 비전 분야와 달리 라이다 쪽에서는 3D depth가 곧바로 나오기 때문에 비전의 BA보다 메리트가 적다.
2. BALM 2.0 Contribution
기존 BALM 1 에서는 edge, plane feature를 사용해 scene geometry를 표현하고, 각 raw point(edge or plane)에서 매칭되는 geometry feature(edge or plane)까지의 natural Euclidean distance를 직접적으로 최소화 한다. 결정해야 하는 변수는 라이다 포즈들과 feature parameter들인데, geometry parameters(i.e., edge, plane)이 analytically solved 되어 최적화는 오직 라이다 포즈에 대해서만 dependent 하다. geometry feature들의 수가 많을 수 있는데, 최적화에서 이들을 제외함으로써 numerical optimization의 차원을 줄여 시간 비용을 줄일 수 있다.
BALM 2.0은 이러한 BALM 1의 방식을 그대로 사용하고 추가적으로 개선한다.
1. point cluster
최종 최적화 문제를 더 효율적으로 표현하고 풀기 위해 point cluster 를 사용해 동일 feature에 연관된 모든 raw point들을 적은 수의 파라미터들(point cluster coordinates)으로 엔코딩한다.
포인트 클러스터를 사용하면 solver는 최적화의 전 과정(cost evaluation, derivatives evaluation, uncertainty evaluation)에서 각 raw point들의 enumeration을 피할 수 있다. 즉, solver가 수많은 포인트를 개별적으로 살펴보지 않아도 돼서 계산 부담이 크게 줄어든다.
또한, BALM 1의 특성 상 최적화에서 연산 복잡도가 feature의 차원에 독립적이었는데, 포인트 클러스터를 사용하면 포인트의 수에도 독립적이다.
2. Consistent LiDAR pose estimation using 2nd-order BA solver
point cluster coordinates를 기반으로 BA 최적화의 closed-form 편미분 방정식을 라이다 포즈들에 대해 2차까지 유도하고 null spaces 와 sparsity 같은 이론적 성질들을 보여준다. 1에서 언급했듯이, 개별 raw point들에 대한 dependence를 제거하여 cost function 평가와 미분 과정의 속도를 높였다. 이론적인 결과에 기반해, 본 논문은 효율적인 2차 BA solver를 개발하였고 github에서 BALM2.0으로 배포했다.
LiDAR의 nominal 포즈 추정 외에도, solver는 second order derivative 정보를 활용해 measurement noises에서 발생하는 pose uncertainty를 추정한다. 이를 통해 LiDAR pose의 consistent estimation이 가능하다.
3. Experiment 에서 높은 성능
본 논문에서 제안하는 방법은 시뮬레이션 및 실제 환경 모두에서 consistency(일관성), 정확도 및 계산 효율성과 같은 다양한 수준에서 광범위하게 평가된다. 여러 데이터셋(Hilti, NTU-VIRAL, UrbanLoco)과 여러 환경(캠퍼스, 도시의 거리, 사무실, 연구실, 건설 현장), 여러 라이다 타입(Ouster OS0-64, Ouster OS1-16, Velodyne HDL 32E), 여러 모션 타입(handheld, UAV-based, ground vehicles-based)을 포함하는 19개 실환경 오픈 시퀀스를 사용했다. 결과적으로 BALM 2.0이 다른 SOTA 방식들보다 localization 정확도, mapping quality, 계산 효율성 측면에서 일관되고 상당히 높은 성능을 달성함을 알 수 있다. 특히 표준 데스크탑 CPU에서 모든 시퀀스를 30초 이내에 처리하면서 라이다 측정 노이즈 level의(즉, 몇 cm) mapping 정확도를 달성한다. 마지막으로, 제안한 방식이 몇몇 중요한 로보틱 기술(LiDAR-Inertial odometry, multi-LiDAR odometry, multi-LiDAR extrinsic calibration, high-accuracy global mapping)에서 어떻게 효율적으로 정확도와 계산 효율성을 향상하는지 보인다.
3. Related Works
A. Multi-view registration
bundle adjustment problem은 multi-view registration problem과 비슷하다. 둘 모두 two-layer 프레임워크를 사용한다. 첫번째 레이어에서는 ICP 등을 이용해 상대적인 포즈를 추정하고 두번째 레이어에서는 구한 상대적인 포즈를 이용해 모든 라이다 포즈들의 posteriori estimate을 최대화 함으로써 포즈 그래프를 구성한다. 이러한 two-layer 프레임워크는 raw point 정합을 글로벌 포즈 추정으로부터 분리해낸다. 따라서 각 raw point 정합은 두 스캔에 포함된 local point의 일부만(겹치는 부분을 공유하는 모든 스캔이 아닌) 포함하고 포즈 그래프 최적화는 상대적인 포즈들의 일부만(raw point가 아닌) 포함한다.
시간을 크게 절약할 수 있어 LIO-SAM 등의 online LiDAR SLAM 시스템에서 많이 사용된다. 그러나 연산 시간의 장점이 정확도 면에서는 근본적인 제약이 된다. 한 번에 두 스캔 사이의 겹치는 부분만을 고려할 수 있는데 정확한 매핑을 위해서는 여러 스캔에서의 오버랩을 고려해야 한다. 게다가 포즈 그래프 최적화는 상대적인 포즈로부터의 constraint만을 고려하고 raw point로 얻을 수 있는 mapping consistency는 완전시 무시한다. 따라서, 높은 정확도의 localization and mapping에 필수적인 globally consistent map을 생성하는(또는 인식하는 것조차) 것은 어렵다.
컴퓨터 비전과 컴퓨터 그래픽스에서는 3D object의 consistent surface modeling을 위한 multi-view 정합 방법으로 여러 range 이미지들로부터 mapping consistency를 직접적으로 최적화 하는 방법을 사용했다. ICP 방식을 확장하여 '한 스캔의 control point'와 매칭되는 '다른 스캔의 control point' 사이의 Euclidean 거리를 최소화하는 방법을 사용하거나, 이를 더 확장하여 'control point'와 '개별 control point 주변의 surface' 사이의 거리를 최적화 한다. 최근에는 먼저 모든 스캔의 클러스터 포인트들에 대해 K-means clustering을 한 후 '클러스터의 각 포인트'에서 'centroid'까지의 유클리디언 거리를 최소화 하여 스캔 포즈를 추정하는 방법이 제안되었다. 이러한 방법들은 스캔 안의 point feature에 달렸기 때문에, 두드러진 point feature를 추출하기 위해서는 densely populated point cloud여야 한다. small object reconstruction에서는 이러한 방법이 문제 없지만, LiDAR measurement는 매우 sparse 하고 repetitive 하지 않기 때문에 scene reconstruction 에는 적절한 방법이 아니다.
B. Bundle or plane adjustment
Kaess는 BA에 plane features을 활용하고 스캔에서 측정된 plane과 최적화 변수(scan poses, plane parameters)로부터 예측된 plane 간의 차이를 최소화 한다. 이는 key-frame-based online SLAM 시스템에 통합되었다. 이 방법은 plane-to-plane 거리를 최소화하므로 각 스캔을 분할(segment)하고 포함된 local plane을 미리 추정해야 한다. 이러한 plane 분할 및 추정에는 일반적으로 RGB-D 카메라로 측정한 dense point clouds 가 필요하다.
라이다 포인트 클라우드에 대한 보다 공식적인 BA 방법인 plannar (bundle) adjustment은 '스캔의 각 포인트'에서 '최적화 변수(scan poses, plane parameters)로부터 예측된 plane'까지의 natural Euclidean distance 를 최소화 한다. Kaess의 plane-to-plane 와 비교하여, point-to-plane metric은 더 빠르고 정확하고 라이다 센서에 적절하다(라이다의 local plane segmentation 또는 추정은 sparse 포인트 클라우드로 인해 신뢰성이 떨어진다). 또한 point-to-plane metric 에서 raw point를 직접 사용하면 raw point의 measurement noise를 고려할 수 있어 최적화 변수를 보다 일관되게 추정할 수 있다. 그후 LM(Levenberg-Marquardt) 알고리즘을 사용해 비선형 least wquare 문제를 푼다. 동일 plane feature에 연관된 measurement point 수가 많으므로 연산량이 크기 때문에 이를 줄이기 위해 residual과 Jacobian의 evaluation에서 개별 포인트의 enumeration을 제거한다. 게다가 visual BA의 구조와 비슷하기 때문에 LM 알고리즘의 각 iteration에서 plane 파라미터를 제거하는 Schur complement 과 호환된다. 이 plane adjustment 방법은 많은 온라인 라이다 SLAM 시스템에서 사용된다.
반면 Ferrer는 plannar (bundle) adjustment와 유사하게 plane feature를 활용하고 plane equation에서 각 raw point의 편차(deviation)를 최소화한다. 결과적인 최적화 비용은 공분산 행렬의 최소 고유값으로 감소하므로 고유 인자(eigen-factor)라고 한다. 저자는 비용 함수(scan poses, plane parameters)의 closed-form gradient를 추가로 도출한다. gradient-based 방법을 사용하여 최적화를 반복적으로 푼다. 고유값의 second order 특성으로 인해 gradient 방법은 매우 느리게 수렴한다(수백 번의 반복 필요).
BALM1은 더 효율적인 BA를 구현했다.
BALM1 장점은 다음과 같다.
1. feature elimination
최적화 전에 closed form으로 analytically solved 될 수 있어 결과 최적화에서 많은 plane parameter를 제거할 수 있다. 이를 통해 최적화 차원을 낮출 수 있고, 이전의 plannar adjustment, visual bundle adjustment 방법들과 차별화 된다.
feature elimination은 또한 최적화에서 plane 표현에서 발생하는 다양한 문제(i.e., Hesse normal representation $(n, d)$ 에서의 normal constraint, closest-point (CP) representation $nd$ 에서의 singularity 문제, 쿼터니언 표현에서의 over-parameterization 문제)들을 제거한다.
2. plane feature와 더불어 edge feature도 사용한다.
BALM1 단점은 다음과 같다.
자코비안과 헤시안을 포함해서 second-order derivatives 의 evaluation이 개별 라이다 포인트를 enumerate 해야 해서 계산 복잡도가 $O(N^2)$이다. $N$은 포인트의 수이다. 따라서 포인트 수가 많은 large-scale problem에서는 사용하기 힘들다. 실제로 BALM 1을 사용해봤을 때 KITTI 데이터셋에서 한번씩 mapping이 안 되는 부분이 있었는데 이와 같은 문제가 아닐까 싶다.
BALM 2.0은 BALM 1을 기반으로 하되, 근본적인 feature elimination 을 장점으로 한다.
4. Bundle Adjustment Formulation and Optimization
