Пример #1
0
    def estimate_homography(self,cv_image):
        '''Estimate ground projection using instrinsic camera calibration parameters'''
        cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
        cv_image_rectified = self.rectify(cv_image)
        logger.info("image rectified")

        ret, corners = cv2.findChessboardCorners(cv_image_rectified, (self.board_['width'], self.board_['height']))
        if ret == False:
            logger.error("No corners found in image")
            exit(1)
        if len(corners) != self.board_['width'] * self.board_['height']:
            logger.error("Not all corners found in image")
            exit(2)
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.01)
        corners2 = cv2.cornerSubPix(cv_image_rectified, corners, (11,11), (-1,-1), criteria)

        #TODO flip checks
        src_pts = []
        for r in range(self.board_['height']):
        	for c in range(self.board_['width']):
        		src_pts.append(np.array([r * self.board_['square_size'] , c * self.board_['square_size']] , dtype='float32') + self.board_['offset'])
        # OpenCV labels corners left-to-right, top-to-bottom
        # so we reverse the groud points
        src_pts.reverse()

        # Compute homography from image to ground
        self.H, mask = cv2.findHomography(corners2.reshape(len(corners2), 2), np.array(src_pts), cv2.RANSAC)
        extrinsics_filename = get_duckiefleet_root() + "/calibrations/camera_extrinsic/" + self.robot_name + ".yaml"
        self.write_homography(extrinsics_filename)
        logger.info("Wrote ground projection to {}".format(extrinsics_filename))
Пример #2
0
def do_all_checks():
    username = getpass.getuser()
    hostname = socket.gethostname()
    filename = 'what_the_duck-%s-%s.html' % (hostname, username)
    WTD = colored('what_the_duck', 'cyan', attrs=['bold'])
    entries = get_checks()
    
#     logger.info("%s checks many things about the Duckiebot configuration" % WTD)
    logger.info('%s will run %s tests.\n' % (WTD, len(entries))) 
    results = run_checks(entries)
    o = ""
    o += display_results(results) + '\n\n'
    o += display_summary(results) + '\n\n'
    
    print(escaped_from_html(o))
    
    write_data_to_file(o, filename)
    print('\nNow send the file "%s" to the TA/instructors.' % filename)
    
    print('\nYou can also upload it using the following command: ')
    print('\n  scp %s [email protected]:%s ' % (filename, filename))
    stats = Statistics(results)
    if stats.nfailures == 0:    
        sys.exit(0)
    else:
        sys.exit(stats.nfailures)
Пример #3
0
    def write_calibration(self):
       '''Load kinematic calibration file'''
       home = expanduser("~")
       filename = (home + "/duckietown_sysid/kinematics/" + self.robot_name + ".yaml")
       if not os.path.isfile(filename):
           logger.warn("no kinematic calibration parameters for {}, taking some from default".format(self.robot_name))
           filename = (home+"/duckietown_sysid/kinematics/default.yaml")
           if not os.path.isfile(filename):
               logger.error("can't find default either, something's wrong, is the duckiefleet root correctly set?")
           else:
               data = yaml_load_file(filename)
       else:
           rospy.loginfo("Loading some kinematic calibration parameters of " + self.robot_name)
           data = yaml_load_file(filename)
       logger.info("Loaded homography for {}".format(os.path.basename(filename)))

       # Load some of the parameters that will not be changed
       param_k        = data['k']
       param_limit    = data['limit']
       param_radius   = data['radius']
       baseline   = data['baseline']
       # simply to increase readability

       gain = self.fit_['gain']
       trim = self.fit_['trim']

       # Write to yaml
       #datasave = {  # This is similar to the inverse_kinematics_node, but it did not work...
       #    "calibration_time": time.strftime("%Y-%m-%d-%H-%M-%S"),
       #    "gain": gain,
       #    "trim": trim,
       #    "baseline": baseline,
       #    "radius": param_radius,
       #    "k": param_k,
       #    "limit": param_limit,
       #}
       datasave = \
       "calibration_time: {}".format(time.strftime("%Y-%m-%d-%H-%M-%S")) + \
       "\ngain: {}".format(gain) + \
       "\ntrim: {}".format(trim) + \
       "\nbaseline: {}".format(baseline) + \
       "\nradius: {}".format(param_radius) + \
       "\nk: {}".format(param_k) + \
       "\nlimit: {}".format(param_limit)


       print("\nThe Estimated Kinematic Calibration is:")
       print("gain     = {}".format(gain))
       print("trim     = {}".format(trim))

       # Write to yaml file
       filename = ( home + "/duckietown_sysid/kinematics/" + self.robot_name + ".yaml")
       with open(filename, 'w') as outfile:
           outfile.write(datasave)
           #outfile.write(yaml.dump(datasave, default_flow_style=False))  # This did not work and gave very weird results

       print("Saved Parameters to " + self.robot_name + ".yaml" )

       print("\nPlease check the plots and judge if the parameters are reasonable.")
       print("Once done inspecting the plot, close them to terminate the program.")
