[Reconstruction] hydra code - src/reconstruction/projective_integrator.cpp

About_work·2024년 10월 14일

lifelong scene graph

목록 보기

6/56

1. 생성자와 updateMap

요약:

ProjectiveIntegrator 클래스는 3D 맵을 업데이트하는 데 사용
센서로부터 받은 데이터를 기반으로,
- 특정 시야 내에서 블록을 선택하고
- TSDF 및 의미론적 데이터를 반영하여 맵을 갱신

1. `ProjectiveIntegrator::ProjectiveIntegrator` 생성자

구성요소 설명:

interpolator_(config::create<ProjectionInterpolator>(config.interp_method)):
- interpolator_는 ProjectionInterpolator 객체로,
  - 주어진 config.interp_method에 따라 생성됩니다. (기본값: adaptive)
- 이 객체는 주어진 좌표(예: 센서로부터 포인트까지의 거리)에서의 값을 추정(보간)하는 데 사용
semantic_integrator_(config.semantic_integrator.create()):
- semantic_integrator_는 semantic_integrator 객체를 설정값에 따라 생성

2. `ProjectiveIntegrator::updateMap` 함수

이 함수는 3D 맵(VolumetricMap)을 업데이트하는 역할을 하며,
입력 데이터(InputData)를 기반으로 TSDF(Truncated Signed Distance Function) 및 기타 정보를 갱신

매개변수 설명:

const InputData& data: 입력 데이터로, 3D 맵을 업데이트하기 위한 센서 정보 및 이미지 데이터를 포함
VolumetricMap& map: 갱신될 3D 맵
bool allocate_blocks: (기본값 True)
- 새로운 블록을 할당할지 여부를 결정하는 플래그
- 만약 true이면, 새로운 블록(데이터 블록)을 생성해 TSDF 맵에 추가

함수 흐름:

auto& tsdf = map.getTsdfLayer();:
블록 할당 (센서의 가시 범위 내 블록 찾기):
```
const auto body_T_sensor = data.getSensorPose().cast<float>();
const auto block_indices = findBlocksInViewFrustum(
    data.getSensor(), body_T_sensor, map.blockSize(), data.min_range, data.max_range);
```
- body_T_sensor: 센서의 좌표 변환 행렬을 얻습니다. 이를 통해 센서의 위치와 방향을 결정합니다.
- findBlocksInViewFrustum: 주어진 센서의 위치와 시야를 기준으로, 해당 시야 내에 포함되는 3D 맵의 블록(공간적 단위)을 찾습니다. block_indices는 이러한 블록들의 인덱스를 저장합니다.
새로운 블록 할당:
```
BlockIndices new_blocks;
if (allocate_blocks) {
  new_blocks = map.allocateBlocks(block_indices);
}
```
- 설명:
  - 만약 새로운 블록을 할당해야 하는 경우(allocate_blocks == true), 해당 블록들을 3D 맵에 할당합니다.
  - 새로 할당된 블록들의 인덱스는 new_blocks에 저장됩니다.
블록 업데이트:
```
updateBlocks(block_indices, data, map);
```
- 설명: 블록들을 업데이트합니다.
업데이트되지 않은 블록 제거:
```
for (const auto& idx : new_blocks) {
  if (!tsdf.getBlock(idx).updated) {
    map.removeBlock(idx);
  }
}
```
- 설명: 할당된 블록 중 업데이트되지 않은 블록들을 맵에서 제거합니다.

3. updateBlocks

요약:

updateBlocks 함수는
- 주어진 블록 인덱스 리스트를 병렬로 처리하여 성능을 최적화
- 각 스레드는 블록 인덱스를 가져와 개별 블록을 업데이트하는 작업을 수행
updateBlock 함수
- 특정 블록 내의 모든 복셀을 순회하며, 각 복셀에 대해 SDF(표면 거리) 및 색상, 의미론적 정보를 갱신

2. `updateBlock` 함수

역할:

개별 블록(block_index)의 모든 복셀을 업데이트

