m = mappings[k]
if len(m) > 1:
cam2cam = read_calib_file(
os.path.join(raw_export_root, seqname, 'calib_cam_to_cam.txt'))
velo2cam = read_calib_file(
os.path.join(raw_export_root, seqname,
'calib_velo_to_cam.txt'))
imu2cam = read_calib_file(
os.path.join(raw_export_root, seqname,
'calib_imu_to_velo.txt'))
intrinsic, extrinsic = get_intrinsic_extrinsic(
cam2cam, velo2cam, imu2cam)
date, seq, frmidx = m.split(' ')
rel_pose = get_pose(root=args.kittiraw_root,
seq="{}/{}".format(date, seq),
index=int(frmidx),
extrinsic=extrinsic)
selfpose_fold = os.path.join(raw_export_root, seqname,
"{}_sync".format(seqname),
'relpose/data')
os.makedirs(selfpose_fold, exist_ok=True)
selfpose_path = os.path.join(selfpose_fold,
'{}.pickle'.format("10".zfill(10)))
wrthdler = open(selfpose_path, "wb")
pickle.dump(rel_pose, wrthdler)
wrthdler.close()
## ============================================= ##
instance_export_root = os.path.join(args.export_root, 'instance')
velo2cam = read_calib_file(
os.path.join(calib_dir, 'calib_velo_to_cam.txt'))
imu2cam = read_calib_file(
os.path.join(calib_dir, 'calib_imu_to_velo.txt'))
intrinsic, extrinsic = get_intrinsic_extrinsic(cam2cam, velo2cam,
imu2cam)
img1path = os.path.join(args.dataset_root, seq, 'image_02', 'data',
"{}.png".format(str(index).zfill(10)))
img2path = os.path.join(args.dataset_root, seq, 'image_02', 'data',
"{}.png".format(str(index + 1).zfill(10)))
if not os.path.exists(img2path):
relpose = np.eye(4)
else:
relpose = get_pose(args.dataset_root, seq, index, extrinsic)
ang = rot2ang(relpose[0:3, 0:3])
t = relpose[0:3, 3]
mvprm = np.concatenate([ang, t])
mvprms.append(mvprm)
posepred_path = os.path.join(args.pred_root, seq, 'image_02/posepred',
"{}.pickle".format(str(index).zfill(10)))
posepred = pickle.load(open(posepred_path, "rb"))[0]
ang_pred = rot2ang(posepred[0:3, 0:3])
t_pred = posepred[0:3, 3]
mvprm_pred = np.concatenate([ang_pred, t_pred])
mvprms_pred.append(mvprm_pred)
mvprms = np.array(mvprms)
scale_RANSAC = list()
for _, entry in enumerate(tqdm(entries)):
seq, frmidx, _ = entry.split(' ')
# Read GPS Locations
calib_dir = os.path.join(args.dataset_root, "{}".format(seq[0:10]))
cam2cam = read_calib_file(
os.path.join(calib_dir, 'calib_cam_to_cam.txt'))
velo2cam = read_calib_file(
os.path.join(calib_dir, 'calib_velo_to_cam.txt'))
imu2cam = read_calib_file(
os.path.join(calib_dir, 'calib_imu_to_velo.txt'))
intrinsic, extrinsic = get_intrinsic_extrinsic(
cam2cam, velo2cam, imu2cam)
try:
pose_gps = get_pose(args.dataset_root, seq, int(frmidx),
extrinsic)
except:
continue
# Read RANSAC Locations
RANSACPos_path = os.path.join(rp, seq, 'image_02',
str(frmidx).zfill(10) + '.pickle')
pose_RANSAC_dict = pickle.load(open(RANSACPos_path, "rb"))
pose_RANSAC = pose_RANSAC_dict[0]
scale_gps.append(np.sqrt(np.sum(pose_gps[0:3, 3]**2)) + 1e-10)
scale_RANSAC.append(
np.sqrt(np.sum(pose_RANSAC[0:3, 3]**2)) + 1e-10)
diff = np.log(np.array(scale_RANSAC)) - np.log(np.array(scale_gps))
fig = plt.figure()
fig.add_subplot(2, 1, 1)
def __init__(self,
entries,
entries_map,
root='datasets/KITTI',
odom_root=None,
ins_root=None,
flowPred_root=None,
mdPred_root=None,
bsposepred_root=None,
flowgt_root=None,
stereogt_root=None,
RANSACPose_root=None,
raw_root=None):
super(KITTI_eigen, self).__init__()
self.root = root
self.odom_root = odom_root
self.flowPred_root = flowPred_root
self.mdPred_root = mdPred_root
self.ins_root = ins_root
self.bsposepred_root = bsposepred_root
self.flowgt_root = flowgt_root
self.stereogt_root = stereogt_root
self.RANSACPose_root = RANSACPose_root
self.raw_root = raw_root
self.entries_map = entries_map
self.image_list = list()
self.intrinsic_list = list()
self.inspred_list = list()
self.flowPred_list = list()
self.mdPred_list = list()
self.flowGt_list = list()
self.stereoGt_list = list()
self.entries = list()
self.RANSACPose_rec = list()
self.IMUPose_rec = list()
for idx, entry in enumerate(entries):
seq, index, _ = entry.split(' ')
index = int(index)
if os.path.exists(
os.path.join(root, seq, 'image_02', 'data',
"{}.png".format(str(index).zfill(10)))):
tmproot = root
else:
tmproot = odom_root
img1path = os.path.join(tmproot, seq, 'image_02', 'data',
"{}.png".format(str(index).zfill(10)))
img2path = os.path.join(tmproot, seq, 'image_02', 'data',
"{}.png".format(str(index + 1).zfill(10)))
if not os.path.exists(img2path):
self.image_list.append([img1path, img1path])
else:
self.image_list.append([img1path, img2path])
mdDepth_path = os.path.join(self.mdPred_root, seq, 'image_02',
"{}.png".format(str(index).zfill(10)))
if not os.path.exists(mdDepth_path):
raise Exception("mD pred file %s missing" % mdDepth_path)
stereogt_path = os.path.join(self.stereogt_root, seq, 'image_02',
"{}.png".format(str(index).zfill(10)))
self.stereoGt_list.append(stereogt_path)
flowpred_path = os.path.join(self.flowPred_root, seq, 'image_02',
"{}.png".format(str(index).zfill(10)))
flowgt_path = os.path.join(self.flowgt_root, seq, 'image_02',
"{}.png".format(str(index).zfill(10)))
# Load Intrinsic for each frame
calib_dir = os.path.join(tmproot, seq.split('/')[0])
cam2cam = read_calib_file(
os.path.join(calib_dir, 'calib_cam_to_cam.txt'))
velo2cam = read_calib_file(
os.path.join(calib_dir, 'calib_velo_to_cam.txt'))
imu2cam = read_calib_file(
os.path.join(calib_dir, 'calib_imu_to_velo.txt'))
intrinsic, extrinsic = get_intrinsic_extrinsic(
cam2cam, velo2cam, imu2cam)
# Read all Ranscac poses
seq_raw, index_raw, _ = entries_map[idx].split(' ')
poses_paths = glob.glob(
os.path.join(self.RANSACPose_root, seq_raw, 'image_02',
'*.pickle'))
RANSACPose_seq = list()
for k in range(len(poses_paths)):
ps = os.path.join(self.RANSACPose_root, seq_raw, 'image_02',
'{}.pickle'.format(str(k).zfill(10)))
tmpPose = pickle.load(open(ps, "rb"))
RANSACPose_seq.append(tmpPose)
# Read all GPS Locations
imupose_list = list()
for k in range(len(poses_paths)):
if k == len(poses_paths) - 1:
imupose_list.append(np.eye(4))
else:
imupose_list.append(
get_pose(raw_root, seq_raw, int(k), extrinsic))
self.IMUPose_rec.append(imupose_list)
self.RANSACPose_rec.append(RANSACPose_seq)
if self.ins_root is not None:
inspath = os.path.join(self.ins_root, seq, 'insmap/image_02',
"{}.png".format(str(index).zfill(10)))
if not os.path.exists(inspath):
raise Exception("instance file %s missing" % inspath)
self.inspred_list.append(inspath)
self.intrinsic_list.append(intrinsic)
self.entries.append(entry)
self.flowPred_list.append(flowpred_path)
self.flowGt_list.append(flowgt_path)
self.mdPred_list.append(mdDepth_path)
assert len(self.intrinsic_list) == len(self.entries) == len(
self.image_list)
pngs = glob.glob(os.path.join(args.dataset_root, fold, 'image_02/data/*.png'))
num_img = len(pngs)
seq = fold.split('/')[0]
# Read all GPS Locations
calib_dir = os.path.join(args.dataset_root, "{}".format(fold[0:10]))
cam2cam = read_calib_file(os.path.join(calib_dir, 'calib_cam_to_cam.txt'))
velo2cam = read_calib_file(os.path.join(calib_dir, 'calib_velo_to_cam.txt'))
imu2cam = read_calib_file(os.path.join(calib_dir, 'calib_imu_to_velo.txt'))
intrinsic, extrinsic = get_intrinsic_extrinsic(cam2cam, velo2cam, imu2cam)
imupose_list = list()
for k in range(num_img):
if k == num_img - 1:
imupose_list.append(np.eye(4))
else:
imupose_list.append(get_pose(args.dataset_root, "{}/{}".format(seq[0:10], fold.split('/')[1]), k, extrinsic))
RANSACPose_list = list()
for k in range(num_img):
pose_RANSAC_path = os.path.join(args.RANSACPose_root, "{}/{}/{}".format(str(k64).zfill(3), seq[0:10], fold.split('/')[1]), 'image_02', str(k).zfill(10) + '.pickle')
if not os.path.exists(pose_RANSAC_path):
# pose_RANSAC_path = os.path.join(args.RANSACPose_root, "{}/{}/{}".format(str(0).zfill(3), seq[0:10], fold.split('/')[1]), 'image_02', str(k).zfill(10) + '.pickle')
RANSACPose_list.append(imupose_list[k])
else:
pose_RANSAC_dict = pickle.load(open(pose_RANSAC_path, "rb"))
RANSACPose_list.append(pose_RANSAC_dict[0])
positions_IMUpose = list()
stpos = np.array([[0, 0, 0, 1]]).T
positions_IMUpose.append(np.array([[0, 0, 0, 1]]))
accumP = np.eye(4)
# Read all GPS Locations
calib_dir = os.path.join(args.dataset_root, "{}".format(seq[0:10]))
cam2cam = read_calib_file(
os.path.join(calib_dir, 'calib_cam_to_cam.txt'))
velo2cam = read_calib_file(
os.path.join(calib_dir, 'calib_velo_to_cam.txt'))
imu2cam = read_calib_file(
os.path.join(calib_dir, 'calib_imu_to_velo.txt'))
intrinsic, extrinsic = get_intrinsic_extrinsic(cam2cam, velo2cam,
imu2cam)
imupose_list = list()
for k in range(len(gtposes)):
taridx = k + seqmap[seq]['stid']
imupose_list.append(
get_pose(args.dataset_root,
"{}/{}_sync".format(seq[0:10],
seq), int(k), extrinsic))
positions_IMUpose = list()
stpos = np.array([[0, 0, 0, 1]]).T
positions_IMUpose.append(stpos[0:3, 0])
accumP = np.eye(4)
for r in imupose_list:
accumP = r @ accumP
positions_IMUpose.append((np.linalg.inv(accumP) @ stpos)[0:3, 0])
positions_IMUpose = np.array(positions_IMUpose)
maxnum = positions_odompose.shape[0]
plt.figure()
plt.scatter(positions_odompose[0:maxnum, 0],
positions_odompose[0:maxnum, 1], 10)