Пример #4
0
 def load_camera_info(self):
     '''Load camera intrinsics'''
     filename = (sys.path[0] + "/calibrations/camera_intrinsic/" +
                 self.robot_name + ".yaml")
     if not os.path.isfile(filename):
         logger.warn(
             "no intrinsic calibration parameters for {}, trying default".
             format(self.robot_name))
         filename = (sys.path[0] +
                     "/calibrations/camera_intrinsic/default.yaml")
         if not os.path.isfile(filename):
             logger.error("can't find default either, something's wrong")
     calib_data = yaml_load_file(filename)
     #     logger.info(yaml_dump(calib_data))
     cam_info = CameraInfo()
     cam_info.width = calib_data['image_width']
     cam_info.height = calib_data['image_height']
     cam_info.K = np.array(calib_data['camera_matrix']['data']).reshape(
         (3, 3))
     cam_info.D = np.array(
         calib_data['distortion_coefficients']['data']).reshape((1, 5))
     cam_info.R = np.array(
         calib_data['rectification_matrix']['data']).reshape((3, 3))
     cam_info.P = np.array(calib_data['projection_matrix']['data']).reshape(
         (3, 4))
     cam_info.distortion_model = calib_data['distortion_model']
     logger.info(
         "Loaded camera calibration parameters for {} from {}".format(
             self.robot_name, os.path.basename(filename)))
     return cam_info
Пример #5
0
def download_url_to_file(url, filename):
    logger.info('Download from %s' % (url))
    tmp = filename + '.download'
    cmd = ['wget', '-O', tmp, url]
    d8n_make_sure_dir_exists(tmp)
    res = system_cmd_result(cwd='.',
                            cmd=cmd,
                            display_stdout=False,
                            display_stderr=False,
                            raise_on_error=True,
                            write_stdin='',
                            capture_keyboard_interrupt=False,
                            env=None)
    if not os.path.exists(tmp):
        msg = 'Downloaded file does not exist but wget did not give any error.'
        msg += '\n url: %s' % url
        msg += '\n downloaded to: %s' % tmp
        msg += '\n' + indent(str(res), ' | ')
        d = os.path.dirname(tmp)
        r = system_cmd_result(d, ['ls', '-l'],
                              display_stdout=False,
                              display_stderr=False,
                              raise_on_error=True)
        msg += '\n Contents of the directory:'
        msg += '\n' + indent(str(r.stdout), ' | ')
        raise Exception(msg)

    os.rename(tmp, filename)

    logger.info('-> %s' % friendly_path(filename))
Пример #6
0
def upload(to_upload):
    filename = '/tmp/what_the_duck'
    S = shelve.open(filename)
    try:
        #         logger.debug('New records: %s  previous: %s' % (len(to_upload), len(S)))

        for u in to_upload:
            S[u['_id']] = u

        if not is_internet_connected():
            msg = 'Internet is not connected: cannot upload results.'
            logger.warning(msg)
        else:
            remaining = []
            for k in S:
                remaining.append(S[k])

            collection = get_upload_collection()
            logger.info('Uploading %s test results' % len(remaining))
            collection.insert_many(remaining)
            logger.info('done')
            for r in remaining:
                del S[r['_id']]
    finally:
        #         logger.info('Remaining %s' % (len(S)))
        S.close()