주요 흐름:

BlockTuple blocks = map.getBlock(block_index);:
- block_index에 해당하는 블록을 가져옵니다.
복셀 업데이트 준비:
```
const auto sensor_T_body = data.getSensorPose().cast<float>().inverse();
```
- sensor_T_body는 센서가 측정한 데이터의 좌표 변환 행렬입니다. 이를 통해 복셀의 좌표를 센서의 좌표계로 변환할 수 있습니다.
- 3D 공간에서 복셀의 위치를 센서의 위치에 맞게 변환하여, 복셀을 기반으로 한 거리 및 다른 계산을 수행합니다.
복셀 업데이트 루프:
```
for (size_t i = 0; i < blocks.tsdf->numVoxels(); ++i) {
  const auto p_sensor = sensor_T_body * blocks.tsdf->getVoxelPosition(i);
  const auto measurement = getVoxelMeasurement(p_sensor, data, truncation_distance, voxel_size);
  if (!measurement.valid) {
    continue;
  }
  auto voxels = blocks.getVoxels(i);
  updateVoxel(data, measurement, truncation_distance, voxels);
  was_updated = true;
}
```
- 복셀 순회:
  - 각 블록의 모든 복셀에 대해 순회합니다.
  - blocks.tsdf->numVoxels()는 해당 블록 내에 있는 복셀의 개수를 반환하며, 각 복셀의 좌표는 blocks.tsdf->getVoxelPosition(i)를 통해 얻습니다.
- 좌표 변환: sensor_T_body * blocks.tsdf->getVoxelPosition(i)는 복셀의 좌표를 센서 좌표계로 변환합니다. 이렇게 변환된 좌표는 센서가 바라보는 방향에 맞추어 계산됩니다.
- 측정값 계산: getVoxelMeasurement 함수는 해당 복셀에 대한 SDF(서피스와의 거리)를 계산하고, 그 값이 유효한지 여부를 판단합니다.
- 복셀 업데이트: 만약 측정값이 유효하면, updateVoxel 함수가 해당 복셀의 값을 갱신합니다. 이 함수는 TSDF 및 색상, 의미론적 데이터를 갱신합니다.
- was_updated 플래그: 복셀이 업데이트되면 was_updated가 true로 설정됩니다. 이는 해당 블록이 적어도 한 번은 업데이트되었음을 의미합니다.
블록 업데이트 플래그 설정:
```
if (was_updated) {
  blocks.tsdf->setUpdated();
}
```
- 블록이 업데이트되었으면, 해당 블록을 "업데이트됨"으로 표시합니다. 이 플래그는 나중에 블록의 상태를 추적하는 데 사용됩니다.

updateVoxel 함수

updateVoxel 함수는 특정 복셀(voxel)의 데이터를 -> 센서로부터 받은 새로운 측정값에 기반하여 갱신
SDF, 색상 정보 및 의미론적(semantic) 데이터까지 통합하는 작업을 수행

요약:

유효한 측정값(SDF) 확인:

SDF 값이 유효한지 확인한 후, 유효하지 않으면 복셀을 갱신하지 않습니다.

가중치와 TSDF 거리 갱신:

새로운 측정값을 복셀의 기존 가중치 및 거리 값과 융합하여 복셀의 상태를 갱신

색상 및 의미론적 데이터 통합:

복셀에 대한 색상 및 의미론적 데이터를, 해당 정보가 존재할 경우 융합
이를 통해 복셀은 3D 맵에서 더 구체적이고 다양한 정보를 가지게 됨

2. 기존 가중치와 새로운 가중치의 융합:

auto& tsdf_voxel = *voxels.tsdf;
const auto prev_weight = tsdf_voxel.weight;
tsdf_voxel.weight =
    std::min(tsdf_voxel.weight + measurement.weight, config.max_weight);

