Reinforced-Feature-Points代码解读

本文是对Reinforced Feature Points:Optimizing Feature Detection and Description for a High-Level Task论文开源代码的解读和自己的思考。
论文链接：https://arxiv.org/pdf/1912.00623.pdf
代码链接：https://github.com/aritra0593/Reinforced-Feature-Points
论文笔记
接下来以Training里的main.py，即训练代码为例进行解读

def get_args_parser():
    # Parse command line arguments.
    parser = argparse.ArgumentParser('Set network parameters', add_help=False)

    parser.add_argument('--dataset1', default='st_peters_square', type=str,
      help='Image directory of outdoor images for training the network')
    parser.add_argument('--dataset2', default='brown_bm_3---brown_bm_3-maxpairs-10000-random---skip-10-dilate-25', type=str,
      help='Image directory of indoor images for training the network')
    parser.add_argument('--weights1', default='weights/superpoint_v1.pth', type=str,
      help='Path to pretrained weights file for backend')
    parser.add_argument('--nms_dist', default=4, type=int, help='Non Maximum Suppression (NMS) distance (default: 4).')
    parser.add_argument('--conf_thresh', default=0.00015, type=float, help='Detector confidence threshold (default: 0.015).')
    parser.add_argument('--nn_thresh', default=0.7, type=float, help='Descriptor matching threshold (default: 0.7).')
    parser.add_argument('--threshold', default=0.001, type=float, help='inlier threshold')
    parser.add_argument('--ratio', default=1.0, type=float, help='lowes ratio test')
    parser.add_argument('--vthreshold1', default=100.0, type=float, help='visibility threshold_outdoors')
    parser.add_argument('--vthreshold2', default=0.5, type=float, help='visibility threshold_indoors')
    parser.add_argument('--lr_bbone', default=0.0000001, type=float, help='learning rate for backbone')
    parser.add_argument('--samp_pts', default=600, type=int, help='number of keypoints sampled')
    parser.add_argument('--cr_check', action='store_false', help='cross check option for BFmatcher (default: true)')
    parser.add_argument('--cuda', action='store_false', help='Use cuda GPU to speed up network processing speed (default: true)')
    parser.add_argument('--start_epoch', default=0, type=int, help='start epoch')
    parser.add_argument('--epochs', default=50, type=int, help = 'number of epochs')
    parser.add_argument('--output_dir', default='output', type=str, help='turn on training for blackbox')
    return parser
def main(args):
    # 调用superpoint模型
    model_bbone = SuperPointFrontend(weights_path=args.weights1, nms_dist=args.nms_dist, conf_thresh=args.conf_thresh,
                            nn_thresh=args.nn_thresh, cuda=args.cuda)

    optimizer = optim.Adam(model_bbone.net.parameters(), lr=args.lr_bbone)
    # 针对dataset1，找到匹配点超过100的匹配图像，针对dataset2，找到匹配置信度超过0.5的匹配图像，并且得到其图片路径(data_dir + img_files)，匹配关系(vis_pairs)，参数(相机内参K和R，t，即cal_db)
    data_dir, img_files, vis_pairs, cal_db = build_dataset(args.dataset1, args.dataset2, args.vthreshold1, args.vthreshold2)
    for epoch in range(50):
        # 打乱匹配对
        random.shuffle(vis_pairs)
        # 开始训练
        train_one_epoch(model_bbone, optimizer, args.cr_check, data_dir, img_files, cal_db, vis_pairs, args.samp_pts, args.threshold, epoch, args.output_dir)

首先创建SuperPoint的模型变量model_bbone，设置优化器(optimizer)，选择adam优化器，进入build_dataset函数，build_dataset函数的核心是create_batch函数，下面一并附上

