Esempio n. 1
0
class Tracking(object):
    def __init__(self, params):

        # Set the variables:
        # Minimize the reprojection error through Bundle Adjustment
        # Set minimum number of measured points
        # Set maximum number of loop interations for map correction
        self.optimizer = BundleAdjustment()
        self.min_measurements = params.pnp_min_measurements
        self.max_iterations = params.pnp_max_iterations

# STEP - REFINE POSE

    def refine_pose(self, pose, cam, measurements):
        # Check if the minimum number of measured points is reached
        assert len(measurements) >= self.min_measurements, (
            'Not enough points')

        # Clear Bundle Adjustment (reprojection error) and add the currently measured position of the robot
        self.optimizer.clear()
        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)
Esempio n. 2
0
class Tracking(object):
    def __init__(self, params):
        self.optimizer = BundleAdjustment()
        self.min_measurements = params.pnp_min_measurements
        self.max_iterations = params.pnp_max_iterations

    def refine_pose(self, pose, cam, measurements):
        assert len(measurements) >= self.min_measurements, (
            'Not enough points')
            
        self.optimizer.clear()
        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)
Esempio n. 3
0
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