def just_refine():
# INIT monodepth_single session
sess = monodepth_single.init_monodepth(args.mono_checkpoint_path)
img1 = cv2.resize(cv2.imread("pose_estimation/stereo.jpeg"),
(im_size[1], im_size[0]),
interpolation=cv2.INTER_CUBIC)
img2 = cv2.resize(cv2.imread("pose_estimation/stereo(1).jpeg"),
(im_size[1], im_size[0]),
interpolation=cv2.INTER_CUBIC)
gray1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
gray2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
# INIT camera matrix
cam_matrix = get_camera_matrix()
try:
cam_matrix_inv = np.linalg.inv(cam_matrix)
except:
raise (Error, "Verify camera matrix")
# Image is 3 channel, frame is grayscale
#ret,image,frame = get_camera_image()
# Predict depth
ini_depth = monodepth_single.get_depth_map(sess, img1)
#plt.imshow(ini_depth)
#pslt.savefig('cnn.png')
# Initalisation
#ini_uncertainty = find_uncertainty.get_initial_uncertainty()
#cv2.imwrite("cnn2.jpg",ini_depth)
disparity, depth = stereo_match.for_just_refine(gray1, gray2)
cv2.imwrite("disp_main.jpg", disparity)
"""
fused_disparity = 0.33*disparity + 0.66*ini_depth
plt.subplot(2,2,1),plt.imshow(img1,cmap = 'gray')
plt.title('Original'), plt.xticks([]), plt.yticks([])
plt.subplot(2,2,2),plt.imshow(ini_depth,cmap = 'gray')
plt.title('CNN - Predicted depth'), plt.xticks([]), plt.yticks([])
plt.subplot(2,2,3),plt.imshow(disparity,cmap = 'gray')
plt.title('Stereo matching results'), plt.xticks([]), plt.yticks([])
plt.subplot(2,2,4),plt.imshow(fused_disparity,cmap = 'gray')
plt.title('Fused Results'), plt.xticks([]), plt.yticks([])
plt.show()
plt.savefig('output1.png')
"""
plt.imshow(ini_depth)
plt.savefig('kitti_original.png')
plt.imshow(disparity)
plt.savefig('stereo_matched.png')
#fused = 0.33*ini_depth + 0.66*disparity
#plt.imshow(fused)
#plt.show()
#plt.savefig('fused.png')
cloud_for_vis(img1, disparity)
#import pose_estimation.depth_map_fusion as depth_map_fusion
#import pose_estimation.stereo_match as stereo_match
#from params import *
#import pose_estimation.camera_pose_estimation as camera_pose_estimation
#import pose_estimation.find_uncertainty as find_uncertainty
#from keyframe_utils import Keyframe as Keyframe
import monodepth_infer.monodepth_single as monodepth_single
parser = argparse.ArgumentParser(
description='Monodepth TensorFlow implementation.')
parser.add_argument(
'--mono_checkpoint_path',
default="checkpoints/model_kitti_resnet/model_kitti_resnet.data",
type=str,
help='path to a specific checkpoint to load')
parser.add_argument('--input_height',
type=int,
help='input height',
default=480)
parser.add_argument('--input_width', type=int, help='input width', default=640)
args = parser.parse_args()
sess = monodepth_single.init_monodepth(args.mono_checkpoint_path)
img1 = cv2.resize(cv2.imread("pose_estimation/cityscapes2.jpg"),
(im_size[1], im_size[0]),
interpolation=cv2.INTER_CUBIC)
# Predict depth
ini_depth = monodepth_single.get_depth_map(sess, img1)
plt.imshow(ini_depth)
plt.show()
def main():
# INIT monodepth_single session
sess = monodepth_single.init_monodepth(args.mono_checkpoint_path)
keyf1 = cv2.resize(cv2.imread("pose_estimation/stereo.jpeg"),
(im_size[1], im_size[0]),
interpolation=cv2.INTER_CUBIC)
# INIT camera matrix
cam_matrix = get_camera_matrix()
try:
cam_matrix_inv = np.linalg.inv(cam_matrix)
except:
raise (Error, "Verify camera matrix")
# Image is 3 channel, frame is grayscale
#ret,image,frame = get_camera_image()
# List of keyframe objects
K = []
# Predict depth
image = cv2.imread("pose_estimation/stereo.jpeg")
ini_depth = monodepth_single.get_depth_map(sess, keyf1)
plt.imshow(ini_depth)
plt.show()
# Initalisation
ini_uncertainty = find_uncertainty.get_initial_uncertainty()
ini_pose, ini_covariance = camera_pose_estimation.get_initial_pose()
ini_covariance = camera_pose_estimation.get_initial_covariance()
K.append(
Keyframe(ini_pose, ini_depth, ini_uncertainty, frame, image,
ini_covariance))
cur_keyframe = K[0]
cur_index = 0
prev_frame = cur_keyframe.I
prev_pose = cur_keyframe.T
while (True):
ret, image, frame = get_camera_image() # frame is the numpy array
if not ret:
_exit_program()
# Finds the high gradient pixel locations in the current frame
u = get_highgrad_element(frame)
# Finds pose of current frame by minimizing photometric residual (wrt prev keyframe)
T, C, _ = camera_pose_estimation.minimize_cost_func(
u, frame, cur_keyframe)
if check_keyframe(T):
# If it is a keyframe, add it to K after finding depth and uncertainty map
depth = monodepth_single.get_depth_map(sess, image)
cur_index += 1
uncertainty = find_uncertainty.get_uncertainty(
T, D, K[cur_index - 1])
# T = np.append(T,np.array([[0,0,0,1]]),0)
# cur_keyframe.T = np.append(cur_keyframe.T,np.array([[0,0,0,1]]),0)
# T_abs = np.matmul(T,cur_keyframe.T) # absolute pose of the new keyframe
# T = T[:3]
# cur_keyframe.T = cur_keyframe.T[:3]
K.append(Keyframe(T, depth, uncertainty, frame, image, C))
K[cur_index].D, K[cur_index].U = depth_map_fusion.fuse_depth_map(
K[cur_index], K[cur_index - 1])
cur_keyframe = K[cur_index]
point_cloud = graph_optimization.pose_graph_optimization(K)
# Visualise point cloud
else: # Refine and fuse depth map. Stereo matching consecutive frame
D_frame = stereo_match.stereo_match(prev_frame, frame, prev_pose,
T)
U_frame = find_uncertainty.get_uncertainty(T, D, cur_keyframe)
frame_obj = Keyframe(T, D_frame, U_frame,
frame) # frame as a keyframe object
cur_keyframe.D, cur_keyframe.U = depth_map_fusion.fuse_depth_map(
frame_obj, cur_keyframe)
# Generate and visualise point cloud?
_delay()
prev_frame = frame
prev_pose = T
continue
dataset = tf.data.Dataset.list_files(args.path+"/rgb/*")
dataset = dataset.map(_parse_fn)
dataset = dataset.batch(batch_size)
dataset = dataset.make_one_shot_iterator()
example, image = dataset.get_next()
# Instantiate monodepth model
model = monodepth_model.MonodepthModel(params, "test", image[0], None)
disp_pp = model.disp_left_est[0]
# GPU options
config = tf.ConfigProto()
config.gpu_options.allow_growth = True
with tf.Session(config = config) as sess:
monodepth_single.init_monodepth(args.checkpoint_path,sess = sess)
i = 1
# Run dataset iterator
while True:
try:
print ("Count ", i)
_example, _image, _disp = sess.run([example,
image,
model.disp_left_est[0]])
_disp_pp = monodepth_single.post_process_disparity(_disp.squeeze().astype(np.float32))
save(_disp_pp, _example)
i+=1