Пример #7
0
def what_the_duck_stats():

    if len(sys.argv) > 1:
        output = sys.argv[1]
    else:
        output = 'what_the_duck_stats.html'

    collection = get_upload_collection()

    res = list(get_valid_data(collection))

    #     for r in res:
    #         del r['_id']
    #
    data = yaml_dump(res)

    write_data_to_file(data, 'last_download.yaml')

    hostnames = defaultdict(lambda: 0)

    for r in res:
        hostnames[r['hostname']] += 1

    s = 'Number of tests by hostname: '
    for h, n in hostnames.items():
        s += '\n- %s: %d tests' % (h, n)

    logger.info(s)

    html = create_summary(res)
    write_data_to_file(html, output)
Пример #8
0
    def estimate_homography(self, cv_image):
        '''Estimate ground projection using instrinsic camera calibration parameters'''
        cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
        cv_image_rectified = self.rectify(cv_image)
        logger.info("image rectified")

        ret, corners = cv2.findChessboardCorners(
            cv_image_rectified, (self.board_['width'], self.board_['height']),
            cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE)
        if ret == False:
            logger.error("No corners found in image")
            exit(1)
        if len(corners) != self.board_['width'] * self.board_['height']:
            logger.error("Not all corners found in image")
            exit(2)
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30,
                    0.01)
        corners2 = cv2.cornerSubPix(cv_image_rectified, corners, (11, 11),
                                    (-1, -1), criteria)

        cv2.drawChessboardCorners(cv_image_rectified, (7, 5), corners2, ret)
        cv2.imshow('corners', cv_image_rectified)
        cv2.waitKey(10000)
        cv2.destroyAllWindows()

        #TODO flip checks
        src_pts = []
        for r in range(self.board_['height']):
            for c in range(self.board_['width']):
                src_pts.append(
                    np.array([
                        r * self.board_['square_size'], c *
                        self.board_['square_size']
                    ],
                             dtype='float32') + self.board_['offset'])
        # OpenCV labels corners left-to-right, top-to-bottom
        # We're having a problem with our pattern since it's not rotation-invariant

        # only reverse order if first point is at bottom right corner
        if ((corners[0])[0][0] <
            (corners[self.board_['width'] * self.board_['height'] - 1])[0][0]
                and (corners[0])[0][0] <
            (corners[self.board_['width'] * self.board_['height'] - 1])[0][1]):
            rospy.loginfo("Reversing order of points.")
            src_pts.reverse()

        # Compute homography from image to ground
        self.H, mask = cv2.findHomography(corners2.reshape(len(corners2), 2),
                                          np.array(src_pts), cv2.RANSAC)
        extrinsics_filename = get_duckiefleet_root(
        ) + "/calibrations/camera_extrinsic/" + self.robot_name + ".yaml"
        self.write_homography(extrinsics_filename)
        logger.info(
            "Wrote ground projection to {}".format(extrinsics_filename))

        # Check if specific point in matrix is larger than zero (this would definitly mean we're having a corrupted rotation matrix)
        if (self.H[1][2] > 0):
            rospy.logerr("WARNING: Homography could be corrupt!")
Пример #9
0
def get_upload_collection():
    s = get_connection_string()

    logger.info('Opening connection to MongoDB...')
    client = pymongo.MongoClient(s)

    db = client[mongo_db]
    collection = db[mongo_collection]
    return collection
Пример #10
0
def job_analyze(log, analyzer):
    easy_algo_db = get_easy_algo_db()
    analyzer_instance = easy_algo_db.create_instance('analyzer', analyzer)
    in_bag = rosbag.Bag(log)
    results = OrderedDict()
    logger.info('Running %s on %s' % (analyzer, log))
    analyzer_instance.analyze_log(in_bag, results)
    in_bag.close()
    return results
