ScanContext 回环检测的python实现（slam)

Scan Context，从英文字面理解就是“扫描 上下文”。类比于我们阅读的时候，需要理解上下文，才能明白其意，LidarSLAM在进行回环检测的时候，也需要将“上下文” （之前的数据）进行比较，方才知道我们是不是又走到了之前的同一个地方（回环）。

Scan Context这篇文章由韩国KAIST大学的Giseop Kim和Ayoung Kim所写，它的主要特点是提出了Scan Context这个非直方图的全局描述符，来帮助我们对“上下文”（当前/之前的数据）进行更快速、有效地搜索。典型的应用就是在LiDAR SLAM中进行回环检测和Place Recognition。

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(poin