import numpy as np
import numpy.linalg as la
from svd_solve import svd, svd_solve
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from fit_plane_LSE import fit_plane_LSE, fit_plane_LSE_RANSAC
points_file = open('cluttered_table.txt', 'r')
points_lines = points_file.readlines()
points_array = [[float(s) for s in line.strip().split(' ')]
for line in points_lines]
points = np.array(points_array, dtype=np.float32)
# homogeneous coord
points = np.concatenate((points, np.ones((points.shape[0], 1))), axis=1)
p, inlier_list = fit_plane_LSE_RANSAC(points)
x = np.arange(np.min(points[:, 0]), np.max(points[:, 0]), 0.1)
z = np.arange(np.min(points[:, 2]), np.max(points[:, 2]), 0.1)
xx, zz = np.meshgrid(x, z)
yy = (-p[0] * xx - p[2] * zz - p[3]) / p[1]
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(points[:, 0], points[:, 1], points[:, 2], c='r')
ax.plot_wireframe(xx, yy, zz)
plt.show()
def evaluate(opt):
"""Evaluates a pretrained model using a specified test set
"""
MIN_DEPTH = 1e-3
MAX_DEPTH = 80
K = np.array([[0.58, 0, 0.5, 0],
[0, 1.92, 0.5, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]], dtype=np.float32)
assert sum((opt.eval_mono, opt.eval_stereo)) == 1, \
"Please choose mono or stereo evaluation by setting either --eval_mono or --eval_stereo"
if opt.ext_disp_to_eval is None:
opt.load_weights_folder = os.path.expanduser(opt.load_weights_folder)
assert os.path.isdir(opt.load_weights_folder), \
"Cannot find a folder at {}".format(opt.load_weights_folder)
print("-> Loading weights from {}".format(opt.load_weights_folder))
filenames = readlines(os.path.join(splits_dir, opt.eval_split, "test_files.txt"))
encoder_path = os.path.join(opt.load_weights_folder, "encoder.pth")
decoder_path = os.path.join(opt.load_weights_folder, "depth.pth")
encoder_dict = torch.load(encoder_path)
dataset = datasets.KITTIRAWDataset(
opt.data_path, filenames,
encoder_dict['height'], encoder_dict['width'],
[0], 4, is_train=False)
dataloader = DataLoader(
dataset, 16, shuffle=False, num_workers=opt.num_workers,
pin_memory=True, drop_last=False)
encoder = networks.ResnetEncoder(opt.num_layers, False)
depth_decoder = networks.DepthDecoder(encoder.num_ch_enc)
model_dict = encoder.state_dict()
encoder.load_state_dict({k: v for k, v in encoder_dict.items() if k in model_dict})
depth_decoder.load_state_dict(torch.load(decoder_path))
encoder.cuda()
encoder.eval()
depth_decoder.cuda()
depth_decoder.eval()
pred_disps = []
print("-> Computing predictions with size {}x{}".format(
encoder_dict['width'], encoder_dict['height']))
with torch.no_grad():
for data in dataloader:
input_color = data[("color", 0, 0)].cuda()
if opt.post_process:
# Post-processed results require each image to have two forward passes
input_color = torch.cat((input_color, torch.flip(input_color, [3])), 0)
output = depth_decoder(encoder(input_color))
pred_disp, _ = disp_to_depth(output[("disp", 0)], opt.min_depth, opt.max_depth)
pred_disp = pred_disp.cpu()[:, 0].numpy()
if opt.post_process:
N = pred_disp.shape[0] // 2
pred_disp = batch_post_process_disparity(pred_disp[:N], pred_disp[N:, :, ::-1])
pred_disps.append(pred_disp)
pred_disps = np.concatenate(pred_disps)
else:
# Load predictions from file
print("-> Loading predictions from {}".format(opt.ext_disp_to_eval))
pred_disps = np.load(opt.ext_disp_to_eval)
if opt.eval_eigen_to_benchmark:
eigen_to_benchmark_ids = np.load(
os.path.join(splits_dir, "benchmark", "eigen_to_benchmark_ids.npy"))
pred_disps = pred_disps[eigen_to_benchmark_ids]
if opt.eval_object:
object_masks = []
for line in filenames:
line = line.split()
folder, frame_index = line[0], int(line[1])
object_mask_filename = os.path.join(
os.path.dirname(__file__),
"object_masks",
folder,
"{:010d}.npy".format(int(frame_index)))
object_mask = np.load(object_mask_filename)
object_masks.append(object_mask)
if opt.save_pred_disps:
output_path = os.path.join(
opt.load_weights_folder, "disps_{}_split.npy".format(opt.eval_split))
print("-> Saving predicted disparities to ", output_path)
np.save(output_path, pred_disps)
if opt.no_eval:
print("-> Evaluation disabled. Done.")
quit()
elif opt.eval_split == 'benchmark':
save_dir = os.path.join(opt.load_weights_folder, "benchmark_predictions")
print("-> Saving out benchmark predictions to {}".format(save_dir))
if not os.path.exists(save_dir):
os.makedirs(save_dir)
for idx in range(len(pred_disps)):
disp_resized = cv2.resize(pred_disps[idx], (1216, 352))
depth = STEREO_SCALE_FACTOR / disp_resized
depth = np.clip(depth, 0, 80)
depth = np.uint16(depth * 256)
save_path = os.path.join(save_dir, "{:010d}.png".format(idx))
cv2.imwrite(save_path, depth)
print("-> No ground truth is available for the KITTI benchmark, so not evaluating. Done.")
quit()
gt_path = os.path.join(splits_dir, opt.eval_split, "gt_depths.npz")
gt_depths = np.load(gt_path, fix_imports=True, encoding='latin1', allow_pickle=True)["data"]
print("-> Evaluating")
if opt.eval_stereo:
print(" Stereo evaluation - "
"disabling median scaling, scaling by {}".format(STEREO_SCALE_FACTOR))
opt.scaling = "disable"
opt.pred_depth_scale_factor = STEREO_SCALE_FACTOR
else:
print(" Mono evaluation - using median scaling")
errors = []
ratios = []
ex_logs = []
mean_scale = []
side_map = {"2": 2, "3": 3, "l": 2, "r": 3}
#resize_ori = transforms.Resize((pred_disps.shape[1],pred_disps.shape[2]),interpolation=Image.ANTIALIAS)
for i in range(pred_disps.shape[0]):
gt_depth = gt_depths[i]
gt_height, gt_width = gt_depth.shape[:2]
line = filenames[i].split()
folder = line[0]
frame_index = line[1]
side = side_map[line[2]]
color = pil_loader(get_image_path(folder,int(frame_index),side))
#color = pil_loader('/mnt/sdb/xuefeng_data/dkit_dataset/20200629_mechanical_fast/images/{:006d}.png'.format(i))
#color = color.crop((0,191,640,383))
pred_disp = pred_disps[i]
pred_disp = cv2.resize(pred_disp, (gt_width, gt_height))
pred_depth = 1 / pred_disp
if opt.eval_split == "eigen":
mask = np.logical_and(gt_depth > MIN_DEPTH, gt_depth < MAX_DEPTH)
crop = np.array(
[0.40810811 * gt_height, 0.99189189 * gt_height,
0.03594771 * gt_width, 0.96405229 * gt_width]).astype(np.int32)
crop_mask = np.zeros(mask.shape)
crop_mask[crop[0]:crop[1], crop[2]:crop[3]] = 1
mask = np.logical_and(mask, crop_mask)
if opt.eval_object:
object_mask = object_masks[i].astype(np.bool)
else:
mask = gt_depth > 0
if opt.scaling == "gt":
ratio = np.median(gt_depth[mask]) / np.median(pred_depth[mask])
if opt.eval_object:
mask = np.logical_and(mask, object_mask)
elif opt.scaling == "dgc":
scale_recovery = ScaleRecovery(1, gt_height, gt_width, K).cuda()
#scale_recovery = ScaleRecovery(1, 192, 640, K).cuda()
pred_depth = torch.from_numpy(pred_depth).unsqueeze(0).cuda()
ratio1,surface_normal1,ground_mask1,cam_points1 = scale_recovery(pred_depth)
#ratio = ratio1.cpu().item()
surface_normal = surface_normal1.cpu()[0,0,:,:].numpy()
ground_mask = ground_mask1.cpu()[0,0,:,:].numpy()
pred_depth = pred_depth[0].cpu().numpy()
cam_points=cam_points1.cpu().numpy()
cam_points2=cam_points.transpose(1,2,0)
cam_points_masked = cam_points2[np.where(ground_mask==1)]
np.random.shuffle(cam_points_masked)
cam_points4 = np.array(cam_points_masked)
print(cam_points4.shape)
cam_points4 = cam_points4[:2000,:]
cam_points3 = np.concatenate((cam_points4, np.ones((cam_points4.shape[0], 1))), axis=1)
print(cam_points3.shape)
plane,inliers = fit_plane_LSE_RANSAC(cam_points3)
#print(plane)
ratio_rans = abs(1.65 / plane[-1])
else:
ratio = 1
#print(ratio)
#print(max(pred_depth))
#print(min(pred_depth))
pred_depth_ori = pred_depth*mask
gt_depth_ori = gt_depth*mask
pred_depth_ori = np.where(mask==1,pred_depth_ori,1)
pred_depth = pred_depth[mask]
gt_depth = gt_depth[mask]
#mean_scale.append(np.mean(gt_depth/pred_depth))
'''
error_try = 100
scale_abs = 0
for ratio_try in np.arange(0.1,50,step=0.1):
pred_depth1=pred_depth * ratio_try
error_tmp = compute_errors(gt_depth, pred_depth1)[0]
#print(error_tmp)
if error_tmp < error_try:
error_try = error_tmp
scale_abs = ratio_try
div_scale = gt_depth_ori / pred_depth_ori
#print(div_scale.shape)
div_values1 = div_scale[mask]
div_scale = (div_scale-scale_abs)/scale_abs
div_values = div_scale[mask]
div_rmse = sqrt(sum((div_values1-scale_abs)*(div_values1-scale_abs))/len(div_values1))
print(min(div_values),max(div_values))
ex_logs.append([i,min(div_values), max(div_values), div_rmse,scale_abs])
#print(div_scale.shape)
#div_scale = div_scale/np.max(div_scale)
mu = np.mean(div_values1)
sigma = np.std(div_values1)
print(min(div_values1),max(div_values1))
fig,ax=plt.subplots()
n, bins, patches = ax.hist(div_values1,150,range=(3,130),density = True)
y = norm.pdf(bins, mu, 0.8*sigma)
ax.plot(bins, y, 'r')
plt.xlabel('Scale')
plt.ylabel('Density')
plt.savefig(os.path.join(os.path.dirname(__file__), "hist_imgs2","{:010d}.jpg".format(i)))
plt.close()
#blend_img = blending_imgs(div_scale, color,i)
#blend_img.save(os.path.join(os.path.dirname(__file__), "blend_imgs","{:010d}.jpg".format(i)))
blending_imgs(surface_normal,color,i,'surface_normals')
blending_imgs(ground_mask,color,i,'ground_masks')
'''
blending_imgs(ground_mask,color,i,ground_mask)
pred_depth *= ratio_rans
ratios.append(ratio_rans)
pred_depth[pred_depth < MIN_DEPTH] = MIN_DEPTH
pred_depth[pred_depth > MAX_DEPTH] = MAX_DEPTH
#blending_imgs(div_scale, color,i,mask)
if len(gt_depth) != 0:
errors.append(compute_errors(gt_depth, pred_depth))
'''
fl = open('ex.txt','w')
fl.writelines(str(ex_logs))
fl.close()
'''
#np.save('mean_scale.npy', mean_scale)
ratios = np.array(ratios)
med = np.median(ratios)
print(" Scaling ratios | med: {:0.3f} | std: {:0.3f}".format(med, np.std(ratios / med)))
mean_errors = np.array(errors).mean(0)
print("\n " + ("{:>8} | " * 7).format("abs_rel", "sq_rel", "rmse", "rmse_log", "a1", "a2", "a3"))
print(("&{: 8.3f} " * 7).format(*mean_errors.tolist()) + "\\\\")
print("\n-> Done!")
import numpy.linalg as la
from svd_solve import svd, svd_solve
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from fit_plane_LSE import fit_plane_LSE, fit_plane_LSE_RANSAC
points_file = open('clean_hallway.txt', 'r')
#points_file = open('cluttered_hallway.txt', 'r')
points_lines = points_file.readlines()
points_array = [[float(s) for s in line.strip().split(' ')] for line in points_lines]
points = np.array(points_array, dtype=np.float32)
# homogeneous coord
points = np.concatenate((points, np.ones((points.shape[0], 1))), axis=1)
p_set_1 = points
p1, inlier_list1, outlier_list1 = fit_plane_LSE_RANSAC(p_set_1, return_outlier_list=True)
p_set_2 = p_set_1[outlier_list1, :]
p2, inlier_list2, outlier_list2 = fit_plane_LSE_RANSAC(p_set_2, return_outlier_list=True)
p_set_3 = p_set_2[outlier_list2, :]
p3, inlier_list3, outlier_list3 = fit_plane_LSE_RANSAC(p_set_3, return_outlier_list=True)
p_set_4 = p_set_3[outlier_list3, :]
p4, inlier_list4, outlier_list4 = fit_plane_LSE_RANSAC(p_set_4, return_outlier_list=True)
'''
x = np.arange(np.min(points[:, 0]), np.max(points[:, 0]), 0.1)
z = np.arange(np.min(points[:, 2]), np.max(points[:, 2]), 0.1)
xx, zz = np.meshgrid(x, z)
yy = (-p[0]*xx -p[2]*zz -p[3])/p[1]
'''