Пример #11
0
def download_if_not_exist(url, filename):
    if not os.path.exists(filename):
        logger.info('Path does not exist: %s' % filename)
        download_url_to_file(url, filename)
        if not os.path.exists(filename):
            msg = 'I expected download_url_to_file() to raise an error if failed.'
            msg += '\n url: %s' % url
            msg += '\n filename: %s' % filename
            raise AssertionError(msg)
    return filename
Пример #12
0
def load_map(map_filename):
    if not isfile(map_filename):
        print('Map does not exist')
        exit(1)
    map_name = splitext(basename(map_filename))[0]
    with open(map_filename) as f:
        contents = f.read()
        data = yaml_load(contents)
    logger.info('Loaded {} map data'.format(map_name))
    return data
Пример #13
0
def calculate_transform(image):
    """
        Returns tuple (bool, float, parameters)
        
        success, health, parameters
        
        parameters['scale']
        parameters['shift']
    """
    centers4 = CENTERS2
    trained4, counter4, score4 = runKMeans(image, num_colors=4, init=centers4)
    trained4 = trained4[[0, 2, 3], :]
    counter4 = [counter4[0], counter4[2], counter4[3]]
    centers4 = centers4[[0, 2, 3], :]
    centers3 = CENTERS
    trained3, counter3, score3 = runKMeans(image, num_colors=3, init=centers3)
    decision34 = ((score3 + 3e7) > score4) > score4
    if (decision34):
        logger.info("picked 3 colors")
        trained = trained3
        counter = counter3
        centers = centers3
    else:
        logger.info("picked 4 colors")
        trained = trained4
        counter = counter4
        centers = centers4

    # milansc: for debug reasons
    print('Score for 3 colors: ' + str(score3))
    print('Score for 4 colors: ' + str(score4))

    print('...in calculate_transform: found centers:')
    print(centers)
    print('...in calculate_transform: found counter:')
    print(counter)
    print('...in calculate_transform: found trained centers:')
    print(trained)

    mapping = identifyColors(trained, centers)
    r, g, b, cost = getparameters2(mapping, trained, counter, centers)

    if r[0][0] == 0.0:
        # XXX: not sure what this is supposed to be
        return False, 0.0, None

    scale = np.array([r[0][0][0], g[0][0][0], b[0][0][0]])
    shift = np.array([r[1][0], g[1][0], b[1][0]])

    eps = np.finfo('double').eps
    health = 1.0 / (cost + eps)

    parameters = dict(scale=scale, shift=shift)

    return True, float(health), parameters
Пример #14
0
def read_scuderia(scuderia):
    """ Returns the contents of scuderia as a dict, with validated fields."""

    if not os.path.exists(scuderia):
        msg = 'Scuderia file %s does not exist' % scuderia
        raise ScuderiaException(msg)
    
    import yaml

    yaml_string = open(scuderia).read()

    try:
        values = yaml.load(yaml_string)
    except yaml.YAMLError as e:
        msg = 'Yaml file is invalid:\n---\n%s' % e
        raise ScuderiaException(msg)

    if not isinstance(values, dict):
        msg = 'Invalid content: %s' % values
        raise ScuderiaException(msg)

    n = len(values)

    names = ", ".join(sorted(values))
    logger.info('I found %d Duckiebots in the scuderia: %s.' % (n, names))

    results = {}
    
    for k, value in values.items():
        try:
            check_good_name(k)
            
            if not isinstance(value, dict):
                msg = 'Entry should be a dict, found %s' % k
                raise_desc(ScuderiaException, msg)
                
            fields = ['owner_duckietown_id', 'username', 'owner_duckietown_id']
            
            for f in fields:
                if not f in value:
                    msg = 'Could not find field "%s".' % f
                    raise ScuderiaException(msg) 
            
            owner_duckietown_id = value['owner_duckietown_id']
            username = value['username']
            robot_name = k
            
            results[robot_name] = ScuderiaEntry(robot_name=robot_name,  username=username, owner_duckietown_id=owner_duckietown_id)
            
        except ScuderiaException as e:
            msg = 'Invalid entry "%s" in scuderia.' % k
            msg += '\n\n' + indent(yaml.dump(value).strip(), '> ') + '\n    '
            raise_wrapped(ScuderiaException, e, msg, compact=True)

    return results
