[PyICP-SLAM] ScanContextManager.py 코드 공부

import numpy as np
np.set_printoptions(precision=4)

import time
from scipy import spatial

def xy2theta(x, y):
    if (x >= 0 and y >= 0): 
        theta = 180/np.pi * np.arctan(y/x);
    if (x < 0 and y >= 0): 
        theta = 180 - ((180/np.pi) * np.arctan(y/(-x)));
    if (x < 0 and y < 0): 
        theta = 180 + ((180/np.pi) * np.arctan(y/x));
    if ( x >= 0 and y < 0):
        theta = 360 - ((180/np.pi) * np.arctan((-y)/x));

    return theta


def pt2rs(point, gap_ring, gap_sector, num_ring, num_sector):
    x = point[0]
    y = point[1]
    # z = point[2]
    
    if(x == 0.0):
        x = 0.001
    if(y == 0.0):
        y = 0.001
    
    theta = xy2theta(x, y)
    faraway = np.sqrt(x*x + y*y)
    
    idx_ring = np.divmod(faraway, gap_ring)[0]       
    idx_sector = np.divmod(theta, gap_sector)[0]

    if(idx_ring >= num_ring):
        idx_ring = num_ring-1 # python starts with 0 and ends with N-1
    
    return int(idx_ring), int(idx_sector)


def ptcloud2sc(ptcloud, sc_shape, max_length):
    num_ring = sc_shape[0]
    num_sector = sc_shape[1]

    gap_ring = max_length/num_ring
    gap_sector = 360/num_sector
    
    enough_large = 500
    sc_storage = np.zeros([enough_large, num_ring, num_sector])
    sc_counter = np.zeros([num_ring, num_sector])
    
    num_points = ptcloud.shape[0]
    for pt_idx in range(num_points):
        point = ptcloud[pt_idx, :]
        point_height = point[2] + 2.0 # for setting ground is roughly zero 
        
        idx_ring, idx_sector = pt2rs(point, gap_ring, gap_sector, num_ring, num_sector)
        
        if sc_counter[idx_ring, idx_sector] >= enough_large:
            continue
        sc_storage[int(sc_counter[idx_ring, idx_sector]), idx_ring, idx_sector] = point_height
        sc_counter[idx_ring, idx_sector] = sc_counter[idx_ring, idx_sector] + 1

    sc = np.amax(sc_storage, axis=0)
        
    return sc


def sc2rk(sc):
    return np.mean(sc, axis=1)

def distance_sc(sc1, sc2):
    num_sectors = sc1.shape[1]

    # repeate to move 1 columns
    _one_step = 1 # const
    sim_for_each_cols = np.zeros(num_sectors)
    for i in range(num_sectors):
        # Shift
        sc1 = np.roll(sc1, _one_step, axis=1) #  columne shift

        #compare
        sum_of_cossim = 0
        num_col_engaged = 0
        for j in range(num_sectors):
            col_j_1 = sc1[:, j]
            col_j_2 = sc2[:, j]
            if (~np.any(col_j_1) or ~np.any(col_j_2)): 
                # to avoid being divided by zero when calculating cosine similarity
                # - but this part is quite slow in python, you can omit it.
                continue 

            cossim = np.dot(col_j_1, col_j_2) / (np.linalg.norm(col_j_1) * np.linalg.norm(col_j_2))
            sum_of_cossim = sum_of_cossim + cossim

            num_col_engaged = num_col_engaged + 1

        # save 
        sim_for_each_cols[i] = sum_of_cossim / num_col_engaged

    yaw_diff = np.argmax(sim_for_each_cols) + 1 # because python starts with 0 
    sim = np.max(sim_for_each_cols)
    dist = 1 - sim

    return dist, yaw_diff

    
