import math import os import time import warnings import numpy as np import torch import yaml from skimage.measure import ransac from skimage.transform import EuclideanTransform from tqdm import tqdm import net import tools from dataset import KittiTotalLoader warnings.filterwarnings("ignore") def npto_XYZRPY(rotmatrix): ''' Usa mathutils per trasformare una matrice di trasformazione omogenea in xyzrpy https://docs.blender.org/api/master/mathutils.html# WARNING: funziona in 32bits quando le variabili numpy sono a 64 bit :param rotmatrix: np array :return: np array with the xyzrpy ''' # qui sotto corrisponde a # quat2eul([ 0.997785 -0.0381564 0.0358964 0.041007 ],'XYZ') roll = math.atan2(-rotmatrix[1, 2], rotmatrix[2, 2]) pitch = math.asin(rotmatrix[0, 2]) yaw = math.atan2(-rotmatrix[0, 1], rotmatrix[0, 0]) x = rotmatrix[:3, 3][0] y = rotmatrix[:3, 3][1] z = rotmatrix[:3, 3][2] return np.array([x, y, z, roll, pitch, yaw]) def yt_error(pose1, pose2): distance = np.linalg.norm((pose1 - pose2)[0:3, 3]) yaw1 = npto_XYZRPY(pose1)[-1] yaw2 = npto_XYZRPY(pose2)[-1] yaw1 = yaw1 % (2 * np.pi) yaw2 = yaw2 % (2 * np.pi) dyaw = abs(yaw1 - yaw2) % (2 * np.pi) angle = dyaw * 180 / np.pi if angle > 180: angle = 360 - angle return distance, angle def rt_error(pose1, pose2): r0 = pose1[:3, :3] r1 = pose2[:3, :3] dr = np.linalg.inv(r0) @ r1 angle = np.arccos(np.clip((np.trace(dr) - 1) / 2, -1, 1)) * 180 / np.pi distance = np.sqrt(np.sum(np.square(pose1[:3, -1] - pose2[:3, -1]))) return distance, angle def pose_err(vlads=None, dataset=None, positive_distance=4., kpts=None, feas=None, model=None, num_cand=10): poses = dataset.poses error_ransac = [] error_uot = [] gt = dataset.gt pairs = [] num_reverse = 0 for i in range(len(gt)): sample = gt[i] idx_query = sample['idx'] positive_idxs = sample['positive_idxs'] for j in range(len(positive_idxs)): idx_positive = positive_idxs[j] if idx_query < idx_positive: pairs.append([idx_query, idx_positive]) _, r = yt_error(poses[idx_query], poses[idx_positive]) if r > 90: num_reverse = num_reverse + 1 # print('Total %d frames, %d pair of loops, %d[%.3f] reverse'%(len(poses),len(pairs),num_reverse,num_reverse/len(pairs))) cnt_ransca = 0 pairs = np.asarray(pairs) idx = np.argsort(pairs[:, 1]) pairs = pairs[idx] times_uot=[] times_ransac=[] for i in tqdm(range(len(pairs)),ncols=60): fea_query = feas[pairs[i][0]] p1 = kpts[pairs[i][0]].cpu().detach().numpy() fea_cand = feas[pairs[i][1]] p2 = kpts[pairs[i][1]].cpu().detach().numpy() pose_to_frame = np.matmul(np.linalg.inv(poses[pairs[i][1]]), poses[pairs[i][0]]) st_ransac=time.time() idx1, idx2, dis = tools.nn_match(fea_query, fea_cand, 'cosine') ransac_flag=False if len(idx1) >= 20: idx1 = idx1.cpu().detach().numpy() idx2 = idx2.cpu().detach().numpy() try: result_ransac, inliers = ransac((p1[idx1, 0:3], p2[idx2, 0:3]), model_class=EuclideanTransform, min_samples=15, max_trials=3, residual_threshold=1.7) num_inlier = np.sum(inliers) if num_inlier > 30: # ransac存在一致的点且无缩 cnt_ransca = cnt_ransca + 1 error_ransac.append(yt_error(pose_to_frame, result_ransac.params)) ransac_flag=True except: pass # if not ransac_flag: # try: # result_ransac, inliers = ransac((p1[idx1, 0:3], p2[idx2, 0:3]), model_class=EuclideanTransform,min_samples=5,max_trials=10,residual_threshold=5) # num_inlier = np.sum(inliers) # cnt_ransca = cnt_ransca + 1 # error_ransac.append(yt_error(pose_to_frame, result_ransac.params)) # except: # pass times_ransac.append(time.time()-st_ransac) fea1 = feas[pairs[i], :, :].to(model.epsilon.device).permute(0, 2, 1) kpts1 = kpts[pairs[i], :, :].to(model.epsilon.device) bd = {'fea_kpt': fea1, 'key_points': kpts1} st_uot=time.time() bd = model(bd) times_uot.append(time.time()-st_uot) pose_estimate1 = bd['transformation'].squeeze(0).cpu().detach().numpy() pose_estimate1 = np.vstack((pose_estimate1, [0, 0, 0, 1])) error_uot.append(yt_error(pose_to_frame, pose_estimate1)) ransac_rate = cnt_ransca / len(pairs) error_ransac = np.asarray(error_ransac) error_uot = np.asarray(error_uot) # np.save('error_ransac_%02d.npy' % dataset.sequence, error_ransac) # np.save('error_uot_%02d.npy' % dataset.sequence, error_uot) et_ransac = np.mean(error_ransac[:, 0]) er_ransac = np.mean(error_ransac[:, 1]) et_uot = np.mean(error_uot[:, 0]) er_uot = np.mean(error_uot[:, 1]) print('uot time: ',np.array(times_uot).mean()) print('ransac time: ',np.array(times_ransac).mean()) return et_ransac, er_ransac, et_uot, er_uot, ransac_rate def estimate_pose(database): try: with open(os.path.join(os.getcwd(), "config.yaml"), "r") as ymlfile: cfg = yaml.load(ymlfile, Loader=yaml.SafeLoader) print('Loading config file from %s' % os.path.join(os.getcwd(), "config.yaml")) except: with open(os.path.join(os.getcwd(), "project/BevNvLcd/config.yaml"), "r") as ymlfile: cfg = yaml.load(ymlfile, Loader=yaml.SafeLoader) print('Loading config file from %s' % os.path.join(os.getcwd(), "project/BevNvLcd/config.yaml")) cfg = cfg['experiment'] path_result = cfg['path_result'] _, _, loader_test = KittiTotalLoader(cfg) model = net.Fusion(cfg) device = torch.device("cuda" if torch.cuda.is_available() else "cpu") model = model.to(device) checkpoint = torch.load(tools.path_join(path_result, 'models', cfg['last_model'])) model.load_state_dict(checkpoint['model']) uot = model.uot # uot = model.bev.uot vlads = database['vlads'] key_points = database['key_points'] fea_kpt = database['fea_kpt'] sequences = database['sequences'] print() print('****************************************************************************') end = 0 for i in range(len(loader_test.dataset.datasets)): start = end end = start + len(loader_test.dataset.datasets[i]) # end=0 # start=0 et_ransac, er_ransac, et_uot, er_uot, rate = pose_err(vlads=vlads[start:end], dataset=loader_test.dataset.datasets[i], kpts=key_points[start:end], feas=fea_kpt[start:end], model=uot) print('Sequence %02d' % (torch.unique(sequences)[i])) print('ransac rate:%.4f, translation error:%.4f[m], rototion error:%.4f[deg]' % (rate, float(et_ransac), float(er_ransac))) print('uot translation error:%.4f[m], uot rototion error:%.4f[deg],' % (float(et_uot), float(er_uot))) print('****************************************************************************') if __name__ == '__main__': # CUDA_VISIBLE_DEVICES=5 python evaluate_pose.py # CUDA_VISIBLE_DEVICES=2 nohup python -u evaluate_pose.py >03090.log 2>&1 & # fuser /dev/nvidia* database = torch.load('/data4/caodanyang/results/FUSIONLCD/07030/database/database_fusion.pth.tar') estimate_pose(database)