Пример #15
0
def load_homography(veh):
    path = '/calibrations/camera_'
    filename = get_duckiefleet_root() + path + 'extrinsic/' + veh + '.yaml'
    if not isfile(filename):
        print('Extrinsic calibration for {} does not exist.'.format(veh))
        exit(2)
    with open(filename) as f:
        contents = f.read()
        data = yaml_load(contents)
    logger.info('Loaded homography for {}'.format(veh))
    return np.array(data['homography']).reshape(3,3)
Пример #16
0
def write_to_file_if_changed(filename, contents):
    if os.path.exists(filename):
        existing = open(filename).read()
        need_write = existing != contents
    else:
        need_write = True

    if need_write:
        with open(filename, 'w') as f:
            f.write(contents)
        logger.info('Written to %s' % filename)
    else:
        logger.info('File already up to date %s' % filename)
Пример #17
0
def get_scuderia_contents():
    ''' Returns the contents of the scuderia file 
            from $(DUCKIEFLEET_ROOT)/scuderia.yaml
    '''
    fn_scuderia = get_scuderia_path()
    try:
        scuderia_contents = read_scuderia(fn_scuderia)
    except ScuderiaException as  e:
        msg = 'Invalid scuderia file %r.' % fn_scuderia
        raise_wrapped(ScuderiaException, e, msg, compact=True)
        
    logger.info('Read %d entries from scuderia %s' % (len(scuderia_contents), fn_scuderia))
    return scuderia_contents
def write_to_dir(data, d):
    if isinstance(data, dict):
        if not os.path.exists(d):
            os.makedirs(d)
        for k, v in data.items():
            write_to_dir(v, os.path.join(d, k))
    elif isinstance(data, str):
        with open(d, 'w') as f:
            f.write(data)
        logger.info('Wrote %s' % d)
    else:
        msg = 'Invalid type.'
        raise_desc(ValueError, msg, data=data, d=d)
def d8n_make_video_from_bag(bag_filename, topic, out):
    """
       Creates a video out of the topic in the bag.
       
       topic: the topic name (any image-like topic)
       out: an .mp4 file. 
       
       Note that needs a bunch more dependencies to be installed.
       
       Until we fix the dependencies:
        
            sudo pip install SystemCmd==1.2 ros_node_utils==1.0 ConfTools==1.8 QuickApp==1.2.2
        
            sudo apt-get install -y  mplayer mencoder
        
            sudo add-apt-repository ppa:mc3man/trusty-media
            sudo apt-get update
            sudo apt-get install -y ffmpeg 
            
        #gstreamer0.10-ffmpeg
        
    """
    try:
        import procgraph_ros  # @UnusedImport
        from procgraph import pg
    except ImportError:
        raise

    # pg -m procgraph_ros bag2mp4 --bag $bag --topic $topic --out $out

    model = 'bag2mp4_fixfps'
    #     model = 'bag2mp4'
    tmpdir = create_tmpdir()
    out_tmp = os.path.join(tmpdir, os.path.basename(out))
    logger.debug('Writing temp file to %s' % out_tmp)
    logger.debug('(You can use mplayer to follow along.)')
    pg(model, config=dict(bag=bag_filename, topic=topic, out=out_tmp))
    md = out_tmp + '.metadata.yaml'
    if os.path.exists(md):
        os.unlink(md)

    dn = os.path.dirname(out)
    if not os.path.exists(dn):
        os.makedirs(dn)

    shutil.copyfile(out_tmp, out)
    logger.info('Created: %s' % out)

    info = out_tmp + '.info.yaml'
    if os.path.exists(info):
        os.unlink(info)
