ICPSLAM

About_work·2024년 8월 3일
0

global mapper

목록 보기
23/37

1. 전반적 설명

1.1. ICPSLAM 클래스 설명

  • ICPSLAM 클래스는 RGB-D 이미지 시퀀스를 사용하여 SLAM을 수행하는 PyTorch 기반 모듈
  • 이 클래스는 RGB-D 이미지 시퀀스를 입력으로 받아서,
    • 포인트 클라우드(Point Cloud)와 카메라의 Pose를 생성

1.2. 주요 구성 요소 및 기능

  1. 입력 파라미터

    • odom: 사용하려는 오도메트리 방법.
      • gt(ground truth), icp(Iterative Closest Point), gradicp(Gradient-based ICP) 중 선택.
        • icp (Iterative Closest Point):
          • 두 포인트 클라우드 간의 변환을 반복적으로 계산하여 정합하는 방법.
        • gradicp (Gradient-based ICP):
          • ICP의 변형으로, 그래디언트 기반 최적화를 사용하여 정합 성능을 개선한 방법.
    • dsratio:
      • 다운샘플링 비율.
      • 입력 프레임을 ICP 수행 전에 다운샘플링하는 데 사용.
    • numiters:
      • 최적화를 수행할 반복 횟수.
    • damp:
      • 비선형 최소 제곱을 위한 감쇠 계수.
    • dist_thresh:
      • src_pc와 tgt_pc 사이의 거리 임계값.
      • 지정된 경우 해당 거리에 있는 포인트들을 제거.
    • lambda_max, B, B2, nu: `
      • gradicp` 방법에 필요한 추가 파라미터.

  2. 클래스 생성자 (init)

    • 클래스 생성자는 입력 파라미터를 받아서
    • 오도메트리 프로바이더(odometry provider)를 초기화하고,
    • 입력 RGB-D 이미지를 처리하기 위한 기본 설정을 수행

  3. forward 메서드

    • 주어진 RGB-D 이미지 시퀀스를 입력으로 받아, 포인트 클라우드와 포즈를 반환
    • 시퀀스의 각 프레임을 처리하며, 첫 번째 프레임의 경우 초기 포즈를 설정
    • 각 프레임에 대해 step 메서드를 호출하여, 포인트 클라우드와 포즈를 업데이트
  1. _map 메서드
    • 현재 프레임의 포인트 클라우드를 전역 포인트 클라우드에 통합하여 업데이트

2. step, (_localize+ _map) 메서드

  • step 함수: 현재 프레임을 사용해 전역 맵을 업데이트
    • _localize 함수: 현재 프레임의 포즈를 계산
    • _map 함수: 현재 프레임의 점들을 전역 맵에 통합

2.1. step 함수

def step(
    self,
    pointclouds: Pointclouds,
    live_frame: RGBDImages,
    prev_frame: Optional[RGBDImages] = None,
    inplace: bool = False,
):
live_frame.poses = self._localize(pointclouds, live_frame, prev_frame)
pointclouds = self._map(pointclouds, live_frame, inplace)

  • 역할:

    • 이 함수는 현재 프레임(live_frame)을 사용하여 전역 맵 점 구름(pointclouds)을 업데이트
    • 이전 프레임(prev_frame)이 주어지면, 현재 프레임과의 상대 변환을 계산하여 현재 프레임의 위치를 추정
    • 이전 프레임이 주어지지 않으면 현재 프레임의 포즈를 사용

  • 인자:

    • pointclouds (gradslam.Pointclouds): 전역 맵 점 구름.
    • live_frame (gradslam.RGBDImages): 현재 프레임의 RGB-D 이미지. 시퀀스 길이는 1이어야 합니다.
    • prev_frame (gradslam.RGBDImages or None): 이전 프레임의 RGB-D 이미지. 시퀀스 길이는 1이어야 합니다.
    • inplace (bool):
      • True로 설정하면 점 구름과 현재 프레임 포즈를 제자리에서 업데이트
      • 기본값은 False.

  • 반환값:

    • pointclouds (gradslam.Pointclouds): 업데이트된 전역 맵 점 구름.
    • poses (torch.Tensor): 현재 프레임 배치의 포즈.
live_frame.poses = self._localize(pointclouds, live_frame, prev_frame)
pointclouds = self._map(pointclouds, live_frame, inplace)
return pointclouds, live_frame.poses
  • self._localize 함수를 호출하여 live_frame의 포즈를 계산
  • self._map 함수를 호출하여 전역 맵 점 구름을 업데이트
  • 업데이트된 점 구름과 포즈를 반환

2.2. _localize 함수

def _localize(
    self, pointclouds: Pointclouds, live_frame: RGBDImages, prev_frame: RGBDImages
):
  • 현재 프레임의 pose를 계산
  • 이전 프레임이 주어진 경우, 선택한 odometry 방법을 사용하여 현재 프레임과 이전 프레임 간의 상대 변환을 계산
  • 이전 프레임이 주어지지 않은 경우, 현재 프레임의 포즈를 그대로 사용합니다.
  • 반환값:
    • poses (torch.Tensor): 현재 프레임 배치의 포즈.