class ScanContextManager:
    def __init__(self, shape=[20,60], num_candidates=10, threshold=0.15): # defualt configs are same as the original paper 
        self.shape = shape
        self.num_candidates = num_candidates
        self.threshold = threshold

        self.max_length = 80 # recommended but other (e.g., 100m) is also ok.

        self.ENOUGH_LARGE = 15000 # capable of up to ENOUGH_LARGE number of nodes 
        self.ptclouds = [None] * self.ENOUGH_LARGE
        self.scancontexts = [None] * self.ENOUGH_LARGE
        self.ringkeys = [None] * self.ENOUGH_LARGE

        self.curr_node_idx = 0
       

    def addNode(self, node_idx, ptcloud):
        sc = ptcloud2sc(ptcloud, self.shape, self.max_length)
        rk = sc2rk(sc)

        self.curr_node_idx = node_idx
        self.ptclouds[node_idx] = ptcloud
        self.scancontexts[node_idx] = sc
        self.ringkeys[node_idx] = rk

    def getPtcloud(self, node_idx):
        return self.ptclouds[node_idx]


    def detectLoop(self):        
        exclude_recent_nodes = 30
        valid_recent_node_idx = self.curr_node_idx - exclude_recent_nodes

        if(valid_recent_node_idx < 1):
            return None, None, None
        else:
            # step 1
            ringkey_history = np.array(self.ringkeys[:valid_recent_node_idx])
            ringkey_tree = spatial.KDTree(ringkey_history)

            ringkey_query = self.ringkeys[self.curr_node_idx]
            _, nncandidates_idx = ringkey_tree.query(ringkey_query, k=self.num_candidates)

            # step 2
            query_sc = self.scancontexts[self.curr_node_idx]
            
            nn_dist = 1.0 # initialize with the largest value of distance
            nn_idx = None
            nn_yawdiff = None
            for ith in range(self.num_candidates):
                candidate_idx = nncandidates_idx[ith]
                candidate_sc = self.scancontexts[candidate_idx]
                dist, yaw_diff = distance_sc(candidate_sc, query_sc)
                if(dist < nn_dist):
                    nn_dist = dist
                    nn_yawdiff = yaw_diff
                    nn_idx = candidate_idx

            if(nn_dist < self.threshold):
                nn_yawdiff_deg = nn_yawdiff * (360/self.shape[1])
                return nn_idx, nn_dist, nn_yawdiff_deg # loop detected!
            else:
                return None, None, None

1. xy2theta(x, y)
입력된 x, y 좌표를 바탕으로 각도(θ)를 계산한다.
x, y 좌표에 따라 4분면을 고려한 각도를 반환합니다. 이 함수는 x 또는 y가 0인 특수 경우를 처리하여, 0으로 나누는 오류를 방지한다.

2. pt2rs(point, gap_ring, gap_sector, num_ring, num_sector)

2D 포인트를 입력받아 해당 포인트가 속하는 링과 섹터의 인덱스를 계산합니다.
포인트의 x, y 좌표를 이용하여 극좌표(θ, r)로 변환하고, 이를 이용하여 링과 섹터 인덱스를 결정한다. 각 포인트의 위치에 따라 다른 링과 섹터로 분류된다.

링(Ring)과 섹터(Sector)

링(Ring)과 섹터(Sector)는 공간 데이터, 특히 3D 스캔 데이터나 라이다 데이터를 효율적으로 구성하고 분석하기 위해 사용되는 용어다. 이러한 구분은 데이터를 시각적으로 분석하거나 특정 알고리즘을 적용하는 데 도움이 되며, 주로 2D나 3D 데이터의 원형 또는 구형 배열을 구분하는 데 사용된다.

링(Ring)

정의: 링은 데이터 포인트들을 원의 중심에서부터 일정한 간격으로 묶은 것이다. 즉, 2차원 평면이나 3 차원 공간에서 중심점으로부터의 거리(반지름)에 따라 데이터 포인트들을 여러 개의 동심원 원환으로 나눈다.

용도: 링은 데이터의 거리적 특성을 이해하는 데 유용하다. 예를 들어, 라이다 스캔에서 링은 로봇이나 센서로부터의 거리에 따라 환경의 특성을 층별로 분리할 수 있게 해준다. 각 링은 센서로부터 일정 거리를 유지한 데이터 포인트들의 그룹이다.

섹터(Sector)

정의: 섹터는 원 또는 구를 중심 각도에 따라 분할한 부분이다. 섹터는 중심점에서 각도를 기준으로 데이터를 구분하여, 전체 360도를 일정한 각도의 부분으로 나눈다.

용도: 섹터는 각도에 따른 데이터의 분포를 이해하는 데 사용된다. 이는 방향성을 분석하거나 특정 방향에서 오는 데이터의 특성을 파악하는 데 도움이 된다. 예를 들어, 섹터를 사용하여 특정 방향으로부터의 물체나 장애물의 위치를 파악할 수 있다.

링(Ring)의 예시

