def __getitem__(self, index):
img1 = frame_utils.read_gen(self.image_list[index][0])
img2 = frame_utils.read_gen(self.image_list[index][1])
img1 = np.array(img1).astype(np.uint8)
img2 = np.array(img2).astype(np.uint8)
depthvls = np.array(Image.open(self.depthvls_list[index])).astype(np.float32) / 256.0
intrinsic = copy.deepcopy(self.intrinsic_list[index])
rel_pose = copy.deepcopy(self.pose_list[index])
inspred = np.array(Image.open(self.inspred_list[index])).astype(np.int)
if self.RANSACPose_root is not None:
np.random.seed(index + int(time.time()))
rep_idces = np.random.choice(list(range(self.npossibility)), self.num_samples, replace=False)
# rep_idces = np.array([0, 1, 2, 3])
# rep_idces = np.array([0])
posepred = list()
ang_decps_l = list()
scl_decps_l = list()
mvd_decps_l = list()
for rep_idx in rep_idces:
posepred_cur = pickle.load(open(self.RANSACPose_list[index].format(str(rep_idx).zfill(3)), "rb"))
for l in self.linlogdedge:
scaledpose = copy.deepcopy(posepred_cur)
for kk in range(scaledpose.shape[0]):
tmpscale = np.sqrt(np.sum(scaledpose[kk, 0:3, 3] ** 2) + 1e-12)
adjustedscale = np.exp(np.log(tmpscale) + l)
scaledpose[kk, 0:3, 3] = scaledpose[kk, 0:3, 3] / tmpscale * adjustedscale
posepred.append(scaledpose)
posepred_decps = copy.deepcopy(posepred_cur)
ang_decps = np.zeros([posepred_decps.shape[0], 3])
scl_decps = np.zeros([posepred_decps.shape[0], 1])
mvd_decps = np.zeros([posepred_decps.shape[0], 3])
for k in range(posepred_decps.shape[0]):
ang_decps[k, :] = self.rot2ang(posepred_decps[k])
tmps = np.sum(posepred_decps[k, 0:3, 3] ** 2)
if tmps == 0:
scl_decps[k, 0] = 1e-6
mvd_decps[k, :] = np.array([0, 0, -1])
else:
scl_decps[k, 0] = np.sqrt(tmps)
mvd_decps[k, :] = posepred_decps[k, 0:3, 3] / scl_decps[k, 0]
ang_decps_l.append(ang_decps)
scl_decps_l.append(scl_decps)
mvd_decps_l.append(mvd_decps)
posepred = np.stack(posepred, axis=0)
ang_decps = np.stack(ang_decps_l, axis=0)
scl_decps = np.stack(scl_decps_l, axis=0)
mvd_decps = np.stack(mvd_decps_l, axis=0)
else:
posepred = None
ang_decps = None
scl_decps = None
mvd_decps = None
inspred, posepred, ang_decps_pad, scl_decps_pad, mvd_decps_pad = self.pad_clip_ins(insmap=inspred, posepred=posepred, ang_decps=ang_decps, scl_decps=scl_decps, mvd_decps=mvd_decps)
if not hasattr(self, 'deepv2dpred_root'):
self.deepv2dpred_root = None
if self.depth_root is not None:
depth = np.array(Image.open(self.depth_list[index])).astype(np.float32) / 256.0
else:
depth = None
if self.deepv2dpred_root is not None:
depthpred_deepv2d = np.array(Image.open(self.deepv2dpredpath_list[index])).astype(np.float32) / 256.0
posepred_deepv2d = read_deepv2d_pose(self.deepv2dpredpose_list[index])
else:
depthpred_deepv2d = None
posepred_deepv2d = None
if self.mdPred_root is not None:
mdDepth_pred = np.array(Image.open(self.mdDepthpath_list[index])).astype(np.float32) / 256.0
else:
mdDepth_pred = None
if self.flowPred_root is not None:
flowpred_RAFT, valid_flow = readFlowKITTI(self.flowpredpath_list[index])
else:
flowpred_RAFT = None
img1, img2, depth, depthvls, depthpred_deepv2d, mdDepth_pred, inspred, flowpred_RAFT, intrinsic = self.aug_crop(img1, img2, depth, depthvls, depthpred_deepv2d, mdDepth_pred, inspred, flowpred_RAFT, intrinsic)
if self.depth_root is not None:
flowgt = self.get_gt_flow(depth=depth, valid=(inspred==0) * (depth>0), intrinsic=intrinsic, rel_pose=rel_pose)
else:
flowgt = None
flowgt_vls = self.get_gt_flow(depth=depthvls, valid=(inspred==0) * (depthvls>0), intrinsic=intrinsic, rel_pose=rel_pose)
if self.istrain and not self.muteaug:
img1, img2 = self.colorjitter(img1, img2)
entry = self.entries[index]
seq, frmidx = entry.split(' ')
frmidx = int(frmidx)
seq = seq.split('/')[1]
IMUposes_seg = list()
RANSACposes_seg = list()
IMU_poses_reced = self.IMU_poses_rec[seq]
RANSAC_poses_reced = self.RANSAC_poses_rec[seq]
for k in range(frmidx - self.span, frmidx + self.span):
if k < 0:
IMUposes_seg.append(np.eye(4))
RANSACposes_seg.append(np.eye(4))
elif k >= len(IMU_poses_reced):
IMUposes_seg.append(np.eye(4))
RANSACposes_seg.append(np.eye(4))
else:
IMUposes_seg.append(IMU_poses_reced[k])
RANSACposes_seg.append(RANSAC_poses_reced[k])
# Forward move
IMUlocations1 = list()
leftarrs1 = list()
rightarrs1 = list()
accumM = np.eye(4)
leftarr = np.eye(4)
rightarr = np.eye(4)
for k in range(len(IMUposes_seg)):
accumM = IMUposes_seg[k] @ accumM
if k < self.span:
rightarr = RANSACposes_seg[k] @ rightarr
if k > self.span:
leftarr = RANSACposes_seg[k] @ leftarr
if k >= self.span:
IMUlocations1.append(np.linalg.inv(accumM)[0:3, 3:4])
leftarrs1.append(leftarr)
rightarrs1.append(rightarr)
# print(np.sum(np.abs(np.linalg.inv(leftarr @ rel_pose @ rightarr)[0:3, 3] - IMUlocations1[k-self.span])))
IMUlocations2 = list()
leftarrs2 = list()
rightarrs2 = list()
accumM = np.eye(4)
leftarr = np.eye(4)
rightarr = np.eye(4)
for k in range(len(IMUposes_seg)):
accumM = np.linalg.inv(IMUposes_seg[-k]) @ accumM
if k < self.span:
rightarr = np.linalg.inv(RANSACposes_seg[-k]) @ rightarr
if k > self.span:
leftarr = np.linalg.inv(RANSACposes_seg[-k]) @ leftarr
if k >= self.span:
IMUlocations2.append(np.linalg.inv(accumM)[0:3, 3:4])
leftarrs2.append(leftarr)
rightarrs2.append(rightarr)
# print(np.sum(np.abs(np.linalg.inv(leftarr @ np.linalg.inv(rel_pose) @ rightarr)[0:3, 3] - IMUlocations2[k-self.span])))
IMUlocations1 = np.stack(IMUlocations1)
leftarrs1 = np.stack(leftarrs1)
rightarrs1 = np.stack(rightarrs1)
IMUlocations2 = np.stack(IMUlocations2)
leftarrs2 = np.stack(leftarrs2)
rightarrs2 = np.stack(rightarrs2)
data_blob = self.wrapup(img1=img1, img2=img2, flowgt=flowgt, flowgt_vls=flowgt_vls, depthmap=depth, depthvls=depthvls,
depthpred_deepv2d=depthpred_deepv2d, mdDepth_pred=mdDepth_pred, intrinsic=intrinsic, insmap=inspred, flowpred=flowpred_RAFT, rel_pose=rel_pose,
posepred=posepred, posepred_deepv2d=posepred_deepv2d, ang_decps_pad=ang_decps_pad, scl_decps_pad=scl_decps_pad, mvd_decps_pad=mvd_decps_pad, tag=self.entries[index],
IMUlocations1=IMUlocations1, leftarrs1=leftarrs1, rightarrs1=rightarrs1, IMUlocations2=IMUlocations2, leftarrs2=leftarrs2, rightarrs2=rightarrs2)
return data_blob
# entry = '2011_09_26/2011_09_26_drive_0091_sync 0000000262 l'
entry = '2011_10_03/2011_10_03_drive_0042_sync 0000000948 l'
seq, frmidx, _ = entry.split(' ')
img1_path = os.path.join(kitti_root, seq, 'image_02/data',
str(frmidx).zfill(10) + '.png')
img2_path = os.path.join(kitti_root, seq, 'image_02/data',
str(int(frmidx) + 1).zfill(10) + '.png')
pose_path = os.path.join(pose_root, seq, 'image_02',
str(frmidx).zfill(10) + '.pickle')
if not os.path.exists(img2_path):
continue
img1 = Image.open(img1_path)
img2 = Image.open(img2_path)
flow_pred, _ = readFlowKITTI(
os.path.join(optical_flow_root, seq, 'image_02',
str(frmidx).zfill(10) + '.png'))
h, w, _ = flow_pred.shape
with open(pose_path, 'rb') as f:
pose = pickle.load(f)
pose = pose[0]
R = pose[0:3, 0:3]
t = pose[0:3, 3]
intrinsic = get_intrinsic(
os.path.join(kitti_root,
seq.split('/')[0], 'calib_cam_to_cam.txt'))
intrinsic = intrinsic[0:3, 0:3]
epipole1 = intrinsic @ (-R.T @ t)
def __getitem__(self, index):
img1 = frame_utils.read_gen(self.image_list[index][0])
img2 = frame_utils.read_gen(self.image_list[index][1])
img1 = np.array(img1).astype(np.uint8)
img2 = np.array(img2).astype(np.uint8)
h, w, _ = img1.shape
size_str = "{}_{}".format(str(h), str(w))
if not self.bandepth:
depth = np.array(Image.open(self.depth_list[index])).astype(
np.float32) / 256.0
depthvls = np.array(Image.open(self.depthvls_list[index])).astype(
np.float32) / 256.0
else:
depth = np.zeros([h, w])
depthvls = np.zeros([h, w])
intrinsic = copy.deepcopy(self.intrinsic_list[index])
rel_pose = copy.deepcopy(self.pose_list[index])
inspred = np.array(Image.open(self.inspred_list[index])).astype(np.int)
if self.prediction_root is not None:
depthpred = np.array(Image.open(
self.predDepthpath_list[index])).astype(np.float32) / 256.0
else:
depthpred = None
assert not (self.prediction_root is not None
and self.RANSACPose_root is not None)
if self.prediction_root is not None:
posepred = pickle.load(open(self.predPosepath_list[index], "rb"))
elif self.RANSACPose_root is not None:
posepred = pickle.load(open(self.RANSACPose_list[index], "rb"))
else:
posepred = None
inspred, posepred = self.pad_clip_ins(insmap=inspred,
posepred=posepred)
if not hasattr(self, 'deepv2dpred_root'):
self.deepv2dpred_root = None
if self.deepv2dpred_root is not None:
depthpred_deepv2d = np.array(
Image.open(self.deepv2dpredpath_list[index])).astype(
np.float32) / 256.0
posepred_deepv2d = read_deepv2d_pose(
self.deepv2dpredpose_list[index])
else:
depthpred_deepv2d = None
posepred_deepv2d = None
if self.mdPred_root is not None:
mdDepth_pred = np.array(Image.open(
self.mdDepthpath_list[index])).astype(np.float32) / 256.0
else:
mdDepth_pred = None
if self.flowPred_root is not None:
flowpred_RAFT, valid_flow = readFlowKITTI(
self.flowpredpath_list[index])
else:
flowpred_RAFT = None
img1, img2, depth, depthvls, depthpred, depthpred_deepv2d, mdDepth_pred, inspred, flowpred_RAFT, intrinsic = self.aug_crop(
img1, img2, depth, depthvls, depthpred, depthpred_deepv2d,
mdDepth_pred, inspred, flowpred_RAFT, intrinsic)
flowgt = self.get_gt_flow(depth=depth,
valid=(inspred == 0) * (depth > 0),
intrinsic=intrinsic,
rel_pose=rel_pose)
flowgt_vls = self.get_gt_flow(depth=depthvls,
valid=(inspred == 0) * (depthvls > 0),
intrinsic=intrinsic,
rel_pose=rel_pose)
if self.istrain and not self.muteaug:
img1, img2 = self.colorjitter(img1, img2)
data_blob = self.wrapup(img1=img1,
img2=img2,
flowgt=flowgt,
flowgt_vls=flowgt_vls,
depthmap=depth,
depthvls=depthvls,
depthpred=depthpred,
depthpred_deepv2d=depthpred_deepv2d,
mdDepth_pred=mdDepth_pred,
intrinsic=intrinsic,
insmap=inspred,
flowpred=flowpred_RAFT,
rel_pose=rel_pose,
posepred=posepred,
posepred_deepv2d=posepred_deepv2d,
tag=self.entries[index],
size_str=size_str)
return data_blob
def __getitem__(self, index):
img1 = frame_utils.read_gen(self.image_list[index][0])
img2 = frame_utils.read_gen(self.image_list[index][1])
img1 = np.array(img1).astype(np.uint8)
img2 = np.array(img2).astype(np.uint8)
depthvls = np.array(Image.open(self.depthvls_list[index])).astype(
np.float32) / 256.0
intrinsic = copy.deepcopy(self.intrinsic_list[index])
rel_pose = copy.deepcopy(self.pose_list[index])
inspred = np.array(Image.open(self.inspred_list[index])).astype(np.int)
if self.RANSACPose_root is not None:
np.random.seed(index + int(time.time()))
if self.num_samples == 1:
rep_idces = [0]
elif self.num_samples == 2:
rep_idces = [0, 1]
else:
rep_idces = [0, 1, 2, 3]
# rep_idces = [0, 0, 0, 0]
posepred = list()
ang_decps_l = list()
scl_decps_l = list()
mvd_decps_l = list()
for rep_idx in rep_idces:
posepred_cur = pickle.load(
open(
self.RANSACPose_list[index].format(
str(rep_idx).zfill(3)), "rb"))
for l in self.linlogdedge:
scaledpose = copy.deepcopy(posepred_cur)
for kk in range(scaledpose.shape[0]):
tmpscale = np.sqrt(
np.sum(scaledpose[kk, 0:3, 3]**2) + 1e-12)
adjustedscale = np.exp(np.log(tmpscale) + l)
scaledpose[kk, 0:3, 3] = scaledpose[
kk, 0:3, 3] / tmpscale * adjustedscale
posepred.append(scaledpose)
posepred_decps = copy.deepcopy(posepred_cur)
ang_decps = np.zeros([posepred_decps.shape[0], 3])
scl_decps = np.zeros([posepred_decps.shape[0], 1])
mvd_decps = np.zeros([posepred_decps.shape[0], 3])
for k in range(posepred_decps.shape[0]):
ang_decps[k, :] = self.rot2ang(posepred_decps[k])
tmps = np.sum(posepred_decps[k, 0:3, 3]**2)
if tmps == 0:
scl_decps[k, 0] = 1e-6
mvd_decps[k, :] = np.array([0, 0, -1])
else:
scl_decps[k, 0] = np.sqrt(tmps)
mvd_decps[k, :] = posepred_decps[k, 0:3,
3] / scl_decps[k, 0]
ang_decps_l.append(ang_decps)
scl_decps_l.append(scl_decps)
mvd_decps_l.append(mvd_decps)
posepred = np.stack(posepred, axis=0)
ang_decps = np.stack(ang_decps_l, axis=0)
scl_decps = np.stack(scl_decps_l, axis=0)
mvd_decps = np.stack(mvd_decps_l, axis=0)
else:
posepred = None
ang_decps = None
scl_decps = None
mvd_decps = None
relpose_odom_gt = copy.deepcopy(self.gt_relpose_rec[index])
inspred, posepred, ang_decps_pad, scl_decps_pad, mvd_decps_pad = self.pad_clip_ins(
insmap=inspred,
posepred=posepred,
ang_decps=ang_decps,
scl_decps=scl_decps,
mvd_decps=mvd_decps)
if not hasattr(self, 'deepv2dpred_root'):
self.deepv2dpred_root = None
if self.depth_root is not None:
depth = np.array(Image.open(self.depth_list[index])).astype(
np.float32) / 256.0
else:
depth = None
if self.deepv2dpred_root is not None:
depthpred_deepv2d = np.array(
Image.open(self.deepv2dpredpath_list[index])).astype(
np.float32) / 256.0
posepred_deepv2d = read_deepv2d_pose(
self.deepv2dpredpose_list[index])
else:
depthpred_deepv2d = None
posepred_deepv2d = None
if self.mdPred_root is not None:
mdDepth_pred = np.array(Image.open(
self.mdDepthpath_list[index])).astype(np.float32) / 256.0
else:
mdDepth_pred = None
if self.flowPred_root is not None:
flowpred_RAFT, valid_flow = readFlowKITTI(
self.flowpredpath_list[index])
else:
flowpred_RAFT = None
img1, img2, depth, depthvls, depthpred_deepv2d, mdDepth_pred, inspred, flowpred_RAFT, intrinsic = self.aug_crop(
img1, img2, depth, depthvls, depthpred_deepv2d, mdDepth_pred,
inspred, flowpred_RAFT, intrinsic)
if self.depth_root is not None:
flowgt = self.get_gt_flow(depth=depth,
valid=(inspred == 0) * (depth > 0),
intrinsic=intrinsic,
rel_pose=rel_pose)
else:
flowgt = None
flowgt_vls = self.get_gt_flow(depth=depthvls,
valid=(inspred == 0) * (depthvls > 0),
intrinsic=intrinsic,
rel_pose=rel_pose)
if self.istrain and not self.muteaug:
img1, img2 = self.colorjitter(img1, img2)
data_blob = self.wrapup(img1=img1,
img2=img2,
flowgt=flowgt,
flowgt_vls=flowgt_vls,
depthmap=depth,
depthvls=depthvls,
depthpred_deepv2d=depthpred_deepv2d,
mdDepth_pred=mdDepth_pred,
intrinsic=intrinsic,
insmap=inspred,
flowpred=flowpred_RAFT,
rel_pose=rel_pose,
posepred=posepred,
posepred_deepv2d=posepred_deepv2d,
relpose_odom_gt=relpose_odom_gt,
ang_decps_pad=ang_decps_pad,
scl_decps_pad=scl_decps_pad,
mvd_decps_pad=mvd_decps_pad,
tag=self.entries[index])
return data_blob
def __getitem__(self, index):
if self.is_test:
frame_id = self.image_list[index][0]
frame_id = frame_id.split('/')[-1]
img1 = frame_utils.read_gen(self.image_list[index][0])
img2 = frame_utils.read_gen(self.image_list[index][1])
img1 = np.array(img1).astype(np.uint8)[..., :3]
img2 = np.array(img2).astype(np.uint8)[..., :3]
img1 = torch.from_numpy(img1).permute(2, 0, 1).float()
img2 = torch.from_numpy(img2).permute(2, 0, 1).float()
return img1, img2, frame_id
else:
if not self.init_seed:
worker_info = torch.utils.data.get_worker_info()
if worker_info is not None:
np.random.seed(worker_info.id)
random.seed(worker_info.id)
self.init_seed = True
index = index % len(self.image_list)
frame_id = self.image_list[index][0]
frame_id = frame_id.split('/')[-1]
img1 = frame_utils.read_gen(self.image_list[index][0])
img2 = frame_utils.read_gen(self.image_list[index][1])
flow, valid = frame_utils.readFlowKITTI(self.flow_list[index])
img1 = np.array(img1).astype(np.uint8)[..., :3]
img2 = np.array(img2).astype(np.uint8)[..., :3]
if self.do_augument:
img1, img2, flow, valid = self.augumentor(
img1, img2, flow, valid)
img1 = torch.from_numpy(img1).permute(2, 0, 1).float()
img2 = torch.from_numpy(img2).permute(2, 0, 1).float()
flow = torch.from_numpy(flow).permute(2, 0, 1).float()
valid = torch.from_numpy(valid).float()
if self.do_pad:
ht, wd = img1.shape[1:]
pad_ht = (((ht // 8) + 1) * 8 - ht) % 8
pad_wd = (((wd // 8) + 1) * 8 - wd) % 8
pad_ht1 = [0, pad_ht]
pad_wd1 = [pad_wd // 2, pad_wd - pad_wd // 2]
pad = pad_wd1 + pad_ht1
img1 = img1.view(1, 3, ht, wd)
img2 = img2.view(1, 3, ht, wd)
flow = flow.view(1, 2, ht, wd)
valid = valid.view(1, 1, ht, wd)
img1 = torch.nn.functional.pad(img1, pad, mode='replicate')
img2 = torch.nn.functional.pad(img2, pad, mode='replicate')
flow = torch.nn.functional.pad(flow,
pad,
mode='constant',
value=0)
valid = torch.nn.functional.pad(valid,
pad,
mode='replicate',
value=0)
img1 = img1.view(3, ht + pad_ht, wd + pad_wd)
img2 = img2.view(3, ht + pad_ht, wd + pad_wd)
flow = flow.view(2, ht + pad_ht, wd + pad_wd)
valid = valid.view(ht + pad_ht, wd + pad_wd)
if self.is_test:
return img1, img2, flow, valid, frame_id
return img1, img2, flow, valid