• 생성자와 addFreespaceFootprint 메서드가 중요

1. 코드 설명

• 이 코드는 로봇의 발자국(freespace footprint)을 3D 볼류메트릭 맵(Volumetric Map)에서 처리하는 RobotFootprintIntegrator 클래스
• 주요 목적은 로봇의 위치를 기반으로 로봇이 이동한 공간(자유 공간)을 3D 맵에서 TSDF(Truncated Signed Distance Field) 값으로 업데이트하고, 자유 공간 영역을 마킹하는 역할을 수행합니다.

알고리즘 요약

• Bounding Box 계산:
  • 로봇의 발자국이 차지하는 공간을 3D 바운딩 박스로 표현하고, TSDF 블록 단위로 확장하여 로봇이 이동한 공간을 추적합니다.
• TSDF 업데이트:
  • 로봇의 현재 위치와 이동 경로에 해당하는 TSDF 블록들을 찾아 자유 공간으로 마킹하며, 각 보셀의 TSDF 값을 조정
• 결과 관리:
  • 업데이트된 블록을 TSDF 및 메쉬 레이어에 반영하고, 필요 없는 블록을 제거

결론

이 코드의 목적은 로봇이 이동한 경로를 자유 공간으로 인식하고, 3D 맵에서 그 공간을 TSDF로 표현하여, 이후 다른 로봇이나 시스템이 해당 공간을 자유 공간으로 인식할 수 있도록 하는 것입니다. 이를 통해 로봇의 움직임을 실시간으로 반영한 환경 인식 및 경로 계획이 가능해집니다.

구조와 역할

2. RobotFootprintIntegrator 생성자

이 생성자는 config 객체를 받아서 해당 설정을 사용해 클래스의 인스턴스를 초기화합니다. 로봇의 발자국을 나타내는 Bounding Box가 bbox_min과 bbox_max를 사용해 설정됩니다.

RobotFootprintIntegrator::RobotFootprintIntegrator(const Config& config)
    : config(config), bbox(config.bbox_min, config.bbox_max) {}

• bbox: 로봇의 바운딩 박스는 로봇이 차지하는 공간을 정의하며, 이후 로봇이 이동한 공간을 자유 공간으로 마킹할 때 사용됩니다.

4. addFreespaceFootprint 함수

• 이 함수는 로봇이 이동한 공간을 자유 공간(freespace)으로 처리하여 TSDF 값을 업데이트하는 역할을 합니다.
• 즉, 로봇의 바운딩 박스 내부에 해당하는 TSDF 블록의 각 보셀(voxel)에 대해 자유 공간으로 마킹하고, TSDF 값을 변경하는 것이 목표입니다.

주요 동작

• 로봇의 위치에 영향을 받는 블록 찾기: getAffectedBlocks 함수를 사용하여 로봇의 위치와 바운딩 박스 내부에 해당하는 TSDF 블록들을 찾습니다.
• 블록 업데이트:
  • 각 블록 내의 보셀에 대해, 자유 공간에 해당하는 보셀은 TSDF 값을 최대 절단 거리(truncation distance)로 설정하고, 가중치도 업데이트합니다.
  • voxel.distance = map.config.truncation_distance: 해당 보셀의 거리 값을 자유 공간에 맞게 설정합니다.
  • voxel.weight = config.tsdf_weight: 자유 공간을 반영한 TSDF 값에 가중치를 부여합니다.
• 새 블록 처리:
  • 만약 새로운 블록이 할당되었으면, 이 블록을 TSDF와 함께 할당하고 업데이트합니다.
  • 새롭게 추가된 블록은 이후 ESDF 및 메쉬로 업데이트되지 않으면 제거됩니다.

1. declare_config 함수

이 함수는 RobotFootprintIntegrator 클래스의 설정(Config)을 정의하고 초기화하는 역할을 합니다. 이 설정 값들은 로봇의 발자국을 처리하는 데 필요한 매개변수들로 구성되어 있습니다.

설정 항목

• bbox_min: 로봇의 최소 바운딩 박스(Bounding Box) 좌표.
• bbox_max: 로봇의 최대 바운딩 박스 좌표.
• tsdf_weight: TSDF 업데이트 시 사용할 가중치.

3. getAffectedBlocks 함수

• 이 함수는 로봇의 현재 위치와 바운딩 박스를 기준으로 TSDF 맵의 어떤 블록들이 영향을 받는지 계산합니다.
• 즉, 로봇의 위치에서 영향을 받는 공간의 3D 좌표를 기준으로 TSDF에 반영해야 할 블록들의 인덱스를 반환하는 역할을 합니다.

주요 동작

• 바운딩 박스 확장: TSDF에서 로봇의 이동 영역에 해당하는 공간을 처리하기 위해, 확장된 바운딩 박스를 사용해 로봇이 차지할 수 있는 공간을 더 넓게 잡습니다.
  • inflated.dimensions: 바운딩 박스를 TSDF 블록의 크기만큼 확장합니다. 이는 블록의 대각선 길이를 기준으로 계산됩니다.
• 3D 공간 탐색: 로봇의 중심 좌표를 기준으로 TSDF에서 영향을 받는 블록을 찾아냅니다.
  • max_steps: 로봇이 이동할 수 있는 최대 범위에 따라 TSDF 블록의 개수를 결정하고, 이를 기반으로 로봇의 바운딩 박스가 차지하는 공간에 해당하는 블록들을 추출합니다.

최종 결과

• 로봇이 이동할 수 있는 공간 내의 TSDF 블록들의 인덱스 리스트를 반환합니다.