로봇이 방안을 라이다 센서로 스캔하고 있다. 라이다 센서는 로봇의 위치에서부터 일정 각도와 거리마다 데이터 포인트를 수집한다. 이 데이터를 거리에 따라 여러 그룹으로 나누는데, 이 그룹들을 "링"이라고 한다.

예를 들어, 로봇으로부터 1m 이내의 모든 데이터 포인트를 링 1에, 1m에서 2m 사이를 링 2에, 그리고 2m에서 3m 사이를 링 3에 배치한다. 각 링은 로봇으로부터의 거리에 따라 구분되어, 로봇이 각 거리대에서 어떤 물체를 감지했는지 분석할 수 있게 해준다.

섹터(Sector)의 예시

같은 로봇이 방안을 스캔하는 상황에서, 섹터는 각도에 따라 데이터를 나눈다. 전체 360도를 예를 들어 30도씩 나누어 총 12개의 섹터로 구분할 수 있다.

이제 로봇이 동쪽을 향해 있을 때 감지하는 데이터는 섹터 1에, 북동쪽은 섹터 2에, 북쪽은 섹터 3에 이런 식으로 각각 배치한다. 이 구분은 로봇이 특정 방향에서 어떤 물체를 감지했는지 정확히 알 수 있도록 해준다. 만약 섹터 3(북쪽)에서 물체가 감지되면, 로봇은 그 방향에 무언가가 있다는 것을 알게 된다.

3. ptcloud2sc(ptcloud, sc_shape, max_length)

3D 포인트 클라우드를 입력받아 스캔 컨텍스트를 생성한다. 포인트 클라우드의 각 포인트를 링과 섹터로 분류하고, 각 섹터별로 최대 높이를 저장한다. 이 데이터는 스캔 컨텍스트를 형성하는 데 사용되며, 큰 배열을 사용하여 각 섹터별로 높이 값을 관리한다.

    enough_large = 500 # 각 링과 섹터에 저장할 수 있는 데이터 포인트 수 제한
    # ex. 2번 째 링과 3 섹터에 포함되는 데이터의 수가 500개가 넘어가면 안 됨.
    # 메모리 관리, 성능 최적화, 데이터 품질 보장
    sc_storage = np.zeros([enough_large, num_ring, num_sector])# shape (500,20,60)
    # sc_storage 배열은 각 링(ring)과 섹터(sector) 조합에 대한 
    # 데이터 포인트의 높이 값을 저장하는 3차원 배열. 
    # 이 배열은 포인트 클라우드 데이터에서 각 포인트의 공간적 위치를 링과 섹터로 분류한 후,
    # 해당 위치에 대응되는 높이 정보를 체계적으로 기록하는 데 사용
    sc_counter = np.zeros([num_ring, num_sector])     # shape (20,60)
    # ring row, sector col  data number
    # sc_counter 배열은 각 링과 섹터 조합에 현재 저장된 데이터 포인트의 수

4. distance_sc(sc1, sc2):

두 개의 scan_context 값으로 코사인 유사도 측정을 통해 회전 값과 거리 값을 반환한다. 여기서 거리 값은 유클리드 거리 값이 아닌 코사인 유사도 값을 1에서 뺀 값이다. 두 scan context 간의 구조적 유사성을 수치화하여 loop dection을 위한 값으로 사용된다.

함수에서 코사인 유사도를 측정할 때 열을 오른쪽으로 이동(shift)시키면서 측정하는 이유는 스캔 컨텍스트의 회전 불변성을 확보하기 위함이다. 센서가 같은 장소를 다른 각도로 스캔했을 때 유용하다.
만약 열을 15번 이동시켰다면, 이는 $6\,^{\circ} * 15 = 90\,^{\circ}$
만큼 회전한 것과 같다. 따라서 두 스캔 컨텍스트가 원래 상태에서 90도 회전한 상태로 가장 유사하게 일치한다면, 15번 열을 이동시킨 상태에서 가장 높은 유사도를 얻을 수 있다.

코사인 유사도 : 두 벡터 간의 유사성을 측정하는 방법 중 하나로, 벡터 간의 코사인 각도를 계산하여 그 결과값을 유사도로 사용한다. 이 값은 두 벡터의 방향성이 얼마나 유사한지를 나타내며, 벡터의 크기는 고려하지 않는다. 코사인 유사도는 -1에서 1 사이의 값을 가지며, 1에 가까울수록 두 벡터의 방향성이 매우 유사함을 의미하고, 0은 서로 직각을 이루는 경우, -1은 완전히 반대 방향을 가리킨다.