def create_batch(dataset, vis_thresh, id):
    cal_db_list = {}
    vis_pairs = []

    # 设置图片的根目录
    data_dir = 'datasets/' + dataset + '/train/'

    img_db = 'images.txt'
    vis_db = 'visibility.txt'
    cal_db = 'calibration.txt'

    # 分别读取三个文件
    img_db = open(data_dir + img_db, 'r')
    vis_db = open(data_dir + vis_db, 'r')
    cal_db = open(data_dir + cal_db, 'r')

    # readlines会读取文件里的全部内容
    img_files = img_db.readlines()
    vis_files = vis_db.readlines()
    cal_files = cal_db.readlines()

    img_db.close()
    vis_db.close()
    cal_db.close()

    # 处理标定文件中的数据，包含了相机的内参矩阵K，当前帧相对于初始位姿的旋转矩阵R以及平移矩阵T，存入到cal_db_list中
    for i, cal_file in enumerate(cal_files):
        cal = h5py.File(data_dir + cal_file[:-1], 'r')

        K = np.array(cal['K'])
        R = np.array(cal['R'])
        T = np.array(cal['T'])
        imsize = np.array(cal['imsize'])
        #     print(imsize[0,0], imsize[0,1])

        #     K[0, 2] += imsize[0, 0] * 0.5
        #     K[1, 2] += imsize[0, 1] * 0.5
        K[0, 2] += 1024 * 0.5
        K[1, 2] += 1024 * 0.5

        cal_db_list[i] = (K, R, T)

    # 处理匹配对文件，将每对匹配用(i,j,0)的方式保存，这里存在问题，(i,j,0)里的0默认数据集为dataset1，那dataset2里的数据怎么办？
    for i, vis_file in enumerate(vis_files):

        vis_file = open(data_dir + vis_file[:-1])
        vis_infos = vis_file.readlines()

        for j, vis_info in enumerate(vis_infos):
            vis_count = float(vis_info)
            if vis_count > vis_thresh:
                vis_pairs.append((i, j, 0))

        vis_file.close()

    # 打乱匹配对
    random.shuffle(vis_pairs)
    # 如果是dataset1，只取前一万对匹配对
    if id == 1:
        vis_mod = vis_pairs[0:10000]
    # 复制dataset2的全部匹配对
    else:
        vis_mod = vis_pairs.copy()

    return data_dir, img_files, cal_db_list, vis_mod

def build_dataset(dataset1, dataset2, vthresh1, vthresh2):
    data_dir_arr = []
    img_files_arr = []
    vis_pairs_arr = []
    cal_db_arr = []

    data_dir1, img_files1, cal_db1, vis_pair1 = create_batch(dataset1, vthresh1, 1)
    data_dir_arr.append(data_dir1)
    img_files_arr.append(img_files1)
    cal_db_arr.append(cal_db1)
    vis_pairs_arr = vis_pair1.copy()
    data_dir2, img_files2, cal_db2, vis_pair2 = create_batch(dataset2, vthresh2, 2)
    data_dir_arr.append(data_dir2)
    img_files_arr.append(img_files2)
    cal_db_arr.append(cal_db2)
    vis_pairs_arr += vis_pair2.copy()
    random.shuffle(vis_pairs_arr)

    return data_dir_arr, img_files_arr, vis_pairs_arr, cal_db_arr

关于create_batch的详细理解可以参考注释，按照自己的理解，这里可能会存在问题，(i,j,0)的0默认根目录是dataset1所在目录，所以读取dataset2的时候会存在问题，同时，在对K的处理上默认图片的像素为10241024，dataset1中的图片满足该要求，dataset2中的图片为640480，怀疑只是用了dataset1做了有效训练。

回到主函数，进入train_one_epoch()函数，开始正式训练