tsdf_voxel: 이 복셀의 TSDF 데이터를 가리킵니다.
prev_weight: 이 복셀의 기존 가중치(weight)입니다. TSDF 값은 여러 센서 측정값이 융합되기 때문에, 각 측정값에 가중치가 부여됩니다.
가중치 갱신:
- 새로운 측정값(measurement.weight)을 기존 가중치에 더하여 복셀의 가중치를 갱신합니다.
- 다만, 가중치의 합이 config.max_weight를 넘지 않도록 제한합니다. (10000)
- 이를 통해 가중치가 지나치게 커져서 새로운 측정값이 반영되지 않는 상황을 방지합니다.

3. TSDF 거리 갱신:

tsdf_voxel.distance =
    (tsdf_voxel.distance * prev_weight + measurement.sdf * measurement.weight) /
    (prev_weight + measurement.weight);

이 부분은 가중 평균법(weighted averaging)을 사용하여 복셀의 TSDF 값을 갱신하는 부분입니다.
- 기존 TSDF 값(tsdf_voxel.distance)에 기존 가중치(prev_weight)를 곱하고,
- 새로 측정된 SDF 값(measurement.sdf)에 새 가중치(measurement.weight)를 곱한 후,
- 두 값을 더하여 총 가중치로 나누어 새로운 TSDF 값을 계산합니다.
이를 통해 이전의 측정값과 새로운 측정값이 균형 있게 반영됩니다.

4. 추적 정보 갱신:

if (voxels.tracking) {
    voxels.tracking->last_observed = data.timestamp_ns;
}

추적 정보 갱신: 만약 복셀에 대한 추적 정보(tracking)가 있다면, 해당 복셀이 마지막으로 관측된 시간을 현재 데이터의 타임스탬프(data.timestamp_ns)로 갱신합니다.
이 기능은 복셀의 최신성을 추적하는 데 사용되며, 오래된 복셀을 처리하거나 갱신할 때 도움이 될 수 있습니다.

5. 표면과의 거리 확인:

if (measurement.sdf >= truncation_distance) {
    return;
}

measurement.sdf: 해당 복셀이 표면에서 얼마나 떨어져 있는지 나타내는 값입니다.
(매우 중요) 만약 이 값이 truncation_distance 이상이라면, 복셀이 표면에서 너무 멀리 떨어져 있기 때문에 다른 데이터(예: 색상, 의미론적 정보)를 갱신할 필요가 없습니다. 이 경우 함수는 종료됩니다.

6. 색상 데이터 갱신:

if (!data.color_image.empty()) {
    const auto color = interpolator_->interpolateColor(
        data.color_image, measurement.interpolation_weights);
    const float ratio = measurement.weight / (tsdf_voxel.weight + measurement.weight);
    tsdf_voxel.color.merge(color, ratio);
}

색상 데이터:
- 만약 입력 데이터(data.color_image)에 색상 정보가 포함되어 있다면, 해당 색상 데이터를 복셀의 TSDF에 통합
색상 보간(interpolation):
- 복셀의 위치를 기준으로 색상을 보간(interpolation)하여 복셀의 색상 값을 추정
- interpolation_weights는 복셀의 위치를 기반으로 주변 색상 값을 참조하는 가중치
색상 융합: 복셀의 기존 색상과 새로운 색상을 가중 평균 방식으로 융합합니다. 새로운 측정값이 더 많이 반영될 수 있도록 측정값의 가중치(measurement.weight)에 기반한 비율(ratio)을 계산하고 이를 색상에 반영합니다.

7. 의미론적 데이터 갱신:

if (!semantic_integrator_ || !voxels.semantic) {
    return;
}

if (semantic_integrator_->isValidLabel(measurement.label)) {
    semantic_integrator_->updateLikelihoods(measurement.label, *voxels.semantic);
}

유효한 레이블 확인: measurement.label이 유효한 레이블인지 확인합니다.
의미론적 레이블 융합: 유효한 레이블인 경우, semantic_integrator_는 해당 복셀의 의미론적 레이블을 갱신합니다.
- 이는 복셀에 대한 의미론적 클래스(예: 사람, 건물, 도로 등)를 추정하는 데 사용됩니다.
- https://velog.io/@jk01019/hydra-code-srcreconstructionsemanticintegrator.cpp

