def process_frame(img):
img = cv2.resize(img, (W, H))
frame = Frame(img, k)
frames.append(frame)
if len(frames) <= 1:
return
idx1, idx2, Rt = match_frames(frames[-1], frames[-2])
frames[-1].pose = np.dot(Rt, frames[-2].pose)
#homogenous 3-D COORDS
pts4d = triangulate(frames[-1].pose, frames[-2].pose, frames[-1].pts[idx1],
frames[-2].pts[idx2])
pts4d /= pts4d[:, 3:]
#reject points behind the camera
good_pts4d = (np.abs(pts4d[:, 3]) > 0.005) & (pts4d[:, 2] > 0)
for i, p in enumerate(pts4d):
if not good_pts4d[i]:
continue
pt = Point(p)
pt.add_observation(frames[-1], idx1[i])
pt.add_observation(frames[-2], idx2[i])
for pt1, pt2 in zip(frames[-1].pts[idx1], frames[-2].pts[idx2]):
u1, v1 = denormalize(k, pt1)
u2, v2 = denormalize(k, pt2)
cv2.circle(img, (u1, v1), color=(0, 255, 0), radius=3)
cv2.line(img, (u1, v1), (u2, v2), color=(255, 0, 0))
disp.paint(img)
def process_frame(img):
img = cv2.resize(img, (W, H))
frame = Frame(img, K)
frames.append(frame)
if len(frames) <= 1:
return
idx1, idx2, Rt = match_frames(frames[-1], frames[-2])
frames[-1].pose = np.dot(Rt, frames[-2].pose)
pts4d = triangulate(frames[-1].pose, frames[-2].pose, frames[-1].pts[idx1],
frames[-2].pts[idx2])
pts4d /= pts4d[:, 3:]
good_pts4d = (np.abs(pts4d[:, 3]) > 0.005) & (pts4d[:, 2] > 0)
#print(f'{len(matches)} matches')
for i, p in enumerate(good_pts4d):
if not good_pts4d[i]:
continue
pt = Point(p)
pt.add_observation(frames[-1], idx1[i])
pt.add_observation(frames[-2], idx2[i])
for p1, p2 in zip(frames[-1].pts[idx1], frames[-2].pts[idx2]):
u1, v1 = denormalize(K, p1)
u2, v2 = denormalize(K, p2)
cv2.circle(img, (u1, v1), radius=3, color=(0, 255, 0))
cv2.line(img, (u1, v1), (u2, v2), color=(255, 0, 0))
screen.paint(img)
def process_frame(img):
img = cv2.resize(img, (W, H))
frame = Frame(mapp, img, K)
if frame.id == 0:
return
f1 = mapp.frames[-1]
f2 = mapp.frames[-2]
idx1, idx2, Rt = match_frames(f1, f2)
f1.pose = np.dot(Rt, f2.pose)
pts4d = triangulate(f1.pose, f2.pose, f1.pts[idx1], f2.pts[idx2])
pts4d /= pts4d[:, 3:]
good_pts4d = (np.abs(pts4d[:, 3]) > 0.005) & (pts4d[:, 2] > 0)
pts4d = pts4d[good_pts4d]
for i, p in enumerate(pts4d):
if not good_pts4d[i]:
continue
pt = Point(mapp, p)
pt.add_observation(f1, idx1[i])
pt.add_observation(f2, idx2[i])
for pt1, pt2 in zip(f1.pts[idx1], f2.pts[idx2]):
u1, v1 = denormalize(K, pt1)
u2, v2 = denormalize(K, pt2)
cv2.circle(img, (u1, v1), color=(0, 255, 0), radius=3)
cv2.line(img, (u1, v1), (u2, v2), color=(255, 0, 0))
#display.paint(img)
mapp.display()
def process_frame(img):
img = cv2.resize(img, (W,H))
frame = Frame(mapp, img, K)
if frame.id == 0:
return
print("\n*** frame %d ***" % (frame.id,))
f1 = mapp.frames[-1]
f2 = mapp.frames[-2]
idx1, idx2, Rt = match_frames(f1, f2)
f1.pose = np.dot(Rt, f2.pose)
for i,idx in enumerate(idx2):
if f2.pts[idx] is not None:
f2.pts[idx].add_observation(f1, idx1[i])
good_pts4d = np.array([f1.pts[i] is None for i in idx1])
# locally in front of camera
# reject pts without enough "parallax" (this right?)
pts_tri_local = triangulate(Rt, np.eye(4), f1.kps[idx1], f2.kps[idx2])
good_pts4d &= np.abs(pts_tri_local[:, 3]) > 0.005
# homogeneous 3-D coords
# reject points behind the camera
pts_tri_local /= pts_tri_local[:, 3:]
good_pts4d &= pts_tri_local[:, 2] > 0
# project into world
pts4d = np.dot(np.linalg.inv(f1.pose), pts_tri_local.T).T
print("Adding: %d points" % np.sum(good_pts4d))
for i,p in enumerate(pts4d):
if not good_pts4d[i]:
continue
u,v = int(round(f1.kpus[idx1[i],0])), int(round(f1.kpus[idx1[i],1]))
pt = Point(mapp, p, img[v,u])
pt.add_observation(f1, idx1[i])
pt.add_observation(f2, idx2[i])
for pt1, pt2 in zip(f1.kps[idx1], f2.kps[idx2]):
u1, v1 = denormalize(K, pt1)
u2, v2 = denormalize(K, pt2)
cv2.circle(img, (u1, v1), color=(0,255,0), radius=3)
cv2.line(img, (u1, v1), (u2, v2), color=(255,0,0))
# 2-D display
if disp is not None:
disp.paint(img)
# optimize the map
if frame.id >= 4:
err = mapp.optimize()
print("Optimize: %f units of error" % err)
# 3-D display
mapp.display()
def display_frame(img, kp1, kp2):
for pt1, pt2 in zip(kp1, kp2):
u1, v1 = denormalize(K, pt1)
u2, v2 = denormalize(K, pt2)
cv2.circle(img, (u1, v1), 3, (0, 255, 0))
cv2.line(img, (u1, v1), (u2, v2), (255, 0, 0))
cv2.imshow('frame', img)
cv2.waitKey(1)
def process_frame(img):
img = cv2.resize(img, (W, H))
frame = Frame(mapp, img, K)
if frame.id == 0:
return
print("\n*** frame %d ***" % (frame.id, ))
f1 = mapp.frames[-1]
f2 = mapp.frames[-2]
idx1, idx2, Rt = match_frames(f1, f2)
#print("=============================", idx1, idx2, Rt,f1.pts,f2.pts)
f1.pose = np.dot(Rt, f2.pose)
for i, idx in enumerate(idx2):
if f2.pts[idx] is not None:
f2.pts[idx].add_observation(f1, idx1[i])
# homogeneous 3-D coords
pts4d = triangulate(f1.pose, f2.pose, f1.kps[idx1], f2.kps[idx2])
pts4d /= pts4d[:, 3:]
#print(pts4d)
# reject pts without enough "parallax" (this right?)
# reject points behind the camera
unmatched_points = np.array([f1.pts[i] is None for i in idx1])
print("Adding: %d points" % np.sum(unmatched_points))
good_pts4d = (np.abs(pts4d[:, 3]) > 0.005) & (pts4d[:, 2] >
0) & unmatched_points
#print(sum(good_pts4d), len(good_pts4d))
#print(good_pts4d)
for i, p in enumerate(pts4d):
if not good_pts4d[i]:
continue
pt = Point(mapp, p)
pt.add_observation(f1, idx1[i])
pt.add_observation(f2, idx2[i])
for pt1, pt2 in zip(f1.kps[idx1], f2.kps[idx2]):
u1, v1 = denormalize(K, pt1)
u2, v2 = denormalize(K, pt2)
cv2.circle(img, (u1, v1), color=(0, 255, 0), radius=3)
cv2.line(img, (u1, v1), (u2, v2), color=(255, 0, 0))
# 2-D display
if disp is not None:
disp.paint(img)
# optimize the map
if frame.id >= 4:
mapp.optimize()
# 3-D display
mapp.display()
def process_frame(img):
img = cv2.resize(img, (W, H))
frame = Frame(img, mapp, K)
if frame.id == 0:
return
f1 = mapp.frames[-1]
f2 = mapp.frames[-2]
idx1, idx2, Rt = match_frames(f1, f2)
f1.pose = np.dot(Rt, f2.pose)
pts4d = triangulate(f1.pose, f2.pose, f1.pts[idx1], f2.pts[idx2])
#homogeneous 3-Dcoords
pts4d /= pts4d[:, 3:]
#rejecting pts without good parallax
#reject pts behind cam
good_pts4d = (np.abs(pts4d[:, 3]) > 0.005) & (pts4d[:, 2] > 0)
#print(sum(good_pts4d), len(good_pts4d))
for i, p in enumerate(pts4d):
if not good_pts4d[i]:
continue
pt = Point(mapp, p)
pt.add_observation(f1, idx1[i])
pt.add_observation(f2, idx2[i])
#print(f1.pose)
#print(pts4d)
for pt1, pt2 in zip(f1.pts[idx1], f2.pts[idx2]):
#u1, v1 = map(lambda x: int(round(x)), pt1)
#u2, v2 = map(lambda x: int(round(x)), pt2)
u1, v1 = denormalize(K, pt1)
u2, v2 = denormalize(K, pt2)
cv2.circle(img, (u1, v1), color=(0, 255, 0), radius=3)
cv2.line(img, (u1, v1), (u2, v2), color=(255, 0, 0))
#print(img.shape)
# 2-D display
if disp is not None: disp.paint(img)
#3-D display
mapp.display()
def process_frame(img):
img = cv2.resize(img, (W, H))
frame = Frame(mapp, img, K)
if frame.id == 0:
return
f1 = mapp.frames[-1]
f2 = mapp.frames[-2]
idx1, idx2, Rt = match_frames(f1, f2)
# 삼각측량
# triangulatePoints(첫번째 카메라의 3X4 투영 행렬, 두번째 카메라의 3X4 투영 행렬, 첫번째 이미지의 특징점, 두번째 이미지의 특징점)
# 반환되는 내용은 homogeneous 좌표에서 재구성된 4XN 배열
# DLT 찾아보길
f1.pose = np.dot(Rt, f2.pose)
pts4d = triangulate(f1.pose, f2.pose, f1.pts[idx1], f2.pts[idx2])
# homogenous 좌표계의 w를 나누는 것?
# homogenous 3-D coords
pts4d /= pts4d[:, 3:]
# reject pts without enough "parallax"
# reject pts behind camera
good_pts4d = (np.abs(pts4d[:, 3]) > 0.005) & (pts4d[:, 2] > 0)
pts4d = pts4d[good_pts4d]
for i, p in enumerate(pts4d):
if not good_pts4d[i]:
continue
pt = Point(mapp, p)
pt.add_observation(f1, idx1[i])
pt.add_observation(f2, idx2[i])
# denormalize for display
# 정규화한 포인트들을 다시 화면에 맞춤
for pt1, pt2 in zip(f1.pts[idx1], f2.pts[idx2]):
u1, v1 = denormalize(K, pt1)
u2, v2 = denormalize(K, pt2)
cv2.circle(img, (u1, v1), color=(0, 255, 0), radius=3)
cv2.line(img, (u1, v1), (u2, v2), color=(255, 0, 0))
mapp.display()
cv2.imshow("image", img)
cv2.waitKey(1)
def process_frame(img):
img = cv2.resize(img, (W, H))
frame = Frame(mapp, img, K)
if frame.id == 0:
return
f1 = mapp.frames[-1]
f2 = mapp.frames[-2]
idx1, idx2, Rt = match_frames(f1, f2)
f1.pose = np.dot(Rt, f2.pose)
for i, idx in enumerate(idx2):
if f2.pts[idx] is not None:
f2.pts[idx].add_observation(f1, idx1[i])
# homogeneous 3-D coords
pts4d = triangulate(f1.pose, f2.pose, f1.kps[idx1], f2.kps[idx2])
pts4d /= pts4d[:, 3:]
# reject pts without enough "parallax" (this right?)
# reject points behind the camera
unmatched_points = np.array([f1.pts[i] is None for i in idx1])
print("Adding: {} points".format(np.sum(unmatched_points)))
good_pts4d = (np.abs(pts4d[:, 3]) > 0.005) & (pts4d[:, 2] >
0) & unmatched_points
for i, p in enumerate(pts4d):
if not good_pts4d[i]:
continue
# print(idx1[i])
# print(f1.pts[idx1[i]])
# print(f1.pts)
u, v = int(round(f1.kpus[idx1[i], 0])), int(round(f1.kpus[idx1[i], 1]))
pt = Point(mapp, p, img[v, u])
pt.add_observation(f1, idx1[i])
pt.add_observation(f2, idx2[i])
for pt1, pt2 in zip(f1.kps[idx1], f2.kps[idx2]):
u1, v1 = denormalize(K, pt1)
u2, v2 = denormalize(K, pt2)
cv2.circle(img, (u1, v1), color=(0, 255, 0), radius=3)
cv2.line(img, (u1, v1), (u2, v2), color=(255, 0, 0))
# cv2.imshow("result", img)
# cv2.waitKey(1)
if frame.id >= 4:
mapp.optimize()
# 3-D
mapp.display()
def process_frame(img):
img = cv2.resize(img, (W, H))
# frame object, include frame.pts and frame.des
frame = Frame(mapp, img, K)
if len(mapp.frames) <= 1:
return
f1 = mapp.frames[-1]
f2 = mapp.frames[-2]
idx1, idx2, Rt = match(f1, f2)
f1.pose = np.dot(Rt, f2.pose)
# print(Rt)
# print(spts.shape)
# print(pts.shape)
# homogenous 3-D coords
pts4d = triangulate(f1.pose, f2.pose, f1.pts[idx1], f2.pts[idx2])
pts4d /= pts4d[:, 3:]
# rject pts without enough "parallax"
# reject points behind the camera
good_pts4d = (np.abs(pts4d[:, 3]) > 0.005) & (pts4d[:, 2] > 0)
for i, p in enumerate(pts4d):
if not good_pts4d[i]:
continue
pt = Point(mapp, p)
pt.add_observation(f1, idx1[i])
pt.add_observation(f2, idx2[i])
# visualize pts on screen
for pt1, pt2 in zip(f1.pts[idx1], f2.pts[idx2]):
u1, v1 = denormalize(frame.K, pt1)
u2, v2 = denormalize(frame.K, pt2)
cv2.circle(img, (u1, v1), color=(0, 255, 0), radius=3)
cv2.line(img, (u1, v1), (u2, v2), color=(0, 0, 255))
cv2.imshow('test', img)
cv2.waitKey(1)
mapp.display()
def process_frame(img):
img = cv2.resize(img, (W, H))
frame = Frame(mapp, img, K)
if frame.id == 0:
return
f1 = mapp.frames[-1]
f2 = mapp.frames[-2]
idx1, idx2, Rt = match_frames(f1, f2)
f1.pose = np.dot(Rt, f2.pose)
# homogeneous 3-D coords
pts4d = triangulate(f1.pose, f2.pose, f1.pts[idx1], f2.pts[idx2])
pts4d /= pts4d[:, 3:]
# reject pts without enough "parallax" (this right?)
# reject points behind the camera
good_pts4d = (np.abs(pts4d[:, 3]) > 0.005) & (pts4d[:, 2] > 0)
for i, p in enumerate(pts4d):
if not good_pts4d[i]:
continue
pt = Point(mapp, p)
pt.add_observation(f1, idx1[i])
pt.add_observation(f2, idx2[i])
for pt1, pt2 in zip(f1.pts[idx1], f2.pts[idx2]):
u1, v1 = denormalize(K, pt1)
u2, v2 = denormalize(K, pt2)
cv2.circle(img, (u1, v1), color=(0, 255, 0), radius=3)
cv2.line(img, (u1, v1), (u2, v2), color=(255, 0, 0))
# 2-D display
if disp is not None:
disp.paint(img)
# 3-D display
mapp.display()
def process_frame(img):
start_time = time.time()
img = cv2.resize(img, (W,H))
frame = Frame(mapp, img, K)
if frame.id == 0:
return
f1 = mapp.frames[-1]
f2 = mapp.frames[-2]
idx1, idx2, Rt = match_frames(f1, f2)
if frame.id < 5:
# get initial positions from fundamental matrix
f1.pose = np.dot(Rt, f2.pose)
else:
# kinematic model
velocity = np.dot(f2.pose, np.linalg.inv(mapp.frames[-3].pose))
f1.pose = np.dot(velocity, f2.pose)
for i,idx in enumerate(idx2):
if f2.pts[idx] is not None:
f2.pts[idx].add_observation(f1, idx1[i])
# pose optimization
#print(f1.pose)
pose_opt = mapp.optimize(local_window=1, fix_points=True)
print("Pose: %f" % pose_opt)
#print(f1.pose)
# search by projection
sbp_pts_count = 0
if len(mapp.points) > 0:
map_points = np.array([p.homogeneous() for p in mapp.points])
projs = np.dot(np.dot(K, f1.pose[:3]), map_points.T).T
projs = projs[:, 0:2] / projs[:, 2:]
good_pts = (projs[:, 0] > 0) & (projs[:, 0] < W) & \
(projs[:, 1] > 0) & (projs[:, 1] < H)
for i, p in enumerate(mapp.points):
if not good_pts[i]:
continue
q = f1.kd.query_ball_point(projs[i], 5)
for m_idx in q:
if f1.pts[m_idx] is None:
# if any descriptors within 32
for o in p.orb():
o_dist = hamming_distance(o, f1.des[m_idx])
if o_dist < 32.0:
p.add_observation(f1, m_idx)
sbp_pts_count += 1
break
good_pts4d = np.array([f1.pts[i] is None for i in idx1])
# reject pts without enough "parallax" (this right?)
pts4d = triangulate(f1.pose, f2.pose, f1.kps[idx1], f2.kps[idx2])
good_pts4d &= np.abs(pts4d[:, 3]) > 0.005
# homogeneous 3-D coords
pts4d /= pts4d[:, 3:]
# locally in front of camera
# NOTE: This check is broken and maybe unneeded
#pts_tri_local = np.dot(f1.pose, pts4d.T).T
#good_pts4d &= pts_tri_local[:, 2] > 0
print("Adding: %d new points, %d search by projection" % (np.sum(good_pts4d), sbp_pts_count))
for i,p in enumerate(pts4d):
if not good_pts4d[i]:
continue
u,v = int(round(f1.kpus[idx1[i],0])), int(round(f1.kpus[idx1[i],1]))
pt = Point(mapp, p[0:3], img[v,u])
pt.add_observation(f1, idx1[i])
pt.add_observation(f2, idx2[i])
for i1, i2 in zip(idx1, idx2):
pt1 = f1.kps[i1]
pt2 = f2.kps[i2]
u1, v1 = denormalize(K, pt1)
u2, v2 = denormalize(K, pt2)
if f1.pts[i1] is not None:
if len(f1.pts[i1].frames) >= 5:
cv2.circle(img, (u1, v1), color=(0,255,0), radius=3)
else:
cv2.circle(img, (u1, v1), color=(0,128,0), radius=3)
else:
cv2.circle(img, (u1, v1), color=(0,0,0), radius=3)
cv2.line(img, (u1, v1), (u2, v2), color=(255,0,0))
# 2-D display
if disp is not None:
disp.paint(img)
# optimize the map
if frame.id >= 4 and frame.id%5 == 0:
err = mapp.optimize()
print("Optimize: %f units of error" % err)
# 3-D display
mapp.display()
print("Map: %d points, %d frames" % (len(mapp.points), len(mapp.frames)))
print("Time: %.2f ms" % ((time.time()-start_time)*1000.0))
def process_frame(img):
start_time = time.time()
img = cv2.resize(img, (W, H))
frame = Frame(map3d, img, K)
if frame.id == 0:
return
frame1 = map3d.frames[-1]
frame2 = map3d.frames[-2]
idx1, idx2, rt = match(frame1, frame2)
if frame.id < 5:
frame1.pose = np.dot(rt, frame2.pose)
else:
velocity = np.dot(frame2.pose, np.linalg.inv(map3d.frames[-3].pose))
frame1.pose = np.dot(velocity, frame2.pose)
for i, idx in enumerate(idx2):
if frame2.pts[idx] is not None:
frame2.pts[idx].add_observation(frame1, idx1[i])
# pose optimization
pose_optimizer = map3d.optimize(local_window=1, fix_points=True)
# print("Pose: %f" % pose_optimizer)
# projection search
projection_pts_count = 0
if len(map3d.points) > 0:
map_points = np.array([p.homogenous() for p in map3d.points])
projs = np.dot(np.dot(K, frame1.pose[:3]), map_points.T).T
projs = projs[:, 0:2] / projs[:, 2:]
good_pts = (projs[:, 0] > 0) & (projs[:, 0] < W) & (
projs[:, 1] > 0) & (projs[:, 1] < H)
for i, p in enumerate(map3d.points):
if not good_pts[i]:
continue
queue = frame1.kd.query_ball_point(projs[i], 5)
for q in queue:
if frame1.pts[q] is None:
for o in p.orb():
o_dist = hamming_distance(o, frame1.des[q])
if o_dist < 32.0:
p.add_observation(frame1, q)
projection_pts_count += 1
break
good_pts3d = np.array([frame1.pts[i] is None for i in idx1])
pts3d = triangulate(frame1.pose, frame2.pose, frame1.kps[idx1],
frame2.kps[idx2])
good_pts3d &= np.abs(pts3d[:, 3]) > 0.005
# homogeneous 3-D coordinates
pts3d /= pts3d[:, 3:]
# should reject points hehind the camera
pts_tri_local = np.dot(frame1.pose, pts3d.T).T
good_pts3d &= pts_tri_local[:, 2] > 0
print("Adding: %d points" % np.sum(good_pts3d))
# loop to create 3D points using points obtained from the image frames
for i, p in enumerate(pts3d):
if not good_pts3d[i]:
continue
u, v = int(round(frame1._kps[idx1[i],
0])), int(round(frame1._kps[idx1[i], 1]))
pt = Point(map3d, p[0:3], img[v, u])
pt.add_observation(frame1, idx1[i])
pt.add_observation(frame2, idx2[i])
# for pt1, pt2 in ret_val:
for i1, i2 in zip(idx1, idx2):
pt1 = frame1.kps[i1]
pt2 = frame2.kps[i2]
u1, u2 = denormalize(K, pt1)
v1, v2 = denormalize(K, pt2)
# create circles, improve coloring
if frame1.pts[i1] is not None:
if len(frame1.pts[i1].frames) >= 5:
cv2.circle(img, (u1, u2), color=(0, 255, 0), radius=3)
else:
cv2.circle(img, (u1, u2), color=(0, 128, 0), radius=3)
else:
cv2.circle(img, (u1, u2), color=(0, 0, 0), radius=3)
cv2.line(img, (u1, u2), (v1, v2), color=(255, 0, 255))
if disp is not None:
disp.paint(img) # 2D display
# 3D map optimization
if frame.id >= 4 and frame.id % 5 == 0:
error = map3d.optimize()
print("Optimize: %f units of error" % error)
map3d.display() # 3D display
end_time = time.time()
print("Time: %.2f ms" % ((end_time - start_time) * 1000.0))
print("Map: %d points, %d frames" % (len(map3d.points), len(map3d.frames)))