def __init__(self, params, cam):
self.params = params
self.cam = cam
self.motion_model = MotionModel()
self.map = Map()
self.preceding = None # last keyframe
self.current = None # current frame
self.status = defaultdict(bool)
self.optimizer = BundleAdjustment()
self.bundle_adjustment = LocalBA()
self.min_measurements = params.pnp_min_measurements
self.max_iterations = params.pnp_max_iterations
self.timer = RunningAverageTimer()
self.lines = True
def optimize_map(self):
"""
Python doesn't really work with the multithreading model, so just putting optimization on the main thread
"""
adjust_keyframes = self.map.search_adjust_keyframes()
# Set data time increases with iterations!
# self.timer = RunningAverageTimer()
self.bundle_adjustment = LocalBA()
self.bundle_adjustment.optimizer.set_verbose(True)
# with self.timer:
self.bundle_adjustment.set_data(adjust_keyframes, [])
self.bundle_adjustment.optimize(2)
self.bundle_adjustment.update_poses()
self.bundle_adjustment.update_points()
def mapping_process(camera, conn: Pipe):
optimizer = LocalBA()
keyframes = []
mappoints = []
measurements = []
stopped = False
while not stopped:
# we need two things, keyframe poses and measurements
# the measurements have mappoints, keyframe id and projected position
new_keyframes = []
new_mappoints = []
new_measurements = []
while not conn.empty()
new_data = conn.recv()
if type(new_data) is PipeKeyFrame:
new_keyframes.append(new_data)
elif type(new_data) is PipeMapPoint:
new_mappoints.append(new_data)
elif type(new_data) is PipeMeasurement:
new_measurements.append(new_data)
for kf in new_keyframes:
optimizer.add_keyframe(kf.id, kf.pose, camera, fixed=True)
for mp in new_mappoints:
optimizer.add_mappoint(mp.id, mp.position)
def __init__(self, graph, params):
self.graph = graph
self.params = params
self.local_keyframes = []
self.optimizer = LocalBA()
class Mapping(object):
def __init__(self, graph, params):
self.graph = graph
self.params = params
self.local_keyframes = []
self.optimizer = LocalBA()
def add_keyframe(self, keyframe, measurements):
"""Add a new keyframe from the tracking thread into the map
- Step 1. Add the keyframe to the covisiblity graph
- Step 2. Create the new map points from triangulation with the unmatched
keypoints from the left and right images (no matching with the local map points)
Args:
keyframe ([type]): [description]
measurements ([type]): [description]
"""
self.graph.add_keyframe(keyframe)
# Triangulate the unmatched keypoints within the stereo pair after the local mappoint tracking`
self.create_points(keyframe)
# Add the measurments into the co-graph
for m in measurements:
self.graph.add_measurement(keyframe, m.mappoint, m)
# why clear the local_keyframes list?
self.local_keyframes.clear()
self.local_keyframes.append(keyframe)
# Find the covisible keyframes
self.fill(self.local_keyframes, keyframe)
self.refind(self.local_keyframes, self.get_owned_points(keyframe))
self.bundle_adjust(self.local_keyframes)
self.points_culling(self.local_keyframes)
def create_points(self, keyframe):
"""Create new mappoints throught stereo triangulation with the yet unmatched keypoints
Args:
keyframe (KeyFrame): input newly added keyframe
"""
mappoints, measurements = keyframe.triangulate()
self.add_measurements(keyframe, mappoints, measurements)
def fill(self, keyframes, keyframe):
"""Find the top local_window_size covisible keyframes with the input keyframe and store
the results into the list of keyframes.
Args:
keyframes (list of Keyframe): output list of covisible keyframes
keyframe (Keyframe): input keyframe
"""
# Sort all the covisible frames by their weight (shared measurements)
covisible = sorted(keyframe.covisibility_keyframes().items(),
key=lambda _: _[1],
reverse=True)
# Keep local_window_size covisible frames
for kf, n in covisible:
if n > 0 and kf not in keyframes and self.is_safe(kf):
keyframes.append(kf)
if len(keyframes) >= self.params.local_window_size:
return
def add_measurements(self, keyframe, mappoints, measurements):
"""Add the keyframe, mappoints, measurements into the map, also
increase the measurement count for the map points
Args:
keyframe (KeyFrame): The new keyframe
mappoints (list of MapPoint): the new map points
measurements (list of Measurement): the new measurements
"""
for mappoint, measurement in zip(mappoints, measurements):
self.graph.add_mappoint(mappoint)
self.graph.add_measurement(keyframe, mappoint, measurement)
mappoint.increase_measurement_count()
def get_owned_points(self, keyframe):
"""Get the mappoints anchored to current keyframe, usually refers to those mappoints
from stereo triangulation.
Args:
keyframe (KeyFrame): current keyframe
Returns:
[type]: [description]
"""
owned = []
for m in keyframe.measurements():
if m.from_triangulation():
owned.append(m.mappoint)
return owned
def filter_unmatched_points(self, keyframe, mappoints):
''' For the map points where within frustrum and there is no
measurement, we will stort them into filtered
'''
filtered = []
for i in np.where(keyframe.can_view(mappoints))[0]:
pt = mappoints[i]
if (not pt.is_bad()
and not self.graph.has_measurement(keyframe, pt)):
filtered.append(pt)
return filtered
def refind(self, keyframes, new_mappoints): # time consuming
''' For the new map points which does not have measurement in current
keyframe, will try to find again on those keyframes, if found
those measurements will be marked as Source.REFIND measurements
'''
if len(new_mappoints) == 0:
return
for keyframe in keyframes:
# Find the 3D points which has no measurement in current KF
filtered = self.filter_unmatched_points(keyframe, new_mappoints)
if len(filtered) == 0:
continue
for mappoint in filtered:
mappoint.increase_projection_count()
# Find matchings in current KF
measuremets = keyframe.match_mappoints(filtered,
Measurement.Source.REFIND)
for m in measuremets:
self.graph.add_measurement(keyframe, m.mappoint, m)
m.mappoint.increase_measurement_count()
def bundle_adjust(self, keyframes):
"""Run BA to refine the map: Keyframes pose + mappoints
Args:
keyframes ([type]): [description]
Returns:
[type]: [description]
"""
# adjust_keyframes: all the keyframes which are not fixed
adjust_keyframes = set()
for kf in keyframes:
if not kf.is_fixed():
adjust_keyframes.add(kf)
# The covisiable frames of all the KF of adjust_keyframes but not inside adjust_keyframes
# This is to avoid the local BA will change the global map too much
fixed_keyframes = set()
for kf in adjust_keyframes:
for ck, n in kf.covisibility_keyframes().items():
if (n > 0 and ck not in adjust_keyframes and self.is_safe(ck)
and ck < kf):
fixed_keyframes.add(ck)
self.optimizer.set_data(adjust_keyframes, fixed_keyframes)
completed = self.optimizer.optimize(self.params.ba_max_iterations)
self.optimizer.update_poses()
self.optimizer.update_points()
if completed:
# Remove the measurement whose reprojection errors are too larget
self.remove_measurements(self.optimizer.get_bad_measurements())
return completed
def is_safe(self, keyframe):
return True
def remove_measurements(self, measurements):
for m in measurements:
m.mappoint.increase_outlier_count()
self.graph.remove_measurement(m)
def points_culling(self, keyframes): # Remove bad mappoints
mappoints = set(chain(*[kf.mappoints() for kf in keyframes]))
for pt in mappoints:
if pt.is_bad():
self.graph.remove_mappoint(pt)
class Mapping(object):
def __init__(self, graph, params):
self.graph = graph
self.params = params
self.local_keyframes = []
self.optimizer = LocalBA()
def add_keyframe(self, keyframe, measurements):
self.graph.add_keyframe(keyframe)
self.create_points(keyframe)
for m in measurements:
self.graph.add_measurement(keyframe, m.mappoint, m)
self.local_keyframes.clear()
self.local_keyframes.append(keyframe)
self.fill(self.local_keyframes, keyframe)
self.refind(self.local_keyframes, self.get_owned_points(keyframe))
self.bundle_adjust(self.local_keyframes)
self.points_culling(self.local_keyframes)
def fill(self, keyframes, keyframe):
covisible = sorted(keyframe.covisibility_keyframes().items(),
key=lambda _: _[1],
reverse=True)
for kf, n in covisible:
if n > 0 and kf not in keyframes and self.is_safe(kf):
keyframes.append(kf)
if len(keyframes) >= self.params.local_window_size:
return
def create_points(self, keyframe):
mappoints, measurements = keyframe.triangulate()
self.add_measurements(keyframe, mappoints, measurements)
def add_measurements(self, keyframe, mappoints, measurements):
for mappoint, measurement in zip(mappoints, measurements):
self.graph.add_mappoint(mappoint)
self.graph.add_measurement(keyframe, mappoint, measurement)
mappoint.increase_measurement_count()
def bundle_adjust(self, keyframes):
adjust_keyframes = set()
for kf in keyframes:
if not kf.is_fixed():
adjust_keyframes.add(kf)
fixed_keyframes = set()
for kf in adjust_keyframes:
for ck, n in kf.covisibility_keyframes().items():
if (n > 0 and ck not in adjust_keyframes and self.is_safe(ck)
and ck < kf):
fixed_keyframes.add(ck)
self.optimizer.set_data(adjust_keyframes, fixed_keyframes)
completed = self.optimizer.optimize(self.params.ba_max_iterations)
self.optimizer.update_poses()
self.optimizer.update_points()
if completed:
self.remove_measurements(self.optimizer.get_bad_measurements())
return completed
def is_safe(self, keyframe):
return True
def get_owned_points(self, keyframe):
owned = []
for m in keyframe.measurements():
if m.from_triangulation():
owned.append(m.mappoint)
return owned
def filter_unmatched_points(self, keyframe, mappoints):
filtered = []
for i in np.where(keyframe.can_view(mappoints))[0]:
pt = mappoints[i]
if (not pt.is_bad()
and not self.graph.has_measurement(keyframe, pt)):
filtered.append(pt)
return filtered
def refind(self, keyframes, new_mappoints): # time consuming
if len(new_mappoints) == 0:
return
for keyframe in keyframes:
filtered = self.filter_unmatched_points(keyframe, new_mappoints)
if len(filtered) == 0:
continue
for mappoint in filtered:
mappoint.increase_projection_count()
measuremets = keyframe.match_mappoints(filtered,
Measurement.Source.REFIND)
for m in measuremets:
self.graph.add_measurement(keyframe, m.mappoint, m)
m.mappoint.increase_measurement_count()
def remove_measurements(self, measurements):
for m in measurements:
m.mappoint.increase_outlier_count()
self.graph.remove_measurement(m)
def points_culling(self, keyframes): # Remove bad mappoints
mappoints = set(chain(*[kf.mappoints() for kf in keyframes]))
for pt in mappoints:
if pt.is_bad():
self.graph.remove_mappoint(pt)
class Tracker(object):
def __init__(self, params, cam):
self.params = params
self.cam = cam
self.motion_model = MotionModel()
self.map = Map()
self.preceding = None # last keyframe
self.current = None # current frame
self.status = defaultdict(bool)
self.optimizer = BundleAdjustment()
self.bundle_adjustment = LocalBA()
self.min_measurements = params.pnp_min_measurements
self.max_iterations = params.pnp_max_iterations
self.timer = RunningAverageTimer()
self.lines = True
def initialize(self, frame):
keyframe = frame.to_keyframe()
mappoints, measurements = keyframe.create_mappoints_from_triangulation(
)
assert len(mappoints) >= self.params.init_min_points, (
'Not enough points to initialize map.')
keyframe.set_fixed(True)
self.extend_graph(keyframe, mappoints, measurements)
if self.lines:
maplines, line_measurements = keyframe.create_maplines_from_triangulation(
)
print(f'Initialized {len(maplines)} lines')
for mapline, measurement in zip(maplines, line_measurements):
self.map.add_mapline(mapline)
self.map.add_line_measurement(keyframe, mapline, measurement)
keyframe.add_measurement(measurement)
mapline.add_measurement(measurement)
self.preceding = keyframe
self.current = keyframe
self.status['initialized'] = True
self.motion_model.update_pose(frame.timestamp, frame.position,
frame.orientation)
# def clear_optimizer(self):
# # Calling optimizer.clear() doesn't fully clear for some reason
# # This prevents running time from scaling linearly with the number of frames
# self.optimizer = BundleAdjustment()
# self.bundle_adjustment = LocalBA()
def refine_pose(self, pose, cam, measurements):
assert len(measurements) >= self.min_measurements, (
'Not enough points')
self.optimizer = BundleAdjustment()
self.optimizer.add_pose(0, pose, cam, fixed=False)
for i, m in enumerate(measurements):
self.optimizer.add_point(i, m.mappoint.position, fixed=True)
self.optimizer.add_edge(0, i, 0, m)
self.optimizer.optimize(self.max_iterations)
return self.optimizer.get_pose(0)
def update(self, i, left_img, right_img, timestamp):
# Feature extraction takes 0.12s
origin = g2o.Isometry3d()
left_frame = Frame(i, origin, self.cam, self.params, left_img,
timestamp)
right_frame = Frame(i, self.cam.compute_right_camera_pose(origin),
self.cam, self.params, right_img, timestamp)
frame = StereoFrame(left_frame, right_frame)
if i == 0:
self.initialize(frame)
return
# All code in this functions below takes 0.05s
self.current = frame
predicted_pose, _ = self.motion_model.predict_pose(frame.timestamp)
frame.update_pose(predicted_pose)
# Get mappoints and measurements take 0.013s
local_mappoints = self.get_local_map_points(frame)
print(local_mappoints)
if len(local_mappoints) == 0:
print('Nothing in local_mappoints! Exiting.')
exit()
measurements = frame.match_mappoints(local_mappoints)
# local_maplines = self.get_local_map_lines(frame)
# line_measurements = frame.match_maplines(local_maplines)
# Refined pose takes 0.02s
try:
pose = self.refine_pose(frame.pose, self.cam, measurements)
frame.update_pose(pose)
self.motion_model.update_pose(frame.timestamp, pose.position(),
pose.orientation())
tracking_is_ok = True
except:
tracking_is_ok = False
print('tracking failed!!!')
if tracking_is_ok and self.should_be_keyframe(frame, measurements):
# Keyframe creation takes 0.03s
self.create_new_keyframe(frame)
# self.optimize_map()
def optimize_map(self):
"""
Python doesn't really work with the multithreading model, so just putting optimization on the main thread
"""
adjust_keyframes = self.map.search_adjust_keyframes()
# Set data time increases with iterations!
# self.timer = RunningAverageTimer()
self.bundle_adjustment = LocalBA()
self.bundle_adjustment.optimizer.set_verbose(True)
# with self.timer:
self.bundle_adjustment.set_data(adjust_keyframes, [])
self.bundle_adjustment.optimize(2)
self.bundle_adjustment.update_poses()
self.bundle_adjustment.update_points()
def extend_graph(self, keyframe, mappoints, measurements):
self.map.add_keyframe(keyframe)
for mappoint, measurement in zip(mappoints, measurements):
self.map.add_mappoint(mappoint)
self.map.add_point_measurement(keyframe, mappoint, measurement)
keyframe.add_measurement(measurement)
mappoint.add_measurement(measurement)
def create_new_keyframe(self, frame):
keyframe = frame.to_keyframe()
keyframe.update_preceding(self.preceding)
mappoints, measurements = keyframe.create_mappoints_from_triangulation(
)
self.extend_graph(keyframe, mappoints, measurements)
if self.lines:
maplines, line_measurements = keyframe.create_maplines_from_triangulation(
)
frame.visualise_measurements(line_measurements)
print(f'New Keyframe with {len(maplines)} lines')
for mapline, measurement in zip(maplines, line_measurements):
self.map.add_mapline(mapline)
self.map.add_line_measurement(keyframe, mapline, measurement)
keyframe.add_measurement(measurement)
mapline.add_measurement(measurement)
self.preceding = keyframe
def get_local_map_points(self, frame):
checked = set()
filtered = []
# Add in map points from preceding and reference
for pt in self.preceding.mappoints(): # neglect can_view test
# if pt in checked or pt.is_bad():
# print('bad')
# continue
pt.increase_projection_count()
filtered.append(pt)
return filtered
def get_local_map_lines(self, frame):
checked = set()
filtered = []
# Add in map points from preceding and reference
for ln in self.preceding.maplines(): # neglect can_view test
if ln in checked or ln.is_bad():
continue
ln.increase_projection_count()
filtered.append(ln)
return filtered
def should_be_keyframe(self, frame, measurements):
n_matches = len(measurements)
n_matches_ref = len(self.preceding.measurements())
return ((n_matches / n_matches_ref) <
self.params.min_tracked_points_ratio) or n_matches < 20
class Mapping(object):
def __init__(self, graph, params):
self.graph = graph
self.params = params
self.local_keyframes = []
self.optimizer = LocalBA()
# Get local keyframe created by THREAD - TRACKING
def add_keyframe(self, keyframe, measurements):
self.graph.add_keyframe(keyframe)
self.create_points(keyframe)
for m in measurements:
self.graph.add_measurement(keyframe, m.mappoint, m)
self.local_keyframes.clear()
self.local_keyframes.append(keyframe)
self.fill(self.local_keyframes, keyframe)
self.refind(self.local_keyframes, self.get_owned_points(keyframe))
self.bundle_adjust(self.local_keyframes)
self.points_culling(self.local_keyframes)
def fill(self, keyframes, keyframe):
covisible = sorted(keyframe.covisibility_keyframes().items(),
key=lambda _: _[1],
reverse=True)
for kf, n in covisible:
if n > 0 and kf not in keyframes and self.is_safe(kf):
keyframes.append(kf)
if len(keyframes) >= self.params.local_window_size:
return
# THREAD - TRACKING
# STEP - KEYFRAME SELECTION
def create_points(self, keyframe):
mappoints, measurements = keyframe.triangulate()
self.add_measurements(keyframe, mappoints, measurements)
def add_measurements(self, keyframe, mappoints, measurements):
for mappoint, measurement in zip(mappoints, measurements):
self.graph.add_mappoint(mappoint)
self.graph.add_measurement(keyframe, mappoint, measurement)
mappoint.increase_measurement_count()
# STEP - BUNDLE ADJUSTMENT
def bundle_adjust(self, keyframes):
adjust_keyframes = set()
for kf in keyframes:
if not kf.is_fixed():
adjust_keyframes.add(kf)
fixed_keyframes = set()
for kf in adjust_keyframes:
for ck, n in kf.covisibility_keyframes().items():
if (n > 0 and ck not in adjust_keyframes and self.is_safe(ck)
and ck < kf):
fixed_keyframes.add(ck)
# TREAD - LOOP CLOSURE: Correct the map points
self.optimizer.set_data(adjust_keyframes, fixed_keyframes)
completed = self.optimizer.optimize(self.params.ba_max_iterations)
self.optimizer.update_poses()
self.optimizer.update_points()
if completed:
self.remove_measurements(self.optimizer.get_bad_measurements())
return completed
# Set status saved
def is_safe(self, keyframe):
return True
# Save the points in the keyframe
def get_owned_points(self, keyframe):
owned = []
for m in keyframe.measurements():
if m.from_triangulation():
owned.append(m.mappoint)
return owned
# Estimate points without correspondence in the map
def filter_unmatched_points(self, keyframe, mappoints):
filtered = []
for i in np.where(keyframe.can_view(mappoints))[0]:
pt = mappoints[i]
if (not pt.is_bad()
and not self.graph.has_measurement(keyframe, pt)):
filtered.append(pt)
return filtered
# STEP - FIND NEW MEASUREMENTS
# Refinement of the camera pose (keyframe map) and the 3D pose (point cloud map)
def refind(self, keyframes, new_mappoints): # time consuming
if len(new_mappoints) == 0:
return
for keyframe in keyframes:
filtered = self.filter_unmatched_points(keyframe, new_mappoints)
if len(filtered) == 0:
continue
for mappoint in filtered:
mappoint.increase_projection_count()
measuremets = keyframe.match_mappoints(filtered,
Measurement.Source.REFIND)
for m in measuremets:
self.graph.add_measurement(keyframe, m.mappoint, m)
m.mappoint.increase_measurement_count()
# Minimize double summation of keyframe map and point cloud map
def remove_measurements(self, measurements):
for m in measurements:
m.mappoint.increase_outlier_count()
self.graph.remove_measurement(m)
# STEP - REMOVE BAD POINTS
def points_culling(self, keyframes): # Remove bad mappoints
mappoints = set(chain(*[kf.mappoints() for kf in keyframes]))
for pt in mappoints:
if pt.is_bad():
self.graph.remove_mappoint(pt)