4. 핵심 수학적 원리

4.1 TSDF 업데이트

TSDF는 현재 센서 데이터에 의해 계산된 새로운 거리를 이전 TSDF 값과 가중합 (weighted averaging)하여 업데이트합니다.
이때 새로운 TSDF 값은 다음과 같이 계산됩니다:

4.2 SDF 계산

float ProjectiveIntegrator::computeSDF(const InputData& data, const InterpolationWeights& weights, const float truncation_distance, const float distance_to_voxel) const;

SDF는 센서로부터 voxel까지의 거리를 측정하여 표면과의 거리를 계산합니다. 이때 SDF 값은 표면에서의 거리 차이로 정의됩니다:

이를 통해 해당 voxel이 표면에 가까운지 또는 표면 외부에 있는지를 결정하게 됩니다.

4.3 가중치 계산

float ProjectiveIntegrator::computeWeight(const Sensor& sensor, const Point& p_C, const float sdf, const float truncation_distance, const float voxel_size) const;

가중치는 센서의 센서 노이즈 모델에 따라 다르게 적용됩니다.
가까운 거리에서는 높은 가중치를, 먼 거리에서는 낮은 가중치를 부여하여, 데이터의 정확도를 반영합니다.
이 과정에서 가중치는 거리의 제곱에 반비례하여 감소합니다:
이는 거리가 멀수록 데이터의 신뢰도가 낮아짐을 반영한 계산입니다.

About_work

새로운 것이 들어오면 이미 있는 것과 충돌을 시도하라.

이전 포스트

hydra 용어 정리 - deformation Graph

다음 포스트

[Reconstruction] hydra code - src/reconstruction/projective_integrator.cpp

lifelong scene graph

1. 생성자와 updateMap

요약:

1. `ProjectiveIntegrator::ProjectiveIntegrator` 생성자

구성요소 설명:

2. `ProjectiveIntegrator::updateMap` 함수

매개변수 설명:

함수 흐름:

3. updateBlocks

요약:

2. `updateBlock` 함수

역할:

주요 흐름:

updateVoxel 함수

요약:

2. 기존 가중치와 새로운 가중치의 융합:

3. TSDF 거리 갱신:

4. 추적 정보 갱신:

5. 표면과의 거리 확인:

6. 색상 데이터 갱신:

7. 의미론적 데이터 갱신:

4. 핵심 수학적 원리

4.1 TSDF 업데이트

4.2 SDF 계산

4.3 가중치 계산

hydra 용어 정리 - deformation Graph

[Reconstruction] hydra code - src/reconstruction/semantic_integrator.cpp

0개의 댓글

[Reconstruction] hydra code - src/reconstruction/projective_integrator.cpp

lifelong scene graph

1. 생성자와 updateMap

요약:

1. ProjectiveIntegrator::ProjectiveIntegrator 생성자

구성요소 설명:

2. ProjectiveIntegrator::updateMap 함수

매개변수 설명:

함수 흐름:

3. updateBlocks

요약:

2. updateBlock 함수

역할:

주요 흐름:

updateVoxel 함수

요약:

2. 기존 가중치와 새로운 가중치의 융합:

3. TSDF 거리 갱신:

4. 추적 정보 갱신:

5. 표면과의 거리 확인:

6. 색상 데이터 갱신:

7. 의미론적 데이터 갱신:

4. 핵심 수학적 원리

4.1 TSDF 업데이트

4.2 SDF 계산

4.3 가중치 계산

hydra 용어 정리 - deformation Graph

[Reconstruction] hydra code - src/reconstruction/semantic_integrator.cpp

0개의 댓글

1. `ProjectiveIntegrator::ProjectiveIntegrator` 생성자

2. `ProjectiveIntegrator::updateMap` 함수

2. `updateBlock` 함수