def d8n_write_to_bag_context(out_bag_filename):
    """
        with d8n_write_to_bag_context(filename) as bag:
            bag.write(topic_name, msg)
    
    """
    import rosbag  # @UnresolvedImport

    d8n_make_sure_dir_exists(out_bag_filename)
    out_bag = rosbag.Bag(out_bag_filename + '.tmp', 'w')
    yield out_bag
    out_bag.close()
    os.rename(out_bag_filename + '.tmp', out_bag_filename)
    logger.info('Written bag to %s' % out_bag_filename)
Пример #21
0
 def load_homography(self):
     '''Load homography (extrinsic parameters)'''
     filename = (get_duckiefleet_root() + "/calibrations/camera_extrinsic/" + self.robot_name + ".yaml")
     if not os.path.isfile(filename):
         logger.warn("no extrinsic calibration parameters for {}, trying default".format(self.robot_name))
         filename = (get_duckiefleet_root() + "/calibrations/camera_extrinsic/default.yaml")
         if not os.path.isfile(filename):
             logger.error("can't find default either, something's wrong")
         else:
             data = yaml_load_file(filename)
     else:
         data = yaml_load_file(filename)
     logger.info("Loaded homography for {}".format(os.path.basename(filename)))
     return np.array(data['homography']).reshape((3,3))
Пример #22
0
def upload_results(results):
    to_upload = []
    d0 = get_local_keys()

    for result in results:
        d1 = json_from_result(result)
        d1.update(d0)
        to_upload.append(d1)

    collection = get_upload_collection()

    logger.info('Inserting %s tests' % len(to_upload))
    collection.insert_many(to_upload)
    logger.info('done')
Пример #23
0
def dropbox_links_main(query):
    logger.info('NOTE: Run this inside ~/Dropbox/duckietown-data. ')
    logger.info(
        'NOTE: There are some hard-coded paths to modify in dropbox_links.py')

    output = get_urls_path()
    if os.path.exists(output):
        urls = yaml.safe_load(open(output).read())
        for k, v in list(urls.items()):
            if not v.startswith('http'):
                del urls[k]
    else:
        urls = {}
    command = '/home/andrea/bin/dropbox'
    base = '/mnt/dorothy-duckietown-data/'
    db = get_easy_logs_db()
    logs = db.query(query)
    logger.info('Found %d logs.' % len(logs))

    for logname, log in logs.items():
        if logname in urls:
            logger.info('Already have %s' % logname)
            continue

        filename = log.filename
        only = filename.replace(base, '')

        cmd = [command, 'sharelink', only]
        res = system_cmd_result(cwd='.',
                                cmd=cmd,
                                display_stdout=False,
                                display_stderr=True,
                                raise_on_error=True,
                                write_stdin='',
                                capture_keyboard_interrupt=False,
                                env=None)
        link = res.stdout.strip()
        if 'responding' in link:
            logger.debug('Dropbox is not responding, I will stop here.')

            break

        logger.info('link : %s' % link)
        urls[logname] = link

    yaml.default_flow_style = True
    #     yaml.indent(mapping=2, sequence=4, offset=2)
    with open(output, 'w') as f:
        yaml.dump(urls, f)
Пример #24
0
 def load_board_info(self, filename=''):
     '''Load calibration checkerboard info'''
     if not os.path.isfile(filename):
         filename = get_ros_package_path('ground_projection') + '/config/ground_projection_node/default.yaml'
     target_data = yaml_load_file(filename)
     target_info = {
         'width': target_data['board_w'],
         'height': target_data['board_h'],
         'square_size': target_data['square_size'],
         'x_offset': target_data['x_offset'],
         'y_offset': target_data['y_offset'],
         'offset': np.array([target_data['x_offset'], -target_data['y_offset']]),
         'size': (target_data['board_w'], target_data['board_h']),
       }
     logger.info("Loaded checkerboard parameters")
     return target_info
