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)