def train_one_epoch(model_bbone: torch.nn.Module,optimizer: torch.optim.Optimizer, cr_check, data_dir, img_files, cal_db, vis_pairs_mod, samp_pts, threshold, epoch, output_dir):
    temp = 0
    lamda1 = 1
    # 记录损失均值
    mean_loss_heap = []
    # 记录损失的最小值
    min_loss_heap = []
    nfeatures = 2000
    # loweRatio = args.ratio

    counter = 0
    # 设置计时器
    start = time.time()
    file_saver = True
    # 依次读取匹配对中的匹配
    for i, vis_pair in enumerate(vis_pairs_mod):
        #     for ite in range(batch_size):
        counter += 1
        img_stack = []

        img1_idx = vis_pair[0]
        img2_idx = vis_pair[1]
        db_index = vis_pair[2]

        # 按照之前的设定，db_index都为0，读取的应该都是dataset1的根目录，所以img1和img2都是dataset1中图片，[:-1]的作用是去除末尾的\n
        img1 = cv2.imread(data_dir[db_index] + img_files[db_index][img1_idx][:-1])
        img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY).astype('float32') / 255.
        img_stack.append(img1)

        img2 = cv2.imread(data_dir[db_index] + img_files[db_index][img2_idx][:-1])
        img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY).astype('float32') / 255.
        img_stack.append(img2)

        # img1和img2都放入到img_stack中处理，并将其转化为numpy格式
        img_arr = np.asarray(img_stack)

        grad_stack = []
        desc_grad_stack = []
        loss_stack = []

        heatmap, coarse_desc, log_map, coarse_org = model_bbone.run(img_arr)
        for itera in range(3):
            # 随机采样，得到关键点和对应的描述子
            pts_stack_1, desc_stack_1, inv_prob_arr = model_bbone.key_pt_sampling(img_arr, heatmap, samp_pts,
                                                                                  coarse_desc)
            # 主要作用是求得匹配
            matched, desc_shape = desc_map(pts_stack_1, desc_stack_1, cr_check)
            desc_grad_mini = []
            loss_mini = []
            for desc_itera in range(3):
                pts_stack, desc_stack, desc_grad, match_samples = desc_sampling(matched, pts_stack_1, desc_stack_1)
                loss = error_calculator(pts_stack, desc_stack, lamda1, match_samples, cal_db, img1_idx, img2_idx,
                                         db_index, threshold)
                if (np.isnan(loss[0, 0]) == True):
                    loss[0, 0] = temp
                loss_stack.append(loss[0, 0])
                #         print(np.sum(loss))
                temp = loss[0, 0]
                loss_mini.append(temp)
                desc_grad_mini.append(desc_grad)
                grad_stack.append(inv_prob_arr)

            loss_arr_mini = np.asarray(loss_mini)
            desc_grad_arr_mini = np.asarray(desc_grad_mini)
            # 求平均
            mean_loss_mini = np.mean(loss_arr_mini)
            loss_arr_mini = loss_arr_mini - mean_loss_mini
            desc_grad_upd = np.sum(loss_arr_mini[:, np.newaxis, np.newaxis, np.newaxis] * desc_grad_arr_mini,
                                   axis=0)
            desc_grad_stack.append(
                torch.autograd.grad(desc_stack_1, coarse_desc, torch.from_numpy(desc_grad_upd / 3.0).float())[
                    0].cpu().numpy())

        loss_arr = np.asarray(loss_stack)
        grad_arr = np.asarray(grad_stack)
        desc_grad_arr = np.asarray(desc_grad_stack)
        mean_loss = np.mean(loss_arr)
        std_loss = np.std(loss_arr)
        mean_loss_heap.append(mean_loss)
        min_loss_heap.append(np.amin(loss_arr))
        print(mean_loss, np.amin(loss_arr), std_loss)
        loss_arr = loss_arr - mean_loss

        update = np.sum(loss_arr[:, np.newaxis, np.newaxis, np.newaxis] * grad_arr, axis=0)
        #     update_desc = np.sum(loss_arr[:, np.newaxis, np.newaxis, np.newaxis, np.newaxis]*desc_grad_arr, axis = 0)
        update_desc = np.sum(desc_grad_arr, axis=0)
        #     print("iteration : ", ite)
        print("epoch : ", epoch, "iteration :", i)
        #     print(len(loss_arr), len(loss_mini), len(grad_stack), len(desc_grad_stack))

        torch.autograd.backward([log_map, coarse_org], [torch.from_numpy(update / 9.0).cuda().float(),
                                                        torch.from_numpy(
                                                            (lamda1 * update_desc) / 3.0).cuda().float()])

        optimizer.step()
        #        scheduler.step()

        optimizer.zero_grad()

    end = time.time()
    print("time taken for this epoch: ", end - start)
    # 每十个epoch保存一下模型
    if epoch % 10 == 0:
        with open(output_dir + "/mean_loss_heap_ransac.txt", "wb") as fp:  # Pickling
            pickle.dump(mean_loss_heap, fp)
        with open(output_dir + "/min_loss_heap_ransac.txt", "wb") as fp:  # Pickling
            pickle.dump(min_loss_heap, fp)
        torch.save(model_bbone.net.state_dict(), output_dir + '/ransac_cross_check_True.pth')

