def test_average_height():
mesh_file = '/path/to/mesh_file'
pose_file = '/path/to/pose_file'
poses = load_poses(pose_file)
submap_test = MapModule(poses, mesh_file)
# get local map
for idx in range(len(submap_test.tiles)):
print("tile_idx: ", idx, " z: ", submap_test.tiles[idx].z)
def test_get_rearranged_vertices_buffer():
mesh_file = '/path/to/mesh_file'
pose_file = '/path/to/pose_file'
poses = load_poses(pose_file)
submap_test = MapModule(poses, mesh_file)
global_vertices = np.array(submap_test.global_vertices).reshape((-1, 3))
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(global_vertices)
o3d.visualization.draw_geometries([pcd])
def test_get_map():
mesh_file = '/path/to/mesh_file'
pose_file = '/path/to/pose_file'
poses = load_poses(pose_file)
submap_test = MapModule(poses, mesh_file, keep_tile_maps=True)
# get global map
global_mesh = submap_test.get_global_map()
o3d.visualization.draw_geometries([global_mesh])
# get local map
for idx in range(len(submap_test.tiles)):
global_mesh = submap_test.get_local_map(idx)
o3d.visualization.draw_geometries([global_mesh])
def test_tile_map_vertex():
mesh_file = '/path/to/mesh_file'
pose_file = '/path/to/pose_file'
poses = load_poses(pose_file)
submap_test = MapModule(poses, mesh_file)
for idx in range(len(submap_test.tiles)):
tile = submap_test.tiles[idx]
pcd = o3d.geometry.PointCloud()
vertices = np.array(tile.vertices).reshape((-1, 3))
pcd.points = o3d.utility.Vector3dVector(vertices)
o3d.visualization.draw_geometries([pcd])
config = yaml.load(open(config_filename), Loader=yaml.FullLoader) else: config = yaml.load(open(config_filename)) # specify parameters resolution = config['resolution'] # resolution of grid is 20 cm offset = config['offset'] # for each frame, we generate grids inside a 1m*1m square num_frames = config['num_frames'] # specify the output folders virtual_scan_folder = '../' + config['virtual_scan_folder'] map_file = '../' + config['map_file'] # load poses pose_file = '../' + config['pose_file'] poses = np.array(utils.load_poses(pose_file)) inv_frame0 = np.linalg.inv(poses[0]) # load calibrations calib_file = '../' + config['calib_file'] T_cam_velo = utils.load_calib(calib_file) T_cam_velo = np.asarray(T_cam_velo).reshape((4, 4)) T_velo_cam = np.linalg.inv(T_cam_velo) # convert kitti poses from camera coord to LiDAR coord new_poses = [] for pose in poses: new_poses.append(T_velo_cam.dot(inv_frame0).dot(pose).dot(T_cam_velo)) poses = np.array(new_poses) # load LiDAR scans
def main(config):
""" This script can be used to create mesh maps using LiDAR scans with GT poses.
It assumes you have the data in the kitti-like format like:
data
└── sequences
└── 00
├── calib.txt
├── poses.txt
└── velodyne
├── 000000.bin
├── 000001.bin
└── ...
How to run it and check a quick example:
$ ./build_gt_map.py /path/to/config.yaml
"""
# load scans and poses
scan_folder = config['scan_folder']
scan_paths = load_files(scan_folder)
# load poses
pose_file = config['pose_file']
poses = load_poses(pose_file)
inv_frame0 = np.linalg.inv(poses[0])
# load calibrations
# Note that if your poses are already in the LiDAR coordinate system, you
# just need to set T_cam_velo as a 4x4 identity matrix
calib_file = config['calib_file']
T_cam_velo = load_calib(calib_file)
T_cam_velo = np.asarray(T_cam_velo).reshape((4, 4))
T_velo_cam = np.linalg.inv(T_cam_velo)
# convert poses into LiDAR coordinate system
new_poses = []
for pose in poses:
new_poses.append(T_velo_cam.dot(inv_frame0).dot(pose).dot(T_cam_velo))
new_poses = np.array(new_poses)
gt_poses = new_poses
# Use the whole sequence if -1 is specified
n_scans = len(scan_paths) if config['n_scans'] == -1 else config['n_scans']
# init mesh map
mesh_file = config['mesh_file']
if os.path.exists(mesh_file):
exit(print('The mesh map already exists at:', mesh_file))
global_mesh = o3d.geometry.TriangleMesh()
cloud_map = o3d.geometry.PointCloud()
# counter for local map
count = 1
local_map_size = config['local_map_size']
# config for range images
range_config = config['range_image']
for idx in tqdm(range(n_scans)):
# load the point cloud
curren_points = load_vertex(scan_paths[idx])
# get rid of invalid points
dist = np.linalg.norm(curren_points[:, :3], 2, axis=1)
curren_points = curren_points[(dist < range_config['max_range'])
& (dist > range_config['min_range'])]
# convert into open3d format and preprocess the point cloud
local_cloud = o3d.geometry.PointCloud()
local_cloud.points = o3d.utility.Vector3dVector(curren_points[:, :3])
# estimated normals
local_cloud = compute_normals_range(local_cloud,
range_config['fov_up'],
range_config['fov_down'],
range_config['height'],
range_config['width'],
range_config['max_range'])
# preprocess point clouds
local_cloud = preprocess_cloud(local_cloud,
config['voxel_size'],
config['crop_x'],
config['crop_y'],
config['crop_z'],
downsample=True)
# integrate the local point cloud
local_cloud.transform(gt_poses[idx])
cloud_map += local_cloud
if idx > 0:
# if the car stops, we don't count the frame
relative_pose = np.linalg.inv(gt_poses[idx - 1]).dot(gt_poses[idx])
traj_dist = np.linalg.norm(relative_pose[:3, 3])
if traj_dist > 0.2:
count += 1
# build a local mesh map
if count % local_map_size == 0:
# segment the ground
ground, rest = pcd_ground_seg_open3d(cloud_map, config)
# build the local poisson mesh
mesh = run_poisson(ground + rest,
depth=config['depth'],
min_density=config['min_density'])
# simply the ground to save space
mesh = mesh_simplify(mesh, config)
mesh.compute_vertex_normals()
mesh.compute_triangle_normals()
# integrate the local mesh into global mesh
global_mesh += mesh
# re-init cloud map
cloud_map = o3d.geometry.PointCloud()
# save the mesh map
print("Saving mesh to " + mesh_file)
o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Error)
o3d.io.write_triangle_mesh(mesh_file, global_mesh)
# visualize the mesh map
if config['visualize']:
o3d.visualization.draw_geometries([global_mesh])
# load config file
config_filename = '../config/localization.yml'
if len(sys.argv) > 1:
config_filename = sys.argv[1]
config = yaml.load(open(config_filename), Loader=yaml.FullLoader)
start_idx = config['start_index']
grid_res = config['resolution']
numParticles = config['numParticles']
save_result = config['save_result']
visualize = config['visualize']
data_root_folder = config['data_root_folder']
# load poses
pose_file = config['pose_file']
poses = utils.load_poses(pose_file)
inv_frame0 = np.linalg.inv(poses[0])
# load calibrations
calib_file = config['calib_file']
T_cam_velo = utils.load_calib(calib_file)
T_cam_velo = np.asarray(T_cam_velo).reshape((4, 4))
T_velo_cam = np.linalg.inv(T_cam_velo)
new_poses = []
for pose in poses:
new_poses.append(T_velo_cam.dot(inv_frame0).dot(pose).dot(T_cam_velo))
poses = np.array(new_poses)
# load map
map_folder = os.path.join(data_root_folder, config['infer_seqs_map'],
config_filename = '../config/evaluation.yml'
if len(sys.argv) > 1:
config_filename = sys.argv[1]
config = yaml.safe_load(open(config_filename))
# load setups
plot_loc_traj = config['plot_loc_traj']
save_evaluation_results = config['save_evaluation_results']
# load ground truth files
pose_file = config['pose_file']
calib_file = config['calib_file']
# load ground truth poses
poses = np.array(load_poses(pose_file))
inv_frame0 = np.linalg.inv(poses[0])
# load calibrations
T_cam_velo = load_calib(calib_file)
T_cam_velo = np.asarray(T_cam_velo).reshape((4, 4))
T_velo_cam = np.linalg.inv(T_cam_velo)
# convert poses in LiDAR coordinate system
new_poses = []
for pose in poses:
new_poses.append(T_velo_cam.dot(inv_frame0).dot(pose).dot(T_cam_velo))
new_poses = np.array(new_poses)
gt_poses = new_poses
gt_xy_raw = gt_poses[:, :2, 3]
