def __init__(self, params):
self.params = params
self.tracker = Tracking(params)
self.motion_model = MotionModel()
self.graph = CovisibilityGraph()
self.mapping = MappingThread(self.graph, params)
self.loop_closing = LoopClosing(self, params)
self.loop_correction = None
self.reference = None # reference keyframe
self.preceding = None # last keyframe
self.current = None # current frame
self.status = defaultdict(bool)
class SPTAM(object):
"""
- The interaction between tracking and mapping is through keyframes and CovisibilityGraph
Args:
object ([type]): [description]
"""
def __init__(self, params):
self.params = params
self.tracker = Tracking(params)
self.motion_model = MotionModel()
self.graph = CovisibilityGraph()
self.mapping = MappingThread(self.graph, params)
self.loop_closing = LoopClosing(self, params)
self.loop_correction = None
self.reference = None # reference keyframe which contains the most local map points
self.preceding = None # last keyframe
self.current = None # current frame
self.status = defaultdict(bool)
def stop(self):
self.mapping.stop()
if self.loop_closing is not None:
self.loop_closing.stop()
def initialize(self, frame):
"""Use stereo triangulation to initialize the map
Args:
frame (StereoFrame): new incoming stereo frames(with feature extracted)
"""
mappoints, measurements = frame.triangulate()
assert len(mappoints) >= self.params.init_min_points, (
'Not enough points to initialize map.')
# The first frame is always KF
keyframe = frame.to_keyframe()
# The first keyframe should be fixed
keyframe.set_fixed(True)
self.graph.add_keyframe(keyframe)
# All the measurements and mappoints are anchored to this very keyframe
self.mapping.add_measurements(keyframe, mappoints, measurements)
if self.loop_closing is not None:
self.loop_closing.add_keyframe(keyframe)
self.reference = keyframe
self.preceding = keyframe
self.current = keyframe
self.status['initialized'] = True
self.motion_model.update_pose(frame.timestamp, frame.position,
frame.orientation)
def track(self, frame):
"""
- Step 1: predict the pose with constant velocity model to get predicted_pose
- Step 2: use the sptam.preceding and self.reference frames as seed frame to extract
the local map points (sptam.filter_points(frame)), which can be viewed within current
frame with the initial pose estimation
- Step 3: Find the 2D image matchings with 3D map points' feature descriptors. Also update
the feature descriptor for the matched 3D map points to inprove the long term tracking capability
- Step 4: Update the self.reference frame by querying the graph to find which frame
has containts the most of current local map points set
- Step 5: Do a motion only BA to refine the current frame pose
- Step 6: Promote current frame to be KF if
- a. number of matched 3D map points is less than 20
- b. ration between matched 3D map points in current frame vs reference frame is less than a threhsold
"""
while self.is_paused():
time.sleep(1e-4)
self.set_tracking(True)
self.current = frame
print('Tracking:', frame.idx, ' <- ', self.reference.id,
self.reference.idx)
# Step 1: predict the pose
predicted_pose, _ = self.motion_model.predict_pose(frame.timestamp)
frame.update_pose(predicted_pose)
if self.loop_closing is not None:
if self.loop_correction is not None:
estimated_pose = g2o.Isometry3d(frame.orientation,
frame.position)
estimated_pose = estimated_pose * self.loop_correction
frame.update_pose(estimated_pose)
self.motion_model.apply_correction(self.loop_correction)
self.loop_correction = None
# Step 2: find the local map points using self.reference and self.preceding
# frame as seed
local_mappoints = self.filter_points(frame)
# Step 3: find the matching of the 3D map points in current image with descriptor
measurements = frame.match_mappoints(local_mappoints,
Measurement.Source.TRACKING)
print('measurements:', len(measurements), ' ', len(local_mappoints))
tracked_map = set()
# Update the map point feature descripotr
for m in measurements:
mappoint = m.mappoint
mappoint.update_descriptor(m.get_descriptor())
mappoint.increase_measurement_count()
tracked_map.add(mappoint)
try:
# Find which KF contains the most seedpoints
self.reference = self.graph.get_reference_frame(tracked_map)
pose = self.tracker.refine_pose(frame.pose, frame.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):
print('new keyframe', frame.idx)
keyframe = frame.to_keyframe()
keyframe.update_reference(self.reference)
keyframe.update_preceding(self.preceding)
self.mapping.add_keyframe(keyframe, measurements)
if self.loop_closing is not None:
self.loop_closing.add_keyframe(keyframe)
self.preceding = keyframe
self.set_tracking(False)
def filter_points(self, frame):
"""Use the preceding and reference frame as seeds to extrat the local
3D map points.
- Step 1: Use preceding and reference as seed to get a set of local 3D map points
- Step 2: Remove the map points which cannot be viewed by the current frame with the initial estimate pose
- Step 3: Add the 3d points in the preceding and reference frames into the list
"""
# get local 3D map points
local_mappoints = self.graph.get_local_map_v2(
[self.preceding, self.reference])[0]
# Check whether those map points are within the frustrum of current view point
can_view = frame.can_view(local_mappoints)
print('filter points:', len(local_mappoints), can_view.sum(),
len(self.preceding.mappoints()), len(self.reference.mappoints()))
checked = set()
filtered = []
for i in np.where(can_view)[0]:
pt = local_mappoints[i]
if pt.is_bad():
continue
pt.increase_projection_count()
filtered.append(pt)
checked.add(pt)
# Add the 3D map points with in the preceding and reference frames
# into the local map points
for reference in set([self.preceding, self.reference]):
for pt in reference.mappoints(): # neglect can_view test
if pt in checked or pt.is_bad():
continue
pt.increase_projection_count()
filtered.append(pt)
return filtered
def should_be_keyframe(self, frame, measurements):
if self.adding_keyframes_stopped():
return False
n_matches = len(measurements)
n_matches_ref = len(self.reference.measurements())
print('keyframe check:', n_matches, ' ', n_matches_ref)
return ((n_matches / n_matches_ref) <
self.params.min_tracked_points_ratio) or n_matches < 20
def set_loop_correction(self, T):
self.loop_correction = T
def is_initialized(self):
return self.status['initialized']
def pause(self):
self.status['paused'] = True
def unpause(self):
self.status['paused'] = False
def is_paused(self):
return self.status['paused']
def is_tracking(self):
return self.status['tracking']
def set_tracking(self, status):
self.status['tracking'] = status
def stop_adding_keyframes(self):
self.status['adding_keyframes_stopped'] = True
def resume_adding_keyframes(self):
self.status['adding_keyframes_stopped'] = False
def adding_keyframes_stopped(self):
return self.status['adding_keyframes_stopped']
class SPTAM(object):
def __init__(self, params):
self.params = params
self.tracker = Tracking(params)
self.motion_model = MotionModel()
self.graph = CovisibilityGraph()
self.mapping = MappingThread(self.graph, params)
self.loop_closing = LoopClosing(self, params)
self.loop_correction = None
self.reference = None # reference keyframe
self.preceding = None # last keyframe
self.current = None # current frame
self.status = defaultdict(bool)
def stop(self):
self.mapping.stop()
if self.loop_closing is not None:
self.loop_closing.stop()
def initialize(self, frame):
mappoints, measurements = frame.triangulate()
assert len(mappoints) >= self.params.init_min_points, (
'Not enough points to initialize map.')
keyframe = frame.to_keyframe()
keyframe.set_fixed(True)
self.graph.add_keyframe(keyframe)
self.mapping.add_measurements(keyframe, mappoints, measurements)
if self.loop_closing is not None:
self.loop_closing.add_keyframe(keyframe)
self.reference = keyframe
self.preceding = keyframe
self.current = keyframe
self.status['initialized'] = True
self.motion_model.update_pose(
frame.timestamp, frame.position, frame.orientation)
def track(self, frame):
while self.is_paused():
time.sleep(1e-4)
self.set_tracking(True)
self.current = frame
print('Tracking:', frame.idx, ' <- ', self.reference.id, self.reference.idx)
predicted_pose, _ = self.motion_model.predict_pose(frame.timestamp)
frame.update_pose(predicted_pose)
if self.loop_closing is not None:
if self.loop_correction is not None:
estimated_pose = g2o.Isometry3d(
frame.orientation,
frame.position)
estimated_pose = estimated_pose * self.loop_correction
frame.update_pose(estimated_pose)
self.motion_model.apply_correction(self.loop_correction)
self.loop_correction = None
local_mappoints = self.filter_points(frame)
measurements = frame.match_mappoints(
local_mappoints, Measurement.Source.TRACKING)
print('measurements:', len(measurements), ' ', len(local_mappoints))
tracked_map = set()
for m in measurements:
mappoint = m.mappoint
mappoint.update_descriptor(m.get_descriptor())
mappoint.increase_measurement_count()
tracked_map.add(mappoint)
try:
self.reference = self.graph.get_reference_frame(tracked_map)
pose = self.tracker.refine_pose(frame.pose, frame.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):
print('new keyframe', frame.idx)
keyframe = frame.to_keyframe()
keyframe.update_reference(self.reference)
keyframe.update_preceding(self.preceding)
self.mapping.add_keyframe(keyframe, measurements)
if self.loop_closing is not None:
self.loop_closing.add_keyframe(keyframe)
self.preceding = keyframe
self.set_tracking(False)
def filter_points(self, frame):
local_mappoints = self.graph.get_local_map_v2(
[self.preceding, self.reference])[0]
can_view = frame.can_view(local_mappoints)
print('filter points:', len(local_mappoints), can_view.sum(),
len(self.preceding.mappoints()),
len(self.reference.mappoints()))
checked = set()
filtered = []
for i in np.where(can_view)[0]:
pt = local_mappoints[i]
if pt.is_bad():
continue
pt.increase_projection_count()
filtered.append(pt)
checked.add(pt)
for reference in set([self.preceding, self.reference]):
for pt in reference.mappoints(): # neglect can_view test
if pt in checked or pt.is_bad():
continue
pt.increase_projection_count()
filtered.append(pt)
return filtered
def should_be_keyframe(self, frame, measurements):
if self.adding_keyframes_stopped():
return False
n_matches = len(measurements)
n_matches_ref = len(self.reference.measurements())
print('keyframe check:', n_matches, ' ', n_matches_ref)
return ((n_matches / n_matches_ref) <
self.params.min_tracked_points_ratio) or n_matches < 20
def set_loop_correction(self, T):
self.loop_correction = T
def is_initialized(self):
return self.status['initialized']
def pause(self):
self.status['paused'] = True
def unpause(self):
self.status['paused'] = False
def is_paused(self):
return self.status['paused']
def is_tracking(self):
return self.status['tracking']
def set_tracking(self, status):
self.status['tracking'] = status
def stop_adding_keyframes(self):
self.status['adding_keyframes_stopped'] = True
def resume_adding_keyframes(self):
self.status['adding_keyframes_stopped'] = False
def adding_keyframes_stopped(self):
return self.status['adding_keyframes_stopped']
class SPTAM(object):
# Initialize the variables
# Set connection to the three threads: Tracking, Local Mapping, Loop Closure
# Tracking: Detect the robots position and create a keyframe of comparison between 3D points and 2D map
# Local Mapping: Refine 3D and 2D comparison, minimize the reprojection error and remove bad points
# Loop Closure: Detect revisited places, estimate the relative transformation and correct the keyframes and map features
def __init__(self, params):
self.params = params
# Set thread Tracking
self.tracker = Tracking(params)
self.motion_model = MotionModel()
# Set thread Local Mapping
self.graph = CovisibilityGraph()
self.mapping = MappingThread(self.graph, params)
# Set thread Loop Closure
self.loop_closing = LoopClosing(self, params)
self.loop_correction = None
# Set the keyframe
self.reference = None # reference keyframe
self.preceding = None # last keyframe
self.current = None # current frame
self.status = defaultdict(bool)
# Stop Loop Closure, if already executed
def stop(self):
self.mapping.stop()
if self.loop_closing is not None:
self.loop_closing.stop()
def initialize(self, frame):
# Create initial map
mappoints, measurements = frame.triangulate()
assert len(mappoints) >= self.params.init_min_points, (
'Not enough points to initialize map.')
# Create initial keyframe from initial map
keyframe = frame.to_keyframe()
keyframe.set_fixed(True)
self.graph.add_keyframe(keyframe)
self.mapping.add_measurements(keyframe, mappoints, measurements)
if self.loop_closing is not None:
self.loop_closing.add_keyframe(keyframe)
# Set reference, preceding and current keyframe to initial keyframe
self.reference = keyframe
self.preceding = keyframe
self.current = keyframe
self.status['initialized'] = True
# Set initial pose of the robot
self.motion_model.update_pose(frame.timestamp, frame.position,
frame.orientation)
# THREAD - TRACKING
def track(self, frame):
# While robot is not moving, wait
while self.is_paused():
time.sleep(1e-4)
# When robot is moving, start tracking
self.set_tracking(True)
# STEP - FEATURE EXTRACTION: Capture the actual frame of the 3D world
self.current = frame
print('Tracking:', frame.idx, ' <- ', self.reference.id,
self.reference.idx)
# STEP - POSE PREDICTION: Predict the current position of the robot
predicted_pose, _ = self.motion_model.predict_pose(frame.timestamp)
frame.update_pose(predicted_pose)
# While step Loop Closing, correct current pose
if self.loop_closing is not None:
if self.loop_correction is not None:
# Use g2o for pose graph optimization
estimated_pose = g2o.Isometry3d(frame.orientation,
frame.position)
# Create copy of the frame and execute correction
estimated_pose = estimated_pose * self.loop_correction
frame.update_pose(estimated_pose)
self.motion_model.apply_correction(self.loop_correction)
self.loop_correction = None
# STEP - MATCHING: Project map points and search for matches in the neighbourhood
local_mappoints = self.filter_points(frame)
measurements = frame.match_mappoints(local_mappoints,
Measurement.Source.TRACKING)
print('measurements:', len(measurements), ' ', len(local_mappoints))
# Use BRISK descriptor to describe the features of the points
# Compare the descriptors between map point and features
tracked_map = set()
for m in measurements:
mappoint = m.mappoint
mappoint.update_descriptor(m.get_descriptor())
mappoint.increase_measurement_count()
tracked_map.add(mappoint)
# STEP - POSE REFINEMENT: first get actual pose
try:
self.reference = self.graph.get_reference_frame(tracked_map)
# Compare the previous camera pose with the relative motion in the current, local frame
pose = self.tracker.refine_pose(frame.pose, frame.cam,
measurements)
# Update the pose
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!!!')
# STEP - KEYFRAME SELECTION
if tracking_is_ok and self.should_be_keyframe(frame, measurements):
print('new keyframe', frame.idx)
keyframe = frame.to_keyframe()
keyframe.update_reference(self.reference)
keyframe.update_preceding(self.preceding)
# Set new keyframe
self.mapping.add_keyframe(keyframe, measurements)
# THREAD - LOOP CLOSURE
# Add the keyframe to see if this place already has been visited
if self.loop_closing is not None:
self.loop_closing.add_keyframe(keyframe)
self.preceding = keyframe
self.set_tracking(False)
# Helping method for STEP - MATCHING
def filter_points(self, frame):
# Project the mappoints
local_mappoints = self.graph.get_local_map_v2(
[self.preceding, self.reference])[0]
# Set current view points
can_view = frame.can_view(local_mappoints)
print('filter points:', len(local_mappoints), can_view.sum(),
len(self.preceding.mappoints()), len(self.reference.mappoints()))
checked = set()
filtered = []
# Check if points are in current frame, if yes increase count
for i in np.where(can_view)[0]:
pt = local_mappoints[i]
if pt.is_bad():
continue
pt.increase_projection_count()
filtered.append(pt)
checked.add(pt)
# Get all references for this points
for reference in set([self.preceding, self.reference]):
for pt in reference.mappoints(): # neglect can_view test
if pt in checked or pt.is_bad():
continue
pt.increase_projection_count()
filtered.append(pt)
# Return filtered map points that are in the current view
return filtered
# Helping method for STEP - KEYFRAME SELECTION
def should_be_keyframe(self, frame, measurements):
if self.adding_keyframes_stopped():
return False
# Set actual matches and the ones of the current keyframe
n_matches = len(measurements)
n_matches_ref = len(self.reference.measurements())
print('keyframe check:', n_matches, ' ', n_matches_ref)
# Set only as new keyframe, if minimum number of tracked points ratio is fullfilled
# or if the current keyframe has less than 20 matches
return ((n_matches / n_matches_ref) <
self.params.min_tracked_points_ratio) or n_matches < 20
# THREAD - LOOP CLOSURE
# STEP - LOOP CORRECTION
def set_loop_correction(self, T):
self.loop_correction = T
# Other helping methods
def is_initialized(self):
return self.status['initialized']
def pause(self):
self.status['paused'] = True
def unpause(self):
self.status['paused'] = False
def is_paused(self):
return self.status['paused']
def is_tracking(self):
return self.status['tracking']
def set_tracking(self, status):
self.status['tracking'] = status
def stop_adding_keyframes(self):
self.status['adding_keyframes_stopped'] = True
def resume_adding_keyframes(self):
self.status['adding_keyframes_stopped'] = False
def adding_keyframes_stopped(self):
return self.status['adding_keyframes_stopped']