model_bbone.run()的代码如下：

    def run(self, img):

        # img1里包含了img1和img2
        assert img.ndim == 3, 'Image must be grayscale.'
        assert img.dtype == np.float32, 'Image must be float32.'
        H, W = img.shape[1], img.shape[2]
        inp = img.copy()
        inp = (inp.reshape(2, H, W))
        # 变为torch类型
        inp = torch.from_numpy(inp)
        # 变为Variable变量，进而可以装载梯度信息
        inp = torch.autograd.Variable(inp).view(2, 1, H, W)


        if self.cuda:
            inp = inp.cuda()
        # Forward pass of network.
        outs = self.net.forward(inp)
        semi, coarse_desc_org = outs[0], outs[1]
        coarse_desc = torch.tensor(coarse_desc_org.data, requires_grad=True)

        semi = torch.squeeze(semi)
        # semi的shape是[2,65,128,128],8*8的ceil+1层的heat map, 1024=128*8,128*128相当于原始的1024*1024的图片被8*8的ceil切割成128*128
        semi_dense = torch.ones(semi.shape[0], semi.shape[2], semi.shape[3]).cuda()
        # w为什么这样求解dense还不是很理解
        semi_dense[0, :, :] = torch.log(torch.sum(torch.exp(semi[0, :, :, :]), 0) + .00001)
        semi_dense[1, :, :] = torch.log(torch.sum(torch.exp(semi[1, :, :, :]), 0) + .00001)
        dense = torch.ones(semi.shape[0], semi.shape[1], semi.shape[2], semi.shape[3]).cuda()
        dense[0, :, :, :] = semi[0, :, :, :] - semi_dense[0, :, :]
        dense[1, :, :, :] = semi[1, :, :, :] - semi_dense[1, :, :]
        # 最后一层则为初步的heat map
        nodust = dense[:, :-1, :, :]
        Hc = int(H / self.cell)
        Wc = int(W / self.cell)
        nodust = nodust.transpose(1, 2)
        nodust = nodust.transpose(2, 3)
        no_dust = torch.reshape(nodust, [2, Hc, Wc, self.cell, self.cell])
        no_dust = no_dust.transpose(2, 3)
        no_dust = torch.reshape(no_dust, [2, Hc * self.cell, Wc * self.cell])
        heatmap = torch.exp(no_dust)
        #         print(no_dust.type())

        t1 = torch.sum(torch.sum(heatmap[0, :, :])) + .00001
        t2 = torch.sum(torch.sum(heatmap[1, :, :])) + .00001
        heat_map = torch.ones(2, heatmap.shape[1], heatmap.shape[2])
        # 归一化处理
        heat_map[0, :, :] = heatmap[0, :, :] / t1
        heat_map[1, :, :] = heatmap[1, :, :] / t2
        prob_map = heat_map.data.cpu().numpy()

        xs1, ys1 = np.where(prob_map[0, :, :] >= self.conf_thresh)  # Confidence threshold.
        print(len(xs1))
        return heat_map, coarse_desc, no_dust, coarse_desc_org

