예제 #1
0
파일: sptam.py 프로젝트: SEESAI/stereo_ptam
    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)
예제 #2
0
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']
예제 #3
0
파일: sptam.py 프로젝트: SEESAI/stereo_ptam
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']
예제 #4
0
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']