Voxblox: Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning
Micro Aerial Vehicles (MAVs) that operate in unstructured, unexplored environments require fast and flexible local planning, which can replan when new parts of the map are explored. Trajectory optimization methods fulfill these needs, but require obstacle
arxiv.org
이해가 완벽하지 않아 부족한 점이 많습니다. 지적해주시면 감사하겠습니다 :)
ESDFs: Euclidean Signed Distance Fields
TSDFs: Truncated Signed Distance Fields
[Introduction]
Map을 3차원으로 표현할 때에 일반적으로 사용되는 것이 occupancy grid map, octomap 으로 occupancy probabilty를 저장하기 위해 계층화된 octree 구조를 사용합니다. 그러나 path planning을 하기 위해서는 장애물까지의 거리가 필요한데 occupancy probabilty로는 정보가 충분하지 않겠죠.
이러한 경우에 ESDF 방식을 사용할 수 있습니다. ESDF는 이름에서 알 수 있듯이 Euclidean Signed Distacne를 저장하고 있으니 거리를 계산하기에 적합합니다.
TSDF는 조금 더 조밀한 construction에서 surface reconstruction을 위해서 사용합니다. TSDF는 데이터 구조 구성 방식으로 인해 GPU 컴퓨팅에 적합합니다. 그러나 TSDF의 가장 큰 단점은 fixed-size voxel grid가 필요하다는 것입니다. 곧 이 방법은 map size를 알아야 한다는 것이 되고 그만큼 큰 memory overhead가 생기게 됩니다.
ESDF를 구축하기 위한 일반적인 단계는 다음과 같습니다.
- Complete TSDF를 구축합니다.
- TSDF를 Occupancy grid로 변환합니다.
- Batch method를 사용하여 ESDF를 계산합니다.
그러나 voxblox는 TSDF에서 ESDF로 직접 구축할 수 있는 방법을 제시합니다.
다음은 voxblox의 주요 contributions입니다.
- 동적으로 형성되는 map에서 TSDF를 이용해 ESDF를 점진적으로 구축하는 방법을 제시
- TSDF를 구축하는 다양한 방법을 분석하고, Size가 큰 Voxel로 reconstruction speed와 surface accuracy를 최대화
- Final ESDF의 오류 분석과 실험 분석을 제공하고, 오류를 해결하기 위한 Safety margin을 제안
- MAV에 탑재된 map을 사용하여 online replanning을 수행함으로써 전체 시스템 검증
Multi map layer(TSDF, ESDF 및 Mesh)가 Integrator를 통해 들어오는 센서 데이터와 상호 작용하는 방법을 보여주는 Voxblox System Diagram
Main Process
- Sensor data(RGBD, Point Cloud)를 TSDF로 변환
- TSDF에 따라 propagate를 이용하여 ESDF 업데이트
[TSDF Construction]
TSDF를 구축하는 데에 주요 요소는 Weighting과 Merging이 있습니다. 새로운 Scan에 대한 정보가 TSDF에 통합되면 raycast를 사용하여 센서의 원점에서 각 지점까지의 raycast를 계산하고 광선 방향으로 distance와 weight를 계산합니다. 여기서 weighting function의 선택은 resulting reconstruction의 정확도에 영향을 미칠 수 있고, 특히 Scan당 수천 개의 point가 동일한 voxel로 병합될 수 있는 큰 voxel의 경우 더욱 큰 영향을 미칠 수 있습니다.
- Weighting
현재 voxel의 중심을 \( x \)라고 가정했을 때, \( p \)를 sensor data에서의 3D point의 위치, \( s \)는 sensor origin이라고 하겠습니다. 이때 \( x, p, s \in \mathbb{R}^3 \)입니다. 그리고 특정 point에서 관찰을 통해서 얻은 distance값을 \( d \), weight값을 \( w \)라고 했을 때, 기존의 distance값 \( D \)와 weight값 \( W \)의 update는 다음과 같습니다.
- \( d(x, p, s) = ||p-x||sign((p-x)\cdot(p-s)) \)
- \( w_{const}(x, p) = 1 \)
- \( D_{i+1}(x, p)={W_i(x)D_i(x)+w(x,p)d(x,p) \over W_i(x)+w(x,p)} \)
- \( W_{i+1}(x, p)=min(W_i(x)+w(x,p), W_{max}) \)
Voxblox는 위의 \( w_{const} \)를 \( w_{quad} \)로 변환하기 위한 계산을 합니다.
- \( w_{quad}(x, p)= \begin{cases} 1/z^2 & -\epsilon < d \\ (1/z^2) \cdot (1/(\delta-\epsilon)) \cdot (d+\delta) & -\delta<d<-\epsilon \\ 0 & \ \ \ \ \ \ \ \ \ \ d<-\delta \end{cases} \)
저자들은 \( \delta=4v \) and \( \epsilon = v \), 그리고 \( v \)를 voxel size로 사용했다고 합니다.
- Merging
저자는 computation 속도를 높이기 위해서 larger voxel size를 사용합니다.
Merge는 다음 3가지 과정을 통해 진행됩니다.
- Scan으로 얻은 각 point의 position을 voxel grid로 project 합니다.
- 동일한 voxel에 속하는 point를 그룹화합니다.
- 평균 color와 distance를 계산하고 raycasting을 mean position에서 한번 계산합니다.
[Constructing ESDF from TSDF]
- Construction
저자는 가장 가까운 occupied voxel로부터의 거리를 계산하는 대신 TSDF에 distance를 저장하고 이를 사용하였습니다.
Original implementation에서는 각 voxel은 변경할 수 없는 occupied or free의 상태를 가지지만, voxblox는 이 개념을 fixed band around the surface로 변경합니다.
ESDF의 voxel은 같은 위치에 있는 TSDF의 voxel에서 값을 추출하고, fixed band는 ESDF의 voxel에 의해서 결정됩니다.
거리는 wavefront propagation을 사용하여 업데이트되는데, voxblox는 두 가지 wavefront queue를 사용합니다.
- Raise: voxel은 TSDF로부터의 새로운 거리 값이 ESDF voxel에 저장된 이전 값보다 높을 때 raise queue에 추가됩니다.
- 즉, voxel과 모든 children이 invalidated 되어야 합니다. 이때 wavefront propgates는 더 이상 invalidated 된 parent voxel이 남지 않을 때까지 전파됩니다.
- Lower: 새로운 fixed voxel이 map에 들어오거나 이전에 관찰된 voxel이 값이 낮을 때 lower queue에 추가됩니다. 인접한 voxel의 거리는 인접한 voxel과 현재 voxel까지의 거리를 기준으로 업데이트됩니다.
- wavefront는 이웃으로부터 거리가 줄어들 수 있는 voxel이 남아있지 않을 때 끝이 납니다.
- Propagate Algorithm
- Other functions