Пример #25
0
def get_easy_logs_db_cloud():
    cloud_file = require_resource('cloud.yaml')

    #     cloud_file = os.path.join(get_ros_package_path('easy_logs'), 'cloud.yaml')
    #     if not os.path.exists(cloud_file):
    #         url = "https://www.dropbox.com/s/vdl1ej8fihggide/duckietown-cloud.yaml?dl=1"
    #         download_url_to_file(url, cloud_file)

    logger.info('Loading cloud DB %s' % friendly_path(cloud_file))

    logs = yaml_load_file(cloud_file)

    logs = OrderedDict(logs)
    logger.info('Loaded cloud DB with %d entries.' % len(logs))

    return EasyLogsDB(logs)
Пример #26
0
def load_camera_intrinsics(veh):
    path = '/calibrations/camera_'
    filename = get_duckiefleet_root() + path + 'intrinsic/' + veh + '.yaml'
    if not isfile(filename):
        print('Intrinsic calibration for {} does not exist.'.format(veh))
        exit(3)
    with open(filename) as f:
        contents = f.read()
        data = yaml_load(contents)
    intrinsics = {}
    intrinsics['K'] = np.array(data['camera_matrix']['data']).reshape(3, 3)
    intrinsics['D'] = np.array(data['distortion_coefficients']['data']).reshape(1, 5)
    intrinsics['R'] = np.array(data['rectification_matrix']['data']).reshape(3, 3)
    intrinsics['P'] = np.array(data['projection_matrix']['data']).reshape((3,4))
    intrinsics['distortion_model'] = data['distortion_model']
    logger.info('Loaded camera intrinsics for {}'.format(veh))
    return intrinsics
Пример #27
0
    def estimate_homography(self,cv_image, board_size, offset):
        cv_image_rectified = self.rectify(cv_image)
        logger.info("image rectified")
        
        corners = cv2.findChessboardCorners(cv_image, board_size)
        if corners is None:
            logger.error("No corners found in image")
        criteria = (cv2.CV_TERMCRIT_EPS + cv2.CV_TERMCRIT_ITER,30,0.1)
        cv2.cornerSubPix(cv_image,corners,cv2.Size(11,11),cv2.Size(-1,-1),criteria)

        #TODO flip checks

        for r in range(board_h):
            for c in range(board_w):
                src_pts[r,c] = np.float32([r*square_size,c*square_size]) + offset

        self.H, mask = cv2.findHomography(src_pts, corners, method=cv2.CV_RANSAC)
        write_homography(self.extrinsic_filename)
Пример #28
0
def call_summary():
    db = get_easy_algo_db()
    s = format_db(db)
    logger.info(s)

    errors = []
    for f in db.family_name2config.values():
        if not f.valid:
            errors.append('Family %s: %s' %
                          (f.family_name, f.error_if_invalid))
        for i in f.instances.values():
            if not i.valid:
                errors.append('Family %s / instance %r: %s' %
                              (f, i.instance_name, i.error_if_invalid))

    if errors:
        msg = '\n' + '\n'.join(f.instances)
        raise Exception(msg)
Пример #29
0
def get_valid_data(collection):

    res = list(collection.find())
    out = []
    for r in res:
        if 'what_the_duck_version' in r:
            v = r['what_the_duck_version']
            if v in ['1.1', '1.2']:
                del r['_id']

                if v in ['1.1']:
                    r['type'] = '?'

                out.append(r)

    logger.info('Found %d database entries; %d are compatible.' %
                (len(res), len(out)))
    return out
Пример #30
0
def search_all_configuration_files():
    sources = []
    # We look in $DUCKIETOWN_ROOT/catkin_ws/src
    sources.append(get_catkin_ws_src())
    # then we look in $DUCKIETOWN_FLEET
    sources.append(get_duckiefleet_root())

    configs = []
    pattern = '*' + SUFFIX
    logger.info('Looking for %s files.' % pattern)
    for s in sources:
        fs = locate_files(s, pattern)
        logger.info('%4d files in %s' % (len(fs), s))
        configs.extend(fs)


#     logger.debug('I found:\n' + "\n".join(configs))

    return configs