model_bbone.key_pt_sampling()函数的代码如下：

    def key_pt_sampling(self, img, heat_map, sampl_pts, coarse_desc):

        H, W = img.shape[1], img.shape[2]
        pts_stack = []
        #         desc_stack = []
        # 有关键的位置为1，没有的位置为0
        inv_prob_stack = []
        prob_map = heat_map.data.cpu().numpy()
        sampled1 = np.amin([sampl_pts, 2000])
        pts1 = np.zeros((3, sampled1))  # Populate point data sized 3xN.
        prob_array1 = np.ravel(prob_map[0, :, :])
        #         print(prob_array1)
        #         if (np.sum(prob_array1) != 1):
        #             print("I'm here bitches")
        #             prob_array1[np.argmax(prob_array1)] += 1 - np.sum(prob_array1)
        img_array1 = np.arange(prob_map[0, :, :].shape[0] * prob_map[0, :, :].shape[1])
        desc_stack = torch.zeros(2, 256, sampled1)

        temp = np.random.choice(img_array1, sampled1, p=prob_array1, replace=True)
        # 随机取点，恢复点的坐标(x,y)，np.divide(除法，后面转换为int，相当于求的第几行，即x的值，np.mod(求余，可以求得第几列))
        x_ind1 = (np.divide(temp, prob_map[0, :, :].shape[1])).astype(int)
        y_ind1 = (np.mod(temp, prob_map[0, :, :].shape[1])).astype(int)
        pts1[0, :] = y_ind1
        pts1[1, :] = x_ind1
        pts1[2, :] = prob_map[0, x_ind1, y_ind1]

        inv_prob1 = np.zeros((prob_map[0, :, :].shape[0], prob_map[0, :, :].shape[1]))
        #         inv_prob1[x_ind1, y_ind1] = 1/pts1[2,:]
        inv_prob1[x_ind1, y_ind1] = 1
        inv_prob_stack.append(inv_prob1)

        # --- Process descriptor.
        # 得到相应的描述子
        D1 = coarse_desc.shape[1]
        if pts1.shape[1] == 0:
            desc1 = np.zeros((D1, 0))
        else:
            # Interpolate into descriptor map using 2D point locations.
            samp_pts1 = torch.from_numpy(pts1[:2, :].copy())
            samp_pts1[0, :] = (samp_pts1[0, :] / (float(W) / 2.)) - 1.
            samp_pts1[1, :] = (samp_pts1[1, :] / (float(H) / 2.)) - 1.
            samp_pts1 = samp_pts1.transpose(0, 1).contiguous()
            samp_pts1 = samp_pts1.view(1, 1, -1, 2)
            samp_pts1 = samp_pts1.float()
            samp_pts1 = samp_pts1.cuda()

            coarse1 = coarse_desc[0, :, :, :].unsqueeze(0)
            desc1 = nn.functional.grid_sample(coarse1, samp_pts1)
            desc1 = desc1.reshape(D1, -1)
            desc1a = desc1 / (torch.norm(desc1, p=2, dim=0))
        #             desc1b = desc1a.data.cpu().numpy()
        pts_stack.append(pts1)
        #         desc_stack.append(desc1a)
        desc_stack[0, :, :] = desc1a

        xs2, ys2 = np.where(prob_map[1, :, :] >= self.conf_thresh / 100.0)  # Confidence threshold.
        #         if len(xs2) == 0:
        #             return np.zeros((3, 0)), None, None

        # 对img2进行类似的处理，随机采点并且获得其对应的描述子
        sampled2 = np.amin([sampl_pts, 2000])
        pts2 = np.zeros((3, sampled2))  # Populate point data sized 3xN.
        prob_array2 = np.ravel(prob_map[1, :, :])
        #         if (np.sum(prob_array2) != 1):
        #             prob_array2[np.argmax(prob_array2)] += 1 - np.sum(prob_array2)
        img_array2 = np.arange(prob_map[1, :, :].shape[0] * prob_map[1, :, :].shape[1])

        temp = np.random.choice(img_array2, sampled2, p=prob_array2, replace=True)
        x_ind2 = (np.divide(temp, prob_map[1, :, :].shape[1])).astype(int)
        y_ind2 = (np.mod(temp, prob_map[1, :, :].shape[1])).astype(int)
        pts2[0, :] = y_ind2
        pts2[1, :] = x_ind2
        pts2[2, :] = prob_map[1, x_ind2, y_ind2]

        inv_prob2 = np.zeros((prob_map[1, :, :].shape[0], prob_map[1, :, :].shape[1]))
        #         inv_prob2[x_ind2, y_ind2] = 1/pts2[2,:]
        inv_prob2[x_ind2, y_ind2] = 1
        inv_prob_stack.append(inv_prob2)

        # --- Process descriptor.
        D2 = coarse_desc.shape[1]
        if pts2.shape[1] == 0:
            desc2 = np.zeros((D, 0))
        else:
            # Interpolate into descriptor map using 2D point locations.
            samp_pts2 = torch.from_numpy(pts2[:2, :].copy())
            samp_pts2[0, :] = (samp_pts2[0, :] / (float(W) / 2.)) - 1.
            samp_pts2[1, :] = (samp_pts2[1, :] / (float(H) / 2.)) - 1.
            samp_pts2 = samp_pts2.transpose(0, 1).contiguous()
            samp_pts2 = samp_pts2.view(1, 1, -1, 2)
            samp_pts2 = samp_pts2.float()
            samp_pts2 = samp_pts2.cuda()

            coarse2 = coarse_desc[1, :, :, :].unsqueeze(0)
            desc2 = nn.functional.grid_sample(coarse2, samp_pts2)
            desc2 = desc2.reshape(D2, -1)
            desc2a = desc2 / (torch.norm(desc2, p=2, dim=0))
        #             desc2b = desc2a.data.cpu().numpy()

        pts_stack.append(pts2)
        #         desc_stack.append(desc2a)
        desc_stack[1, :, :] = desc2a
        inv_prob_arr = np.asarray(inv_prob_stack)

        return pts_stack, desc_stack, inv_prob_arr

