Esempio n. 1
0
def export_map_data(gps_file, rss):
    route, segment, split = rss
    dset = '%s_%s' % (segment, split)
    dset_dir = pjoin(pjoin(DATA_DIR, route), dset)
    stem = '_'.join(os.path.basename(gps_file).split('_')[0:-1])
    video = '%s%d.avi' % (stem, CAMERA)
    fname = '--'.join((route, segment, split)) + '.npz'
    opt_pos = '%s/%s' % (GRAPHSLAM_OPT_POS_DIR, fname)
    cmd = 'python %s/lidar_integrator.py %s %s %s %s --export --full' % (GRAPHSLAM_PATH, dset_dir, video, opt_pos, pjoin(GRAPHSLAM_MAPS_DIR, fname))
    print_and_call(cmd)
Esempio n. 2
0
def export_map_data(gps_file, rss):
    route, segment, split = rss
    dset = '%s_%s' % (segment, split)
    dset_dir = pjoin(pjoin(DATA_DIR, route), dset)
    stem = '_'.join(os.path.basename(gps_file).split('_')[0:-1])
    video = '%s%d.avi' % (stem, CAMERA)
    fname = '--'.join((route, segment, split)) + '.npz'
    opt_pos = '%s/%s' % (GRAPHSLAM_OPT_POS_DIR, fname)
    cmd = 'python %s/lidar_integrator.py %s %s %s %s --export --full' % (
        GRAPHSLAM_PATH, dset_dir, video, opt_pos,
        pjoin(GRAPHSLAM_MAPS_DIR, fname))
    print_and_call(cmd)
Esempio n. 3
0
def generate_videos(dummy, sentinel):
    cmd = 'python scripts/generate_videos.py'
    print_and_call(cmd)
    touchf('%s/generate_videos_sentinel' % GRAPHSLAM_VIDEOS_DIR)
Esempio n. 4
0
def eval_maps(dummy, sentinel):
    cmd = 'python scripts/eval_maps.py'
    print_and_call(cmd)
    touchf('%s/eval_maps_sentinel' % GRAPHSLAM_EVAL_DIR)
Esempio n. 5
0
def align_maps(dummy, sentinel):
    cmd = 'python scripts/align_maps_all.py'
    print_and_call(cmd)
    touchf('%s/align_maps_sentinel' % GRAPHSLAM_MAPS_DIR)
Esempio n. 6
0
def export_maps(dummy, sentinel):
    cmd = 'python scripts/export_maps.py'
    print_and_call(cmd)
    touchf('%s/export_maps_sentinel' % GRAPHSLAM_MAPS_DIR)
Esempio n. 7
0
def chunk_and_align(dummy, sentinel):
    cmd = 'python %s/chunk_and_align.py' % GRAPHSLAM_PATH
    print_and_call(cmd)
    touchf('%s/chunk_and_align_sentinel' % GRAPHSLAM_ALIGN_DIR)
Esempio n. 8
0
def clean_pipelines():
    for route, segment, split in RSS_LIST:
        cmd = 'export SCL_ROUTE=%s; export SCL_SEGMENT=%s; export SCL_SPLIT=%s; python %s/mapping/pipeline/pipeline.py clean' % (route, segment, split, SAIL_CAR_LOG_PATH)
        print_and_call(cmd)
Esempio n. 9
0
def run_pipelines(dummy, sentinel):
    for route, segment, split in RSS_LIST:
        cmd = 'export SCL_ROUTE=%s; export SCL_SEGMENT=%s; export SCL_SPLIT=%s; python %s/mapping/pipeline/pipeline.py run estimate_normals' % (route, segment, split, SAIL_CAR_LOG_PATH)
        print_and_call(cmd)
    touchf('%s/run_pipeline_sentinel' % GRAPHSLAM_OUT_DIR)
Esempio n. 10
0
def solve_qps(gps_src_file, output_file, gps_tgt_file):
    cmd = 'python %s/solve_qp.py %s %s %s' % (GRAPHSLAM_PATH,
            gps_src_file, gps_tgt_file, output_file)
    print_and_call(cmd)
Esempio n. 11
0
def match_traces(dummy, output_file):
    cmd = 'python %s/match_traces.py %s' % (GRAPHSLAM_PATH, GRAPHSLAM_MATCH_DIR)
    print_and_call(cmd)
Esempio n. 12
0
if __name__ == '__main__':
    # First compute bounding box / center data
    bounds_file_pairs = list()
    for match in MATCH_JSON_DATA:
        fstem1 = '--'.join(match['rss1'])
        fstem2 = '--'.join(match['rss2'])

        map_data1 = pjoin(GRAPHSLAM_MAPS_DIR, fstem1 + '.h5')
        map_data2 = pjoin(GRAPHSLAM_MAPS_DIR, fstem2 + '.h5')
        pcd1 = splitext(map_data1)[0] + '.pcd'
        pcd2 = splitext(map_data2)[0] + '.pcd'

        # First convert to pcd format
        cmd = '%s/bin/h5_to_pcd --h5 %s --pcd %s' % (MAPPING_PATH, map_data1,
                                                     pcd1)
        print_and_call(cmd)
        cmd = '%s/bin/h5_to_pcd --h5 %s --pcd %s' % (MAPPING_PATH, map_data2,
                                                     pcd2)
        print_and_call(cmd)

        # Then compute the bounding boxes
        bounds1 = '%s/%s.h5' % (GRAPHSLAM_EVAL_DIR, splitext(
            basename(pcd1))[0])
        bounds2 = '%s/%s.h5' % (GRAPHSLAM_EVAL_DIR, splitext(
            basename(pcd2))[0])
        cmd = '%s/bin/compute_bounds %s %s' % (MAPPING_PATH, pcd1, bounds1)
        print_and_call(cmd)
        cmd = '%s/bin/compute_bounds %s %s' % (MAPPING_PATH, pcd2, bounds2)
        print_and_call(cmd)

        bounds_file_pairs.append((bounds1, bounds2))
