def __init__(self, W, H, K):
# main classes
self.mapp = Map()
# params
self.W, self.H = W, H
self.K = K
def __init__(self, W, H, K):
# main classes
self.mapp = Map()
# params
self.W, self.H = W, H
self.K = K
self.distances = []
self.tracker = Tracker(self.mapp)
def __init__(self, video_camera):
self.track_len = 10
self.detect_interval = 15
self.tracks = []
self.cam = video_camera
self.mapp = Map()
self._init_ = 5999
self._frame_id = 0
self.prev_gray = None
self._initial_frame = None
self.counter = 0
self.color_id = 0
self.c = 0
class SLAM(object):
def __init__(self, W, H, K):
# main classes
self.mapp = Map()
# params
self.W, self.H = W, H
self.K = K
def process_frame(self, img, pose=None):
start_time = time.time()
assert img.shape[0:2] == (self.H, self.W)
frame = Frame(self.mapp, img, self.K)
if frame.id == 0:
return
f1 = self.mapp.frames[-1]
f2 = self.mapp.frames[-2]
idx1, idx2, Rt = match_frames(f1, f2)
# add new observations if the point is already observed in the previous frame
# TODO: consider tradeoff doing this before/after search by projection
for i, idx in enumerate(idx2):
if f2.pts[idx] is not None and f1.pts[idx1[i]] is None:
f2.pts[idx].add_observation(f1, idx1[i])
if frame.id < 5 or True:
# get initial positions from fundamental matrix
f1.pose = np.dot(Rt, f2.pose)
else:
# kinematic model (not used)
velocity = np.dot(f2.pose,
np.linalg.inv(self.mapp.frames[-3].pose))
f1.pose = np.dot(velocity, f2.pose)
# pose optimization
if pose is None:
#print(f1.pose)
pose_opt = self.mapp.optimize(local_window=1, fix_points=True)
print("Pose: %f" % pose_opt)
#print(f1.pose)
else:
# have ground truth for pose
f1.pose = pose
sbp_pts_count = 0
# search by projection
if len(self.mapp.points) > 0:
# project *all* the map points into the current frame
map_points = np.array([p.homogeneous() for p in self.mapp.points])
projs = np.dot(np.dot(K, f1.pose[:3]), map_points.T).T
projs = projs[:, 0:2] / projs[:, 2:]
# only the points that fit in the frame
good_pts = (projs[:, 0] > 0) & (projs[:, 0] < self.W) & \
(projs[:, 1] > 0) & (projs[:, 1] < self.H)
for i, p in enumerate(self.mapp.points):
if not good_pts[i]:
# point not visible in frame
continue
if f1 in p.frames:
# we already matched this map point to this frame
# TODO: understand this better
continue
for m_idx in f1.kd.query_ball_point(projs[i], 2):
# if point unmatched
if f1.pts[m_idx] is None:
b_dist = p.orb_distance(f1.des[m_idx])
# if any descriptors within 64
if b_dist < 64.0:
p.add_observation(f1, m_idx)
sbp_pts_count += 1
break
# triangulate the points we don't have matches for
good_pts4d = np.array([f1.pts[i] is None for i in idx1])
# do triangulation in global frame
pts4d = triangulate(f1.pose, f2.pose, f1.kps[idx1], f2.kps[idx2])
good_pts4d &= np.abs(pts4d[:, 3]) != 0
pts4d /= pts4d[:, 3:] # homogeneous 3-D coords
# adding new points to the map from pairwise matches
new_pts_count = 0
for i, p in enumerate(pts4d):
if not good_pts4d[i]:
continue
# check parallax is large enough
# TODO: learn what parallax means
"""
r1 = np.dot(f1.pose[:3, :3], add_ones(f1.kps[idx1[i]]))
r2 = np.dot(f2.pose[:3, :3], add_ones(f2.kps[idx2[i]]))
parallax = r1.dot(r2) / (np.linalg.norm(r1) * np.linalg.norm(r2))
if parallax >= 0.9998:
continue
"""
# check points are in front of both cameras
pl1 = np.dot(f1.pose, p)
pl2 = np.dot(f2.pose, p)
if pl1[2] < 0 or pl2[2] < 0:
continue
# reproject
pp1 = np.dot(K, pl1[:3])
pp2 = np.dot(K, pl2[:3])
# check reprojection error
pp1 = (pp1[0:2] / pp1[2]) - f1.kpus[idx1[i]]
pp2 = (pp2[0:2] / pp2[2]) - f2.kpus[idx2[i]]
pp1 = np.sum(pp1**2)
pp2 = np.sum(pp2**2)
if pp1 > 2 or pp2 > 2:
continue
# add the point
color = img[int(round(f1.kpus[idx1[i], 1])),
int(round(f1.kpus[idx1[i], 0]))]
pt = Point(self.mapp, p[0:3], color)
pt.add_observation(f2, idx2[i])
pt.add_observation(f1, idx1[i])
new_pts_count += 1
print("Adding: %d new points, %d search by projection" %
(new_pts_count, sbp_pts_count))
# optimize the map
if frame.id >= 4 and frame.id % 5 == 0:
err = self.mapp.optimize() #verbose=True)
print("Optimize: %f units of error" % err)
print("Map: %d points, %d frames" %
(len(self.mapp.points), len(self.mapp.frames)))
print("Time: %.2f ms" % ((time.time() - start_time) * 1000.0))
print(np.linalg.inv(f1.pose))
import cv2
from display import Display
from extractor import Frame, denormalize, match_frames, IRt, add_ones
import time
import numpy as np
import g2o
from pointmap import Map, Point
# Camera intrinsics
W, H = 1920 // 2, 1080 // 2
F = 270
K = np.array(([F, 0, W // 2], [0, F, H // 2], [0, 0, 1]))
Kinv = np.linalg.inv(K)
display = Display(W, H)
mapp = Map()
mapp.create_viewer()
def triangulate(pose1, pose2, pts1, pts2):
ret = np.zeros((pts1.shape[0], 4))
pose1 = np.linalg.inv(pose1)
pose2 = np.linalg.inv(pose2)
for i, p in enumerate(zip(add_ones(pts1), add_ones(pts2))):
A = np.zeros((4, 4))
A[0] = p[0][0] * pose1[2] - pose1[0]
A[1] = p[0][1] * pose1[2] - pose1[1]
A[2] = p[1][0] * pose2[2] - pose2[0]
A[3] = p[1][1] * pose2[2] - pose2[1]
_, _, vt = np.linalg.svd(A)
ret[i] = vt[3]
def __init__(self, W, H, K):
# Create Map object
self.mapp = Map()
# Set class parameters: image width, image height
self.W, self.H, self.K = W, H, K
class Tracker():
def __init__(self, video_camera):
self.track_len = 10
self.detect_interval = 15
self.tracks = []
self.cam = video_camera
self.mapp = Map()
self._init_ = 5999
self._frame_id = 0
self.prev_gray = None
self._initial_frame = None
self.counter = 0
self.color_id = 0
self.c = 0
def __process_frame__(self):
while True:
# select which part of the video to start
if self._frame_id < self._init_:
self._frame_id = self._init_
self.cam.set(cv2.CAP_PROP_POS_FRAMES, self._frame_id)
# read the frames
_ret, frame = self.cam.read()
# remove distortions
#frame = undistort(frame,cameraMatrix, distCoeffs)
vis = frame.copy()
frame_gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
if len(self.tracks) > 0:
p0 = np.float32([tr[-1]
for tr in self.tracks]).reshape(-1, 1, 2)
frame_obj = Frame(self._initial_frame,
frame,
self.mapp,
p0,
mask=[])
if frame_obj.id == 0:
self._initial_frame = frame
continue
img0, img1 = self.prev_gray, frame_gray
#f1 = self.mapp.frames[-2]
#f2 = self.mapp.frames[-1]
#f1 = self.mapp.frames[-1]
#f2 = self.mapp.frames[-2]
#match_frame(f1,f2)
self.mapp.add_key_frame()
# p1 : points matching from previous image and the new one, using last points p0
p1, _st, _err = feature_extraction_KLT(img0, img1, p0,
lk_params)
# p0r : points matching from new image and the previous image, using new points p1
p0r, _st, _err = feature_extraction_KLT(
img1, img0, p1, lk_params)
# check the axis distance between those points (Not actulally a distance )
d = abs(p0 - p0r).reshape(-1, 2).max(-1)
# requirement to keep following a track
good = (d < 0.00899) & (d > 0)
new_tracks = []
for tr, (x, y), good_flag in copy(
zip(self.tracks, p1.reshape(-1, 2), good)):
if not good_flag:
continue
tr.append((x, y))
if len(tr) > self.track_len:
del tr[0]
new_tracks.append(tr)
cv.circle(vis, (x, y), 2, (0, 255, 0), -1)
self.tracks = new_tracks
cv.polylines(vis, [np.int32(tr) for tr in self.tracks], False,
color[self.color_id]) #(0, 255, 0)
draw_str(vis, (20, 20), 'track count: %d' % len(self.tracks))
if self._frame_id % self.detect_interval == 0:
# mask is the region of interest where the feature should be detected
mask = np.zeros_like(frame_gray)
mask[:] = 255
for x, y in [np.int32(tr[-1]) for tr in self.tracks]:
cv.circle(mask, (x, y), 5, 0, -1)
p, _, _ = featureExtraction(frame_gray, mask)
if p is not None:
for x, y in np.float32(p).reshape(-1, 2):
self.tracks.append([(x, y)])
self.color_id += 1
# key board stroke
ks = cv2.waitKey(1)
if ks & 0xFF == ord('q'):
break
if ks == 27:
break
cv2.imshow('Display', vis)
self._frame_id += 1
self.prev_gray = frame_gray
self._initial_frame = frame
import cv2
from display import Display
from frame import Frame, denormalize, match_frames, IRt
import numpy as np
import g2o
from pointmap import Map, Point
# camera intrinsics
W, H = 1920 // 2, 1080 // 2
F = 270
K = np.array([[F, 0, W // 2], [0, F, H // 2], [0, 0, 1]])
Kinv = np.linalg.inv(K)
# main classes
mapp = Map()
disp = Display(W, H) if os.getenv("D2D") is not None else None
def triangulate(pose1, pose2, pts1, pts2):
ret = np.zeros((pts1.shape[0], 4))
pose1 = np.linalg.inv(pose1)
pose2 = np.linalg.inv(pose2)
for i, p in enumerate(zip(pts1, pts2)):
A = np.zeros((4, 4))
A[0] = p[0][0] * pose1[2] - pose1[0]
A[1] = p[0][1] * pose1[2] - pose1[1]
A[2] = p[1][0] * pose2[2] - pose2[0]
A[3] = p[1][1] * pose2[2] - pose2[1]
_, _, vt = np.linalg.svd(A)
ret[i] = vt[3]
class SLAM(object):
def __init__(self, W, H, K):
# main classes
self.mapp = Map()
# params
self.W, self.H = W, H
self.K = K
self.distances = []
self.tracker = Tracker(self.mapp)
def triangulate(self, frame_1, frame_2, idx1, idx2):
# triangulate the points we don't have matches for
good_pts4d = np.array([frame_1.pts[i] is None for i in idx1])
# do triangulation in global frame
pts4d = triangulate(frame_1.pose, frame_2.pose, frame_1.kps[idx1],
frame_2.kps[idx2])
good_pts4d &= np.abs(pts4d[:, 3]) != 0
pts4d /= pts4d[:, 3:] # homogeneous 3-D coords
return pts4d, good_pts4d
def match_points(self, pts4d, good_pts4d, frame_1, frame_2, idx1, idx2,
img, new_pts_count):
for i, p in enumerate(pts4d):
if not good_pts4d[i]:
continue
# check parallax is large enough
# TODO: learn what parallax means
"""
r1 = np.dot(frame_1.pose[:3, :3], add_ones(frame_1.kps[idx1[i]]))
r2 = np.dot(frame_2.pose[:3, :3], add_ones(frame_2.kps[idx2[i]]))
parallax = r1.dot(r2) / (np.linalg.norm(r1) * np.linalg.norm(r2))
if parallax >= 0.9998:
continue
"""
# check points are in front of both cameras
pl1 = np.dot(frame_1.pose, p)
pl2 = np.dot(frame_2.pose, p)
if pl1[2] < 0 or pl2[2] < 0:
continue
# reproject
pp1 = np.dot(self.K, pl1[:3])
pp2 = np.dot(self.K, pl2[:3])
# check reprojection error
pp1 = (pp1[0:2] / pp1[2]) - frame_1.key_points[idx1[i]]
pp2 = (pp2[0:2] / pp2[2]) - frame_2.key_points[idx2[i]]
pp1 = np.sum(pp1**2)
pp2 = np.sum(pp2**2)
if pp1 > 2 or pp2 > 2:
continue
# add the point
try:
color = img[int(round(frame_1.key_points[idx1[i], 1])),
int(round(frame_1.key_points[idx1[i], 0]))]
except IndexError:
color = (255, 0, 0)
pt = Point(self.mapp, p[0:3], color)
connect_frame_point(frame_2, pt, idx2[i])
connect_frame_point(frame_1, pt, idx1[i])
new_pts_count += 1
def reoptimize(self, idx1, idx2, img):
sbp_pts_count = self.mapp.search_by_projection(self.tracker.frame_1,
self.K, self.W, self.H)
# adding new points to the map from pairwise matches
new_pts_count = 0
pts4d, good_pts4d = self.triangulate(self.tracker.frame_1,
self.tracker.frame_2, idx1, idx2)
self.match_points(pts4d, good_pts4d, self.tracker.frame_1,
self.tracker.frame_2, idx1, idx2, img, new_pts_count)
return new_pts_count, sbp_pts_count
def process_frame(self, img, pose=None, verts=None):
assert img.shape[0:2] == (self.H, self.W)
start_time = time.time()
self.tracker.image_to_track(img, verts, self.K)
self.tracker.frame_1.id = self.mapp.add_frame(self.tracker.frame_1)
if self.tracker.frame_1.id == 0:
return
try:
idx1, idx2, rotation_matrix = self.tracker.track()
frame_1 = self.tracker.make_initial_pose(rotation_matrix)
self.mapp.connect_observations(self.tracker.frame_1,
self.tracker.frame_2, idx1, idx2)
frame_1 = self.mapp.optimise_pose(pose, frame_1)
new_pts_count, sbp_pts_count = self.reoptimize(idx1, idx2, img)
# optimize the map
if frame_1.id >= 4 and frame_1.id % 5 == 0:
err = self.mapp.optimize() #verbose=True)
logger.info("Optimize: %f units of error" % err)
except NoFrameMatchError as e:
self.mapp.pop_frame()
raise e
# pose_matrix = np.linalg.inv(frame_1.pose)
# previous_pose_matrix = np.linalg.inv(frame_2.pose)
# distance = pose_matrix[:3,3] - previous_pose_matrix[:3,3]
# if frame_1.id > 5:
# self.distances.append(distance)
# plt.plot(self.distances)
# plt.show()
movement = cv2.absdiff(frame_1.img, self.tracker.frame_2.img)
logger.info("Adding: %d new points, %d search by projection" %
(new_pts_count, sbp_pts_count))
logger.info("Map: %d points, %d frames" %
(len(self.mapp.points), len(self.mapp.frames)))
logger.info("Time: %.2f ms" %
((time.time() - start_time) * 1000.0))
return dict(movement=movement)