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

• 생성자와 fromInput 메서드가 쓰임

• ReconstructionOutput은 로봇이 이동하는 동안 수집한 데이터를 바탕으로
  • 3D 볼륨 맵(Volumetric Map)을 구성하고 업데이트하는 기능을 제공
• 이 클래스는 3D 공간의 변화를 추적하고, 그 변화를 재구성하여 다른 모듈들이 접근할 수 있도록 처리
• 이 클래스는 로봇의 위치 정보, 맵 데이터, 아카이브된 블록 목록을 관리하고, 입력 데이터를 바탕으로 맵을 업데이트하거나 새로운 맵을 설정할 수 있습니다.

주요 클래스: ReconstructionOutput

• ReconstructionOutput 클래스는 재구성 모듈의 출력 데이터를 관리하는 구조체로, 다음과 같은 주요 역할을 수행

1. 주요 데이터 필드

• timestamp_ns: 데이터 수집 시점의 타임스탬프 (단위: 나노초).
• world_t_body: 로봇의 월드 좌표계에서의 평행 이동(translation) 벡터.
• world_R_body: 로봇의 월드 좌표계에서의 회전(rotation)을 나타내는 쿼터니언.
• sensor_data: 로봇의 센서에서 수집된 입력 데이터 (예: RGBD 데이터, 포인트 클라우드 등).
• map_: Volumetric Map에 대한 포인터로, 이 맵은 로봇이 수집한 공간 데이터를 바탕으로 한 3D 맵을 표현
• archived_blocks:
  • 재구성 과정에서 아카이브된 블록(이전 데이터)의 목록. 이 블록들은 더 이상 활성화되지 않으며, 과거 정보로 유지됩니다.

2. 주요 메서드 설명

fromInput() 메서드

• 역할: 주어진 입력 패킷(InputPacket)으로부터 새로운 ReconstructionOutput 객체를 생성하는 역할을 합니다.
• 사용 예시: 새로운 데이터 패킷이 들어왔을 때, 이를 재구성 모듈에 전달하기 위한 목적으로 사용됩니다.

OutputPtr ReconstructionOutput::fromInput(const InputPacket& msg) {
  auto new_msg = std::make_shared<ReconstructionOutput>();
  new_msg->timestamp_ns = msg.timestamp_ns;
  new_msg->world_t_body = msg.world_t_body;
  new_msg->world_R_body = msg.world_R_body;
  return new_msg;
}

map() 메서드

• 역할: 이 메서드는 저장된 map_ 객체를 리턴합니다.
• 예외 처리: 맵이 유효하지 않을 경우, 프로그램이 종료되며 오류 메시지가 출력됩니다.
• 코드 설명:

  const VolumetricMap& ReconstructionOutput::map() const {
    CHECK(map_) << "Invalid map!";
    return *map_;
  }

  • CHECK(map_): map_이 유효하지 않으면 오류 메시지를 출력하고, 코드가 종료됩니다.
  • 리턴값: 유효한 VolumetricMap을 반환합니다.

updateFrom() 메서드

• 역할: 이 메서드는 다른 ReconstructionOutput 객체에서 데이터를 받아 맵과 관련된 정보를 업데이트합니다. 만약 새로운 데이터가 전달되면, 기존 데이터를 덮어씁니다.
• 매개변수:
  • msg: 복사할 데이터를 포함하고 있는 ReconstructionOutput 객체.
  • clone_map: 맵을 복사할지 여부를 결정하는 플래그. true이면 맵을 복제하고, false이면 맵을 공유합니다.
• 작동 방식:
  1. 타임스탬프 및 위치 정보 복사: 타임스탬프, 위치, 회전 정보를 새로운 데이터로 업데이트합니다.
  2. 아카이브된 블록 추가: archived_blocks에 새로운 블록들을 추가합니다.
  3. 맵 처리:
     • 맵이 없는 경우: 새 맵을 생성하고, 복제할지 여부에 따라 기존 맵을 복사 또는 공유합니다.
     • 맵이 있는 경우: 새롭게 들어온 맵을 바탕으로 현재 맵을 업데이트합니다.

void ReconstructionOutput::updateFrom(const ReconstructionOutput& msg, bool clone_map) {
  timestamp_ns = msg.timestamp_ns;
  world_t_body = msg.world_t_body;
  world_R_body = msg.world_R_body;
  sensor_data = msg.sensor_data;

  archived_blocks.insert(
      archived_blocks.end(), msg.archived_blocks.begin(), msg.archived_blocks.end());

  if (!msg.map_) {
    LOG(ERROR) << "Reconstruction output message contained no map!";
    return;
  }

  if (!map_ && !clone_map) {
    map_ = msg.map_;  // 기존 맵을 복사하지 않고 그대로 사용
  }

  const auto& new_map = *msg.map_;
  if (!map_) {
    const auto has_labels = new_map.getSemanticLayer() != nullptr;
    map_ = std::make_shared<VolumetricMap>(new_map.config, has_labels);
  }

  map_->updateFrom(new_map);  // 현재 맵을 새 맵으로 업데이트
}

setMap() 메서드

• 역할: 이 메서드는 새로운 VolumetricMap 객체를 설정합니다. 해당 객체는 복제된 맵을 사용하거나, 맵의 포인터를 그대로 사용할 수 있습니다.
• 두 가지 버전:
  • 첫 번째는 맵을 복제하여 설정.
  • 두 번째는 포인터로 직접 맵을 설정.

void ReconstructionOutput::setMap(const VolumetricMap& map) {
  auto new_map = map.clone();  // 복사본 생성
  map_.reset(new_map.release());  // 포인터 설정
}

void ReconstructionOutput::setMap(const std::shared_ptr<VolumetricMap>& map) {
  map_ = map;  // 직접 포인터를 설정
}

getMapPointer() 메서드

• 역할: 현재 설정된 VolumetricMap 객체의 포인터를 반환합니다.

std::shared_ptr<VolumetricMap> ReconstructionOutput::getMapPointer() const {
  return map_;
}