desc_sampling()函数的代码如下：

def desc_sampling(matches, pts_stack, desc_stack):
    desc1 = desc_stack[0, :, :].data.cpu().numpy().T
    desc2 = desc_stack[1, :, :].data.cpu().numpy().T
    p_stack = []
    d_stack = []
    match_dist = np.zeros((len(matches), 1))

    # 获得全部匹配上的描述子
    d1 = np.zeros((256, len(matches)))
    d2 = np.zeros((256, len(matches)))

    for i in range(len(matches)):
        match_dist[i, 0] = matches[i].distance
        d1[:, i] = desc1[np.int(matches[i].queryIdx), :]
        d2[:, i] = desc2[np.int(matches[i].trainIdx), :]

    match_prob = softmax(match_dist).reshape(len(matches, ))
    #     match_prob = scipy.special.softmax(-match_dist).reshape(len(matches,))
    match_array = np.arange(len(matches))
    # 选择匹配数目一半的描述子进行随机采样
    match_sampling = np.int(0.5 * len(matches))
    temp = np.random.choice(match_array, match_sampling, p=match_prob, replace=True)
    #     temp_prob = match_prob[temp]

    p1 = np.zeros((3, match_sampling))  # Populate point data sized 3xN.
    p2 = np.zeros((3, match_sampling))
    d_1 = np.zeros((256, match_sampling))
    d_2 = np.zeros((256, match_sampling))
    prob_samp = np.zeros((len(matches)))

    for j in range(match_sampling):
        p1[:, j] = pts_stack[0][:, np.int(matches[temp[j]].queryIdx)]
        p2[:, j] = pts_stack[1][:, np.int(matches[temp[j]].trainIdx)]
        d_1[:, j] = desc1[np.int(matches[temp[j]].queryIdx), :]
        d_2[:, j] = desc2[np.int(matches[temp[j]].trainIdx), :]

    p_stack.append(p1)
    p_stack.append(p2)
    d_stack.append(d_1)
    d_stack.append(d_2)

    prob_samp[temp] = 1
    #     print(prob_samp)

    d_grad = []
    d1_tensor = torch.tensor(d1, requires_grad=True)
    d2_tensor = torch.tensor(d2, requires_grad=True)
    # 计算img1和img2对应描述子的差
    new_norm = torch.norm(d1_tensor - d2_tensor, p=2, dim=0)
    #     print(new_norm.shape)
    softy = nn.functional.log_softmax(-new_norm, dim=0)
    #     softy = softmax_t(new_norm)
    #     res = softya - softy
    # 反传描述子的差
    softy.backward(torch.from_numpy(prob_samp))
    d1_grad = torch.zeros(256, desc1.shape[0])
    d2_grad = torch.zeros(256, desc2.shape[0])
    # 记录描述子的梯度
    for k in range(len(matches)):
        d1_grad[:, np.int(matches[k].queryIdx)] = d1_tensor.grad[:, k]
        d2_grad[:, np.int(matches[k].trainIdx)] = d2_tensor.grad[:, k]
    d_grad.append(d1_grad.data.cpu().numpy())
    d_grad.append(d2_grad.data.cpu().numpy())

    #     print(d1_grad[:,0])

    return p_stack, d_stack, d_grad, match_sampling