if self.odom in ["icp", "gradicp"]:
    live_frame.poses = prev_frame.poses
    frames_pc = downsample_rgbdimages(live_frame, self.dsratio)
    pc2im_bnhw = find_active_map_points(pointclouds, prev_frame)
    maps_pc = downsample_pointclouds(pointclouds, pc2im_bnhw, self.dsratio)
    transform = self.odomprov.provide(maps_pc, frames_pc)

    return compose_transformations(
        transform.squeeze(1), prev_frame.poses.squeeze(1)
    ).unsqueeze(1)

2.2.1. 개요

  • 요약:
    • prev_frame의 pose를 기반으로, global pcd중 카메라 frame에 매칭되는 점들을 image plane으로 옮긴다.
    • 이렇게 옮겨진 pcd와, live-frame의 RGBD 값을 비교하여, 얼마나 이동했는지 체크한다.

2.2.2. find_active_map_points

pc2im_bnhw = find_active_map_points(pointclouds, prev_frame)
  • find_active_map_points 함수는 주어진 글로벌 맵 포인트 클라우드와 prev_frame으로부터
    • 활성 맵 포인트(prev_frame 안에 있는 맵 포인트)를 찾아서,
    • 해당 포인트들의 인덱스와 이미지 프레임에서의 위치를 반환
  • 이 함수는 글로벌 맵 포인트와 라이브 프레임 간의 연관성을 찾기 위해 사용

2.2.2.1. 주요 출력

  • pc2im_bnhw (torch.Tensor):
    • 활성 맵 포인트의 룩업 테이블.
    • (num_active_map_points, 4) 크기의 텐서.
      • 각 행은 배치 인덱스 b, 포인트 인덱스 n, 그리고 라이브 프레임에서의 높이 h와 너비 w 인덱스를 포함

2.3. _map 함수

def _map(
    self, pointclouds: Pointclouds, live_frame: RGBDImages, inplace: bool = False
):

  • 역할: 현재 프레임의 점들을 사용하여 전역 맵 점 구름을 업데이트

  • 인자:

    • pointclouds (gradslam.Pointclouds): 전역 맵 점 구름.
    • live_frame (gradslam.RGBDImages): 현재 프레임의 RGB-D 이미지. 시퀀스 길이는 1이어야 합니다.
    • inplace (bool): True로 설정하면 점 구름을 제자리에서 업데이트합니다. 기본값은 False.

  • 반환값:

    • pointclouds (gradslam.Pointclouds): 업데이트된 전역 맵 점 구름.
return update_map_aggregate(pointclouds, live_frame, inplace)
  • update_map_aggregate 함수를 호출하여 전역 맵 점 구름을 업데이트하고, 그 결과를 반환합니다. inplace 인자가 True이면 제자리에서 업데이트를 수행합니다.

2.3.1. update_map_aggregate

  • update_map_aggregate 함수는
    • 현재 프레임의 RGB-D 이미지를 사용하여 전역 맵 점 구름을 업데이트
  • pointclouds_from_rgbdimages 함수는
    • RGB-D 이미지를 점 구름으로 변환

2.3.2. pointclouds_from_rgbdimages 함수

def pointclouds_from_rgbdimages(
    rgbdimages: RGBDImages,
    *,
    global_coordinates: bool = True,
    filter_missing_depths: bool = True,
    use_embeddings: bool = False,
) -> Pointclouds:

  • 역할:

    • 이 함수는 RGB-D 이미지를 3D 점 구름으로 변환
    • 각 RGB-D 이미지는 색상, 깊이 값, 그리고 선택적으로 임베딩을 포함할 수 있습니다.

  • 인자:

    • rgbdimages (gradslam.RGBDImages): RGB-D 이미지의 배치를 포함하지만 시퀀스 길이는 1이어야 합니다.
    • global_coordinates (bool): True로 설정하면 글로벌 좌표계에서 점 구름을 생성합니다. 기본값은 True.
    • filter_missing_depths (bool): True로 설정하면 깊이 값이 없는 점들을 제외합니다. 기본값은 True.
    • use_embeddings (bool): True로 설정하면 임베딩을 사용합니다. 기본값은 False.

  • 반환값:

    • pointclouds (gradslam.Pointclouds): 생성된 점 구름.

  • 알고리즘

    • rgbdimages의 타입과 시퀀스 길이를 확인합니다.
    • 배치 크기(B)를 설정하고, RGB-D 이미지를 채널 마지막 형식으로 변환
    • 글로벌 좌표계를 사용할지 여부에 따라 vertex_map과 normal_map을 설정
    • 깊이 값이 없는 점들을 제외할지 여부에 따라 마스크를 설정
    • 마스크를 사용하여 유효한 점, 법선 벡터, 색상을 추출합니다. 깊이 값이 있는 점들만 포함합니다.
    • 점, 법선 벡터, 색상을 포함하는 Pointclouds 객체를 반환

2.3.3. pointclouds.append_points

  • append_points 메서드는 현재의 Pointclouds 객체에
    • 다른 Pointclouds 객체의 점, 노멀, 색상 및 기타 속성을 추가(병합)
      • 말그대로 그냥 append
  • 두 개의 Pointclouds 객체가 동일한 속성을 가지고 있어야 하며, 같은 배치 크기를 가져야 합니다.
    • 이 메서드는 직접적으로(in-place) 현재의 Pointclouds 객체를 수정
profile
About_work
새로운 것이 들어오면 이미 있는 것과 충돌을 시도하라.
이전 포스트

GRAD SLAM

다음 포스트

PointFusion (ICPSLAM)

0개의 댓글