def kitti_loop(vo_fn, im_dir, seq):
i = 0
loops = []
db = []
dbkp = []
avg_rate = 0
loop_count = 0
last_loop_id = -1
skipped = False
j = 0
with open(vo_fn, 'r') as vo_f, open('kitti_traj.txt', 'w') as t_f, \
open('kitti_loops.txt', 'w') as l_f, tf.Session() as sess:
calc = utils.CALC2('model', sess, ret_c5=True)
qt = []
pts = []
ims = []
for line in vo_f.readlines():
if len(line) != 0 and line[
0] != "#": # skip comments and empty line at the end
line_split = line.split()
frame_id = str(i)
i += 1
x = line_split[3]
y = line_split[7]
pts.append([float(x), float(y)])
t_f.write(frame_id + ',' + x + ',' + y + '\n')
fl_nm = str(i).zfill(6) + ".png"
im = cv2.imread(join(im_dir, "image_2/" + fl_nm))
if im is None or im.shape[0] == 0:
print("No image: %s" % fl_nm)
break
ims.append(im)
w = int(4.0 / 3 * im.shape[0]) // 2
_w = im.shape[1] // 2
#im = im[:, (_w-w):(_w+w+1),:]
im = cv2.cvtColor(cv2.resize(im, (vw, vh)), cv2.COLOR_BGR2RGB)
t0 = time()
descr, c5 = calc.run(im)
kp, kp_d = utils.kp_descriptor(c5)
dbkp.append((kp, kp_d))
db.append(descr)
if i > 2 * N:
t1 = time()
j = close_loop(db[:-N], dbkp, descr, (kp, kp_d))
t = (time() - t1) * 1000
qt.append(str(len(db)) + "," + str(t) + "\n")
if j > 0:
if last_loop_id == -1 or loop_count == 0:
last_loop_id = j
print("LCD HYPOTHESIS: %d -> %d" % (i, j))
loop_count += 1
elif abs(j - last_loop_id) < W:
print("LCD HYPOTHESIS INCREASE: %d -> %d" % (i, j))
loop_count += 1
else:
loop_count = 0
last_loop_id = -1
skipped = False
else:
loop_count = 0
last_loop_id = -1
skipped = False
# Only time processing, not IO
if i > 0: # TF take one run to warm up. Dont fudge results
rate = 1.0 / (time() - t0)
avg_rate += (rate - avg_rate) / (i + 1)
print("Frame %d, rate = %f Hz, avg rate = %f Hz" %
(i, rate, avg_rate))
if loop_count >= C:
print("LOOP DETECTED: %d -> %d" % (i, j))
ii = len(pts) - C // 2 - 1
jj = j - W // 2
l_f.write(str(pts[ii][0]) + "," + \
str(pts[ii][1]) + "," + str(pts[jj][0]) + \
"," + str(pts[jj][1]) + "\n")
loop_count = 0
skipped = False
match = np.concatenate((ims[ii], ims[jj]), axis=1)
cv2.imwrite(
'plots/match_kitti%s_%d_%d.png' % (seq, ii, jj), match)
# remove loopp descr since we have revisited this location
db = db[:jj] + db[jj + W // 2:]
pts = pts[:jj] + pts[jj + W // 2:]
ims = ims[:jj] + ims[jj + W // 2:]
with open('kitti_q_times.txt', 'w') as q_f:
q_f.writelines(qt)
def get_prec_recall(model_dir,
data_path,
num_include=5,
title='Precision-Recall Curve',
checkpoint=None,
netvlad_feat=None,
include_calc=False):
database = [] # stored descriptors
database_kp = [] # stored kp and kp descriptors
database_labels = [] # the image labels
database_ims = []
db_calc1 = []
mem_path = data_path + "/memory"
live_path = data_path + "/live"
print("memory path: ", mem_path)
print("live path: ", live_path)
mem_files = sorted([path.join(mem_path, f) for f in listdir(mem_path)])
live_files = sorted([path.join(live_path, f) for f in listdir(live_path)])
if netvlad_feat is not None:
# Assumes netvlad file order was sorted too
db_netvlad = np.fromfile(netvlad_feat + "_db.bin",
dtype=np.float32).reshape(len(mem_files), -1)
q_netvlad = np.fromfile(netvlad_feat + "_q.bin",
dtype=np.float32).reshape(len(live_files), -1)
t_calc = []
ims = np.empty((1, calc2.vh, calc2.vw, 3), dtype=np.float32)
if include_calc:
caffe.set_mode_gpu()
# Must be placed in 'calc_model'
calc1 = caffe.Net('calc_model/deploy.prototxt',
1,
weights='calc_model/calc.caffemodel')
# Use caffe's transformer
transformer = caffe.io.Transformer({'X1': (1, 1, 120, 160)})
transformer.set_raw_scale('X1', 1. / 255)
n_incorrect = 0
with tf.compat.v1.Session() as sess:
calc = utils.CALC2(model_dir, sess, ret_c5=True, checkpoint=checkpoint)
for fl in mem_files:
print("loading image ", fl, " to database")
#ims[0] = cv2.cvtColor(cv2.resize(cv2.imread(fl), (calc2.vw, calc2.vh)),
# cv2.COLOR_BGR2RGB)
_im = cv2.imread(fl)
im = cv2.cvtColor(cv2.resize(_im, (calc2.vw, calc2.vh)),
cv2.COLOR_BGR2RGB)
database_ims.append(im)
ims[0] = im / 255.0
t0 = time()
descr, c5 = calc.run(ims)
t_calc.append(time() - t0)
kp, kp_d = kp_descriptor(c5)
database_kp.append((kp, kp_d))
database.append(descr)
database_labels.append(
int(
re.match('.*?([0-9]+)$',
path.splitext(path.basename(fl))[0]).group(1)))
if include_calc:
im = cv2.equalizeHist(
cv2.cvtColor(
cv2.resize(_im, (160, 120),
interpolation=cv2.INTER_CUBIC),
cv2.COLOR_BGR2GRAY))
calc1.blobs['X1'].data[...] = transformer.preprocess('X1', im)
calc1.forward()
d = np.copy(calc1.blobs['descriptor'].data[...])
d /= np.linalg.norm(d)
db_calc1.append(d)
correct = []
scores = []
correct_reg = []
scores_reg = []
correct_nv = []
scores_nv = []
correct_c1 = []
scores_c1 = []
if include_calc:
db_calc1 = np.concatenate(tuple(db_calc1), axis=0)
database = np.concatenate(tuple(database), axis=0)
matcher = cv2.BFMatcher(cv2.NORM_L2)
#plt.ion()
imdata = None
i = 0
for fl in live_files:
im_label_k = int(
re.match('.*?([0-9]+)$',
path.splitext(path.basename(fl))[0]).group(1))
_im = cv2.imread(fl)
im = cv2.cvtColor(cv2.resize(_im, (calc2.vw, calc2.vh)),
cv2.COLOR_BGR2RGB)
ims[0] = im / 255.0
t0 = time()
descr, c5 = calc.run(ims)
t_calc.append(time() - t0)
kp, kp_d = kp_descriptor(c5)
if netvlad_feat is not None:
sim_nv = np.sum(q_netvlad[i:i + 1, :] * db_netvlad, axis=-1)
i_max_sim_nv = np.argmax(sim_nv)
max_sim_nv = sim_nv[i_max_sim_nv]
scores_nv.append(max_sim_nv)
db_lab = database_labels[i_max_sim_nv]
correct_nv.append(
int(check_match(im_label_k, db_lab, num_include)))
if include_calc:
im = cv2.equalizeHist(
cv2.cvtColor(
cv2.resize(_im, (160, 120),
interpolation=cv2.INTER_CUBIC),
cv2.COLOR_BGR2GRAY))
calc1.blobs['X1'].data[...] = transformer.preprocess('X1', im)
calc1.forward()
d = np.copy(calc1.blobs['descriptor'].data[...])
d /= np.linalg.norm(d)
sim_c1 = np.sum(d * db_calc1, axis=-1)
i_max_sim_c1 = np.argmax(sim_c1)
max_sim_c1 = sim_c1[i_max_sim_c1]
sim = np.sum(descr * database, axis=-1)
i_max_sim_reg = np.argmax(sim)
max_sim_reg = sim[i_max_sim_reg]
t0 = time()
K = 7
top_k_sim_ind = np.argpartition(sim, -K)[-K:]
max_sim = -1.0
i_max_sim = -1
best_match_tuple = None
for k in top_k_sim_ind:
db_kp, db_kp_d = database_kp[k]
matches = matcher.knnMatch(kp_d, db_kp_d, 2)
good = []
pts1 = []
pts2 = []
for m, n in matches:
if m.distance < 0.7 * n.distance:
good.append(m)
pts1.append(db_kp[m.trainIdx].pt)
pts2.append(kp[m.queryIdx].pt)
if len(good) > 7:
pts1 = np.int32(pts1)
pts2 = np.int32(pts2)
curr_sim = sim[k]
if curr_sim > max_sim:
max_sim = curr_sim
i_max_sim = k
best_match_tuple = (kp, db_kp, good, pts1, pts2)
if i_max_sim > -1:
F, mask = cv2.findFundamentalMat(best_match_tuple[3],
best_match_tuple[4],
cv2.FM_RANSAC)
if F is None:
max_sim = -1.0
i_max_sim = -1
print("Comparison took", (time() - t0) * 1000, "ms")
scores.append(max_sim)
db_lab = database_labels[i_max_sim]
correct.append(
int(check_match(im_label_k, db_lab, num_include)
) if i_max_sim > -1 else 0)
scores_reg.append(max_sim_reg)
db_lab = database_labels[i_max_sim_reg]
correct_reg.append(
int(check_match(im_label_k, db_lab, num_include)))
if include_calc:
scores_c1.append(max_sim_c1)
db_lab = database_labels[i_max_sim_c1]
correct_c1.append(
int(check_match(im_label_k, db_lab, num_include)))
'''
if correct[-1]:
mask = np.squeeze(mask)
good = []
for i in range(len(best_match_tuple[2])):
if mask[i]:
good.append(best_match_tuple[2][i])
if 1: #imdata is None:
imdata = plt.imshow(cv2.drawMatches(im, best_match_tuple[0],
database_ims[i_max_sim], best_match_tuple[1], good,
None, flags=4))
else:
imdata.set_data(cv2.drawMatches(im, best_match_tuple[0],
database_ims[i_max_sim], best_match_tuple[1], good,
None, flags=4))
plt.pause(0.0000001)
plt.show()
'''
print("Proposed match G-CALC2:", im_label_k, ", ",
database_labels[i_max_sim], ", score = ", max_sim,
", Correct =", correct[-1])
print("Proposed match CALC2:", im_label_k, ", ",
database_labels[i_max_sim_reg], ", score = ", max_sim_reg,
", Correct =", correct_reg[-1])
if include_calc:
print("Proposed match CALC:", im_label_k, ", ",
database_labels[i_max_sim_c1], ", score = ", max_sim_c1,
", Correct =", correct_c1[-1])
if netvlad_feat is not None:
print("Proposed match NetVLAD:", im_label_k, ", ",
database_labels[i_max_sim_nv], ", score = ", max_sim_nv,
", Correct =", correct_nv[-1])
print()
i += 1
print("Mean CALC2 run time: %f ms" % (1000 * np.mean(np.array(t_calc))))
precision, recall, threshold = precision_recall_curve(correct, scores)
precision_reg, recall_reg, threshold = precision_recall_curve(
correct_reg, scores_reg)
precision_c1 = recall_c1 = None
if include_calc:
precision_c1, recall_c1, threshold = precision_recall_curve(
correct_c1, scores_c1)
pnv = rnv = None
if netvlad_feat is not None:
pnv, rnv, threshold = precision_recall_curve(correct_nv, scores_nv)
print("N Incorrect:", n_incorrect)
return precision, recall, precision_reg, recall_reg, precision_c1, recall_c1, pnv, rnv