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)
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)
def generate_videos(dummy, sentinel): cmd = 'python scripts/generate_videos.py' print_and_call(cmd) touchf('%s/generate_videos_sentinel' % GRAPHSLAM_VIDEOS_DIR)
def eval_maps(dummy, sentinel): cmd = 'python scripts/eval_maps.py' print_and_call(cmd) touchf('%s/eval_maps_sentinel' % GRAPHSLAM_EVAL_DIR)
def align_maps(dummy, sentinel): cmd = 'python scripts/align_maps_all.py' print_and_call(cmd) touchf('%s/align_maps_sentinel' % GRAPHSLAM_MAPS_DIR)
def export_maps(dummy, sentinel): cmd = 'python scripts/export_maps.py' print_and_call(cmd) touchf('%s/export_maps_sentinel' % GRAPHSLAM_MAPS_DIR)
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)
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)
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)
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)
def match_traces(dummy, output_file): cmd = 'python %s/match_traces.py %s' % (GRAPHSLAM_PATH, GRAPHSLAM_MATCH_DIR) print_and_call(cmd)
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))
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()
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)