error_calculator()函数的代码如下：

def error_calculator(pts_stack, desc_stack, lamda, matches, cal_db, img1_idx, img2_idx, db_index, inlierThreshold):
    desc1 = desc_stack[0].T
    desc2 = desc_stack[1].T
    tot_loss = np.zeros((1, 1))
    pts1 = np.zeros((1, matches, 2))
    pts2 = np.zeros((1, matches, 2))
    tot_loss = np.zeros((1, 1))

    pts1[0, :, :] = pts_stack[0].T[:, 0:2]
    pts2[0, :, :] = pts_stack[1].T[:, 0:2]

    K1 = cal_db[db_index][img1_idx][0]
    K2 = cal_db[db_index][img2_idx][0]

    pts1 = cv2.undistortPoints(pts1, K1, None)
    pts2 = cv2.undistortPoints(pts2, K2, None)
    # 设置相机的内参矩阵为单位阵，个人感觉存在问题
    K = np.eye(3, 3)

    # 计算本质矩阵E
    E, mask = cv2.findEssentialMat(pts1, pts2, K, method=cv2.FM_RANSAC, threshold=inlierThreshold)
    # 通过E求解R，t
    inliers, R, t, mask = cv2.recoverPose(E, pts1, pts2, K, mask=mask)

    #     print("Found %d good matches." % len(matches), "Final inlier count: ", inliers)

    # print("Estimate: ")
    # print(R)
    # print(t)

    GT_R1 = cal_db[db_index][img1_idx][1]
    GT_R2 = cal_db[db_index][img2_idx][1]
    # 得到真实的旋转矩阵
    GT_R_Rel = np.matmul(GT_R2, np.transpose(GT_R1))

    GT_t1 = cal_db[db_index][img1_idx][2]
    GT_t2 = cal_db[db_index][img2_idx][2]
    # 得到真实的平移矩阵
    GT_t_Rel = GT_t2.T - np.matmul(GT_R_Rel, GT_t1.T)

    # print("Ground Truth:")
    # print(GT_R_Rel)
    # print(GT_t_Rel)

    dR = np.matmul(R, np.transpose(GT_R_Rel))
    # 转换为旋转向量
    dR = cv2.Rodrigues(dR)[0]
    dR = np.linalg.norm(dR) * 180 / math.pi

    dT = float(np.dot(GT_t_Rel.T, t))
    dT /= float(np.linalg.norm(GT_t_Rel))
    dT = math.acos(dT) * 180 / math.pi

    tot_loss[0, 0] = np.amax([dR, dT])
    tempu_loss = np.copy(tot_loss[0, 0])
    if (tot_loss[0, 0] > 25):
        tot_loss[0, 0] = np.sqrt(25 * tempu_loss)
    if (tot_loss[0, 0] > 75):
        tot_loss[0, 0] = 75
    return tot_loss