def get_camK(self, folder, frame_index):
calib_path = os.path.join(self.data_path, folder.split("/")[0])
velo_filename = os.path.join(
self.data_path, folder,
"velodyne_points/data/{:010d}.bin".format(int(frame_index)))
cam2cam = read_calib_file(
os.path.join(calib_path, 'calib_cam_to_cam.txt'))
velo2cam = read_calib_file(
os.path.join(calib_path, 'calib_velo_to_cam.txt'))
velo2cam = np.hstack(
(velo2cam['R'].reshape(3, 3), velo2cam['T'][..., np.newaxis]))
velo2cam = np.vstack((velo2cam, np.array([0, 0, 0, 1.0])))
# get image shape
im_shape = cam2cam["S_rect_02"][::-1].astype(np.int32)
sfy = self.height / im_shape[0]
sfx = self.width / im_shape[1]
scaleM = np.eye(4)
scaleM[0, 0] = sfx
scaleM[1, 1] = sfy
# compute projection matrix velodyne->image plane
R_cam2rect = np.eye(4)
R_cam2rect[:3, :3] = cam2cam['R_rect_00'].reshape(3, 3)
P_rect = np.eye(4)
P_rect[0:3, :] = cam2cam['P_rect_0' + str(2)].reshape(3, 4)
P_rect = scaleM @ P_rect
P_rect = P_rect[0:3, :]
P_velo2im = np.dot(np.dot(P_rect, R_cam2rect), velo2cam)
# Confused
# cam2cam['P_rect_0' + str(2)][0] / im_shape[1] * self.width
# cam2cam['P_rect_0' + str(2)][0] * sfy
real_camK = np.eye(4)
real_camK[0:3, 0:4] = P_velo2im
real_invcamK = np.linalg.inv(real_camK)
realIn = np.eye(4)
realIn[0:3, 0:4] = P_rect
realEx = R_cam2rect @ velo2cam
if self.load_gt_velodine:
velo = load_velodyne_points(velo_filename)
velo = velo[velo[:, 0] >= 0, :]
np.random.shuffle(velo)
velo = velo[0:10000, :]
else:
velo = None
real_camera_params = dict()
real_camera_params['camK'] = real_camK.astype(np.float32)
real_camera_params['invcamK'] = real_invcamK.astype(np.float32)
real_camera_params['realIn'] = realIn.astype(np.float32)
real_camera_params['realEx'] = realEx.astype(np.float32)
return real_camera_params, velo
def get_intrinsic(self, folder, side):
cam2cam = read_calib_file(
os.path.join(self.data_path,
folder.split('/')[0], 'calib_cam_to_cam.txt'))
K = np.eye(4)
K[0:3, :] = cam2cam['P_rect_0{}'.format(
self.dirmapping[side][-1])].reshape(3, 4)
return K
def val(self):
"""Validate the model on a single minibatch
"""
fpath = '/home/shengjie/Documents/Project_SemanticDepth/splits/eigen/test_files.txt'
val_filenames = readlines(fpath)
btspred = '/media/shengjie/c9c81c9f-511c-41c6-bfe0-2fc19666fb32/Bts_Pred/result_bts_eigen_v2_pytorch_densenet161/pred'
kittiroot = '/home/shengjie/Documents/Data/Kitti/kitti_raw/kitti_data'
vlsroot = '/media/shengjie/disk1/visualization/shapeintegrationpred/bts'
os.makedirs(vlsroot, exist_ok=True)
crph = 352
crpw = 1216
count = 0
for entry in val_filenames:
seq, index, dir = entry.split(' ')
rgb = pil.open(
os.path.join(kittiroot, seq, 'image_02', "data",
"{}.png".format(index)))
w, h = rgb.size
top = int(h - crph)
left = int((w - crpw) / 2)
calibpath = os.path.join(kittiroot,
seq.split('/')[0], 'calib_cam_to_cam.txt')
cam2cam = read_calib_file(calibpath)
K = np.eye(4)
K[0:3, :] = cam2cam['P_rect_02'].reshape(3, 4)
K[0, 2] = K[0, 2] - left
K[1, 2] = K[1, 2] - top
K = torch.from_numpy(K).unsqueeze(0).float()
pred = pil.open(
os.path.join(btspred,
"{}_{}.png".format(seq.split('/')[1], index)))
pred = np.array(pred).astype(np.float) / 256.0
predtorch = torch.from_numpy(pred).unsqueeze(0).unsqueeze(
0).float()
prednorm = self.sfnormOptimizer.depth2norm(depthMap=predtorch,
intrinsic=K)
fig1 = tensor2disp(1 / predtorch, vmax=0.15, ind=0)
fig2 = tensor2rgb((prednorm + 1) / 2, ind=0)
fig = np.concatenate([np.array(fig1), np.array(fig2)], axis=1)
pil.fromarray(fig).save(
os.path.join(
vlsroot, "{}_{}.png".format(
seq.split('/')[1],
str(index).zfill(10))))
count = count + 1
print("%s finished" % entry)
def get_camK(self, folder, frame_index):
calib_path = os.path.join(self.data_path, folder.split("/")[0])
velo_filename = os.path.join(
self.data_path, folder,
"velodyne_points/data/{:010d}.bin".format(int(frame_index)))
cam2cam = read_calib_file(
os.path.join(calib_path, 'calib_cam_to_cam.txt'))
velo2cam = read_calib_file(
os.path.join(calib_path, 'calib_velo_to_cam.txt'))
velo2cam = np.hstack(
(velo2cam['R'].reshape(3, 3), velo2cam['T'][..., np.newaxis]))
velo2cam = np.vstack((velo2cam, np.array([0, 0, 0, 1.0])))
# get image shape
im_shape = cam2cam["S_rect_02"][::-1].astype(np.int32)
sfx = self.height / im_shape[0]
sfy = self.width / im_shape[1]
scaleM = np.eye(4)
scaleM[0, 0] = sfx
scaleM[1, 1] = sfy
# compute projection matrix velodyne->image plane
R_cam2rect = np.eye(4)
R_cam2rect[:3, :3] = cam2cam['R_rect_00'].reshape(3, 3)
P_rect = np.eye(4)
P_rect[0:3, :] = cam2cam['P_rect_0' + str(2)].reshape(3, 4)
P_rect = scaleM @ P_rect
P_rect = P_rect[0:3, :]
P_velo2im = np.dot(np.dot(P_rect, R_cam2rect), velo2cam)
camK = np.eye(4)
camK[0:3, 0:4] = P_velo2im
invcamK = np.linalg.inv(camK)
realIn = np.eye(4)
realIn[0:3, 0:4] = P_rect
realEx = R_cam2rect @ velo2cam
if not self.is_train:
velo = load_velodyne_points(velo_filename)
velo = velo[velo[:, 0] >= 0, :]
np.random.shuffle(velo)
velo = velo[0:10000, :]
else:
velo = None
# Check
# ptsprojected = (realIn @ realEx @ velo.T).T
# ptsprojected[:,0] = ptsprojected[:,0] / ptsprojected[:,2]
# ptsprojected[:, 1] = ptsprojected[:, 1] / ptsprojected[:, 2]
# minx = ptsprojected[:, 0].min()
# maxx = ptsprojected[:, 0].max()
# miny = ptsprojected[:, 1].min()
# maxy = ptsprojected[:, 1].max()
#
# ptsprojected = (P_velo2im @ velo.T).T
# ptsprojected[:,0] = ptsprojected[:,0] / ptsprojected[:,2]
# ptsprojected[:, 1] = ptsprojected[:, 1] / ptsprojected[:, 2]
# minx = ptsprojected[:, 0].min()
# maxx = ptsprojected[:, 0].max()
# miny = ptsprojected[:, 1].min()
# maxy = ptsprojected[:, 1].max()
# P_rect @ R_cam2rect @ velo2cam - (realIn @ realEx)[0:3,:]
return camK, invcamK, realIn, realEx, velo
def val(self):
"""Validate the model on a single minibatch
"""
self.set_eval()
minang = -np.pi / 3 * 2
maxang = 2 * np.pi - np.pi / 3 * 2
vind = 0
semidense_depthfolder = '/media/shengjie/c9c81c9f-511c-41c6-bfe0-2fc19666fb32/Data/kitti/semidense_gt'
rawdata_root = '/home/shengjie/Documents/Data/Kitti/kitti_raw/kitti_data'
semantics_root = '/home/shengjie/Documents/Data/Kitti/kitti_raw/kitti_data'
dirmapping = {'l': 'image_02', 'r': 'image_03'}
vlsfold = '/home/shengjie/Documents/tmp/vls_intshape'
for entry in self.val_filenames:
entry = '2011_09_26/2011_09_26_drive_0039_sync 239 l'
seq, frame, dir = entry.split(' ')
rgbpath = os.path.join(rawdata_root, seq, dirmapping[dir], "data",
"{}.png".format(frame.zfill(10)))
depthpath = os.path.join(semidense_depthfolder, seq,
dirmapping[dir],
"{}.png".format(frame.zfill(10)))
semanticspath = os.path.join(semantics_root, seq,
'semantic_prediction',
dirmapping[dir],
"{}.png".format(frame.zfill(10)))
rgb = pil.open(rgbpath)
rgbnp = np.array(rgb)
depth = np.array(pil.open(depthpath)).astype(np.float32) / 256.0
semantics = np.array(pil.open(semanticspath))
from kitti_utils import read_calib_file
cam2cam = read_calib_file(
os.path.join(rawdata_root,
seq.split('/')[0], 'calib_cam_to_cam.txt'))
K = np.eye(4)
K[0:3, :] = cam2cam['P_rect_0{}'.format(
dirmapping[dir][-1])].reshape(3, 4)
K = torch.from_numpy(K).float().unsqueeze(0).cuda()
gtw, gth = rgb.size
sfoptimizer = SurfaceNormalOptimizer(height=gth,
width=gtw,
batch_size=1).cuda()
rgbs, weights = self.getpredmask(rgbnp)
rgbstorch = torch.from_numpy(rgbs.astype(np.float32)).permute(
[0, 3, 1, 2]).cuda() / 255.0
rgbstorch = F.interpolate(rgbstorch,
[self.opt.height, self.opt.width],
mode='bilinear',
align_corners=True)
weightstorch = torch.from_numpy(weights).float().unsqueeze(
0).expand([2, -1, -1]).cuda()
with torch.no_grad():
outputs_norm = self.models['norm'](
self.models['encoder_norm'](rgbstorch))
outputs_depth = self.models['depth'](
self.models['encoder_depth'](rgbstorch))
pred_ang_netsize = (outputs_norm[("disp", 0)] - 0.5) * 2 * np.pi
pred_ang_cropsize = F.interpolate(pred_ang_netsize,
[self.crph, self.crpw],
mode='bilinear',
align_corners=True)
pred_ang = torch.zeros([2, gth, gtw], device='cuda')
pred_ang[:, :self.crph, :self.crpw] += pred_ang_cropsize[0]
pred_ang[:, gth - self.crph:,
gtw - self.crpw:] += pred_ang_cropsize[1]
pred_ang[:, :self.crph, gtw - self.crpw:] += pred_ang_cropsize[2]
pred_ang[:, gth - self.crph:, :self.crpw] += pred_ang_cropsize[3]
pred_ang = (pred_ang / weightstorch).unsqueeze(0)
_, pred_depth_netsize = disp_to_depth(outputs_depth[("disp", 0)],
min_depth=self.opt.min_depth,
max_depth=self.opt.max_depth)
pred_depth_netsize = pred_depth_netsize * self.STEREO_SCALE_FACTOR
pred_depth_netsize = F.interpolate(pred_depth_netsize,
[self.crph, self.crpw],
mode='bilinear',
align_corners=True)
pred_depth = torch.zeros([1, gth, gtw], device='cuda')
pred_depth[:, :self.crph, :self.crpw] += pred_depth_netsize[0]
pred_depth[:, gth - self.crph:,
gtw - self.crpw:] += pred_depth_netsize[1]
pred_depth[:, :self.crph,
gtw - self.crpw:] += pred_depth_netsize[2]
pred_depth[:,
gth - self.crph:, :self.crpw] += pred_depth_netsize[3]
pred_depth = (pred_depth / weightstorch[0:1]).unsqueeze(0)
# rgb.show()
# tensor2disp(pred_ang[:, 0:1, :, :] - minang, vmax=maxang, ind=vind).show()
# tensor2disp(pred_ang[:, 1:2, :, :] - minang, vmax=maxang, ind=vind).show()
# tensor2disp(1/pred_depth, vmax=0.2, ind=0).show()
vallidarmask = depth > 0
xx, yy = np.meshgrid(range(gtw), range(gth), indexing='xy')
rndind = np.random.randint(0, len(xx[vallidarmask]))
# vlsx = xx[vallidarmask][rndind]
# vlsy = yy[vallidarmask][rndind]
vlsx = 796
vlsy = 177
log = sfoptimizer.ang2log(intrinsic=K, ang=pred_ang)
loghnp = log[0, 0].cpu().numpy()
logvnp = log[0, 1].cpu().numpy()
deptl = np.log(depth)
pred_depthl = np.log(pred_depth[0, 0].cpu().numpy())
intu = 0
intd = 0
intl = 0
intr = 0
goodpts = list()
badpts = list()
for sy in range(2, gth * 2):
if np.mod(sy, 2) == 0:
scany = int(sy / 2) + vlsy
if scany >= 0 and scany < gth:
intd = intd + logvnp[scany - 1, vlsx]
cmpd = pred_depthl[scany, vlsx] - pred_depthl[vlsy,
vlsx]
if depth[scany, vlsx] > 0:
gtd = deptl[scany, vlsx] - deptl[vlsy, vlsx]
if np.abs(intd - gtd) <= np.abs(cmpd - gtd):
goodpts.append(np.array([vlsx, scany]))
else:
badpts.append(np.array([vlsx, scany]))
else:
scany = -int(sy / 2) + vlsy
if scany >= 0 and scany < gth:
intu = intu - logvnp[scany, vlsx]
cmpd = pred_depthl[scany, vlsx] - pred_depthl[vlsy,
vlsx]
if depth[scany, vlsx] > 0:
gtd = deptl[scany, vlsx] - deptl[vlsy, vlsx]
if np.abs(intu - gtd) <= np.abs(cmpd - gtd):
goodpts.append(np.array([vlsx, scany]))
else:
badpts.append(np.array([vlsx, scany]))
for sx in range(2, gtw * 2):
if np.mod(sx, 2) == 0:
scanx = int(sx / 2) + vlsx
if scanx >= 0 and scanx < gtw:
intr = intr + loghnp[vlsy, scanx - 1]
cmpd = pred_depthl[vlsy, scanx] - pred_depthl[vlsy,
vlsx]
if depth[vlsy, scanx] > 0:
gtd = deptl[vlsy, scanx] - deptl[vlsy, vlsx]
if np.abs(intr - gtd) <= np.abs(cmpd - gtd):
goodpts.append(np.array([scanx, vlsy]))
else:
badpts.append(np.array([scanx, vlsy]))
else:
scanx = -int(sx / 2) + vlsx
if scanx >= 0 and scanx < gtw:
intl = intl - loghnp[vlsy, scanx]
cmpd = pred_depthl[vlsy, scanx] - pred_depthl[vlsy,
vlsx]
if depth[vlsy, scanx] > 0:
gtd = deptl[vlsy, scanx] - deptl[vlsy, vlsx]
if np.abs(intl - gtd) <= np.abs(cmpd - gtd):
goodpts.append(np.array([scanx, vlsy]))
else:
badpts.append(np.array([scanx, vlsy]))
cm = plt.get_cmap('magma')
colors = cm(1 / depth[vallidarmask] * 5)
plt.figure(figsize=(16, 8))
plt.imshow(rgb)
plt.scatter(xx[vallidarmask],
yy[vallidarmask],
s=0.1,
c=colors[:, 0:3])
if len(goodpts) > 0:
goodpts = np.stack(goodpts, axis=0)
plt.scatter(goodpts[:, 0], goodpts[:, 1], s=3, c='g')
if len(badpts) > 0:
badpts = np.stack(badpts, axis=0)
plt.scatter(badpts[:, 0], badpts[:, 1], s=3, c='r')
plt.scatter(vlsx, vlsy, s=3, c='b')
plt.savefig(
os.path.join(
vlsfold,
"{}_{}_{}.png".format(seq.split('/')[1], frame, dir)))
plt.close()