Esempio n. 13
0
if __name__ == '__main__':
    # First compute bounding box / center data
    bounds_file_pairs = list()
    for match in MATCH_JSON_DATA:
        fstem1 = '--'.join(match['rss1'])
        fstem2 = '--'.join(match['rss2'])

        map_data1 = pjoin(GRAPHSLAM_MAPS_DIR, fstem1 + '.h5')
        map_data2 = pjoin(GRAPHSLAM_MAPS_DIR, fstem2 + '.h5')
        pcd1 = splitext(map_data1)[0] + '.pcd'
        pcd2 = splitext(map_data2)[0] + '.pcd'

        # First convert to pcd format
        cmd = '%s/bin/h5_to_pcd --h5 %s --pcd %s' % (MAPPING_PATH, map_data1, pcd1)
        print_and_call(cmd)
        cmd = '%s/bin/h5_to_pcd --h5 %s --pcd %s' % (MAPPING_PATH, map_data2, pcd2)
        print_and_call(cmd)

        # Then compute the bounding boxes
        bounds1 = '%s/%s.h5' % (GRAPHSLAM_EVAL_DIR, splitext(basename(pcd1))[0])
        bounds2 = '%s/%s.h5' % (GRAPHSLAM_EVAL_DIR, splitext(basename(pcd2))[0])
        cmd = '%s/bin/compute_bounds %s %s' % (MAPPING_PATH, pcd1, bounds1)
        print_and_call(cmd)
        cmd = '%s/bin/compute_bounds %s %s' % (MAPPING_PATH, pcd2, bounds2)
        print_and_call(cmd)

        bounds_file_pairs.append((bounds1, bounds2))

    # Now compute match quality
    eval_json = dict()
Esempio n. 14
0
def chunk_and_align(start1, start2, enu1, enu2, rss1, rss2, pcd_dir1, pcd_dir2, chunk_num, debug=False):
    chunk1_files = list()
    chunk2_files = list()
    for k in range(0, CHUNK_SIZE):
        ind1 = start1 + k
        chunk1_files.append('%s/%d.pcd' % (pcd_dir1, ind1))
        assert os.path.exists(chunk1_files[-1])
        ind2 = start2 + k
        chunk2_files.append('%s/%d.pcd' % (pcd_dir2, ind2))
        assert os.path.exists(chunk2_files[-1])

    merged_chunks1 = '%s/%s' % (GRAPHSLAM_CHUNK_DIR, '--'.join(rss1) + '+' + '--'.join(rss2) + '%d_1.pcd' % chunk_num)
    merged_chunks2 = '%s/%s' % (GRAPHSLAM_CHUNK_DIR, '--'.join(rss1) + '+' + '--'.join(rss2) + '%d_2.pcd' % chunk_num)

    # Concatenate

    cmd = 'concatenate_points_pcd %s %s' % (' '.join(chunk1_files), merged_chunks1)
    print_and_call(cmd)

    cmd = 'concatenate_points_pcd %s %s' % (' '.join(chunk2_files), merged_chunks2)
    print_and_call(cmd)

    # Translate

    cmd = 'transform_point_cloud %s %s -trans %f,%f,%f' % (merged_chunks1, merged_chunks1, enu1[0]-enu2[0], enu1[1]-enu2[1], enu1[2]-enu2[2])
    print_and_call(cmd)
    #cmd = 'transform_point_cloud %s %s -trans %f,%f,%f' % (merged_chunks2, merged_chunks2, -1*enu2[0], -1*enu2[1], -1*enu2[2])
    #print_and_call(cmd)

    # Generate VTK files so we can easily visualize to debug

    if debug:
        cmd = 'pcl_pcd2vtk %s %s' % (merged_chunks1, vtk_filename(merged_chunks1))
        print_and_call(cmd)
        cmd = 'pcl_pcd2vtk %s %s' % (merged_chunks2, vtk_filename(merged_chunks2))
        print_and_call(cmd)

    # Finally perform alignment

    reg = '%s/bin/align_clouds' % MAPPING_PATH
    h5f = '%s/%s' % (GRAPHSLAM_ALIGN_DIR, '--'.join(rss1) + '+' + '--'.join(rss2) + '--%d' % chunk_num + '.h5')

    cmd = '{reg} --pcd_tgt {tgt} --pcd_src {src} --h5_file {h5f} --icp_iters {iters} --max_dist {dist}'.format(
            reg=reg, tgt=merged_chunks1, src=merged_chunks2, h5f=h5f, iters=ICP_ITERS, dist=ICP_MAX_DIST)
    print_and_call(cmd)