코사인 유사도는 다음 공식으로 계산된다:

여기서:

• $\vec{a} \cdot \vec{b}$는 두 벡터의 내적이다.

• $|\vec{a}|$ 및 $|\vec{b}|$는 각각 벡터 $\vec{a}$와 $\vec{b}$의 유클리디언 노름(크기)이다.

코사인 유사도의 사용 예:

코사인 유사도는 다양한 분야에서 데이터 포인트 간의 유사성을 평가하는 데 널리 사용된다. 특히 텍스트 데이터의 유사성 평가, 추천 시스템, 클러스터링 등에서 유용하게 활용된다. 예를 들어, 텍스트 문서 간의 유사성을 측정할 때 단어 빈도를 기반으로 벡터를 만들고, 이 벡터들 간의 코사인 유사도를 계산하여 유사한 문서를 찾아낼 수 있다.

코사인 유사도의 장점:

크기에 무관하게 유사도를 측정할 수 있다: 벡터의 크기가 서로 다르더라도 방향이 유사하면 높은 유사도가 나타난다.
효율적인 계산: 고차원 벡터 간의 유사도도 효율적으로 계산할 수 있다. 이러한 특성으로 인해 코사인 유사도는 다양한 분야에서 널리 활용되고 있다.

5. sc2rk(sc)
scan context를 입력받아 ring key를 생성하는 함수다. 입력 받은 scan context는 각 링, 각 섹터의 높이 최댓값이 저장된 (20,60) 행렬이다.(여기 코드에서) 행 방향으로 평균 값을 낸 행렬이 Ring key이다. 이렇게 생성된 ring key는 scan context 전체를 간단하게 표현하면서도 중요한 구조적 정보를 가지고 있다.

ScanContextManager Class

1. init(self, shape=[20,60], num_candidates=10, threshold=0.15)

threshold, max_length, ptcloud,scan_context,ring_key를 저장할 배열을 ENOUGH_LARGE만큼 선언한다.

2. addNode(self, node_idx, ptcloud)

들어오는 ptcloud를 scan_context, ring_key로 변환 후, ptcloud, scan_context,ring_key 배열에 저장한다.

3. getPtcloud(self, node_idx)

node_idx에 해당하는 ptcloud를 반환한다.

4. detectLoop(self):

• 최근 노드 제외 : exclude_recent_nodes = 30은 가장 최근에 추가된 30개 노드를 루프 감지 과정에서 제외하는 것을 의미한다. 이는 로봇이 방금 지나온 길을 고려하지 않고 오래된 경로와의 비교를 통해 실제 루프를 감지하는 데 도움을 준다.

• 링 키 기반 노드 검색:

  ringkey_history는 이전 노드들의 링 키를 배열로 저장한다.
  ringkey_tree = spatial.KDTree(ringkey_history)는 링 키 기반으로 KDTree를 생성한다. KDTree는 다차원 공간에서 가장 가까운 이웃을 빠르게 검색할 수 있는 알고리즘이다.
  ringkey_query는 현재 노드의 링 키를 가져온다.
  ringkey_tree.query(ringkey_query, k=self.num_candidates)는
  현재 링 키와 가장 유사한 링 키를 가진 이전 노드들의 인덱스를 찾는다. 여기서 k=self.num_candidates는 검색할 후보의 수를 지정한다. 여기서는 10으로 설정했으므로, 10개의 후보 ring_key 인덱스가 반환된다.
  실제 코드를 돌려본 결과 [36 35 34 32 37 28 26 38 31 39]
  이런 식으로 반환 되는 것을 확인 할 수 있다.

• 스캔 컨텍스트 기반 상세 비교:

  각 후보 노드의 스캔 컨텍스트와 현재 노드의 스캔 컨텍스트를 비교하여 유사도(dist)와 각도 변화(yaw_diff)를 계산한다.
  nn_dist는 발견된 최소 거리를 저장하고, nn_yawdiff는 해당 거리에서의 각도 변화를 저장한다. 이 값들이 업데이트되면서 가장 유사한 노드를 식별한다.

• 루프 감지 결과 반환:

  만약 계산된 최소 거리 nn_dist가 설정된 임계값 self.threshold보다 작다면, 루프가 감지된 것으로 판단하고, 해당 노드 인덱스(nn_idx), 거리(nn_dist), 그리고 각도 변화(nn_yawdiff_deg)를 반환한다.
  루프가 감지되지 않으면 None을 반환한다.