def cli():
parser = argparse.ArgumentParser()
parser.add_argument('output', type=str, help='Where to save the vtk file.')
parser.add_argument('--dim1', type=int, default=0)
parser.add_argument('--dim2', type=int, default=1)
parser.add_argument('--dim3', type=int, default=2)
parser.add_argument('--rotation', action='store_true', help='Print rotation dimensions instead of translation.')
parser.add_argument('--initial-estimate', action='store_true', help='Print initial estimates instead of results.')
parser.add_argument('--center_around_gt', action='store_true', help='Center the results around the ground truth')
parser.add_argument('-rz', '--rotation_around_z', type=float, default=0.0)
parser.add_argument('-k', help='N of neighbors for density computation', type=int, default=12)
args = parser.parse_args()
json_data = json.load(sys.stdin)
if args.rotation:
dims = (3,4,5)
else:
dims = (args.dim1, args.dim2, args.dim3)
left_multiply = rotation_around_z_matrix(args.rotation_around_z)
right_multiply = np.linalg.inv(rotation_around_z_matrix(args.rotation_around_z))
if args.center_around_gt:
left_multiply = np.dot(left_multiply, np.linalg.inv(np.array(json_data['metadata']['ground_truth'])))
points = positions_of_registration_data(json_data, (args.initial_estimate in POSITIVE_STRINGS), left_multiply=left_multiply, right_multiply=right_multiply)
print(points)
data_dict = empty_to_none(data_dict_of_registration_data(json_data, k=args.k))
pointcloud_to_vtk(points[:, dims], args.output, data_dict)
def to_vtk(dataset, clustering, output, center_around_gt=False):
"""
Make a vtk file showing a clustering on a certain dataset.
:arg dataset: The full dataset to which the clustering is applied.
:arg clustering: The clustering data row.
"""
points = positions_of_registration_data(dataset)
if center_around_gt:
points = center_lie_around_t(
points, np.array(dataset['metadata']['ground_truth']))
clustering_data = clusters_of_points(clustering['clustering'], len(points))
data_dict = data_dict_of_registration_data(dataset)
data_dict['clustering'] = np.ascontiguousarray(clustering_data)
pointcloud_to_vtk(points[:, 0:3],
'{}_translation'.format(output),
data=data_dict)
pointcloud_to_vtk(points[:, 3:6],
'{}_rotation'.format(output),
data=data_dict)
mean = np.array(clustering['mean_of_central'])
mean = se3log(mean)
covariance = np.array(clustering['covariance_of_central'])
distribution_to_vtk_ellipsoid(mean[0:3], covariance[0:3, 0:3],
'{}_translation_ellipsoid'.format(output))
distribution_to_vtk_ellipsoid(mean[3:6], covariance[3:6, 3:6],
'{}_rotation_ellipsoid'.format(output))
def generate_one_prediction(i, y_predicted, pair_id, registration_pair_database, output):
distribution_to_vtk_ellipsoid(np.zeros(3), y_predicted[0:3,0:3], output + '/translation_predicted_' + str(i).zfill(4))
distribution_to_vtk_ellipsoid(np.zeros(3), y_predicted[3:6,3:6], output + '/rotation_predicted_' + str(i).zfill(4))
registration_pair = registration_pair_database.get_registration_pair(pair_id['dataset'], pair_id['reading'], pair_id['reference'])
reading = registration_pair.points_of_reading()
pointcloud_to_vtk(reading, output + '/reading_{}'.format(str(i).zfill(4)))
def cli():
parser = argparse.ArgumentParser()
parser.add_argument('input', type=str, help='Registration results')
parser.add_argument('output', type=str, help='Output folder')
args = parser.parse_args()
with open(args.input) as f:
data_dict = json.load(f)
lie = lie_vectors_of_registrations(data_dict)
density = density_of_points(lie)
group = np.zeros((len(lie), 4, 4))
for i, l in enumerate(lie):
group[i] = se3.exp(l)
print(lie)
print(group)
print(density)
sorted_densities = sorted(density)
sample_densities = np.linspace((np.min(density)), (np.max(density)), 50)
print(sample_densities)
print(np.mean(lie, axis=0))
for i in range(0, 4000, 100):
d = sorted_densities[i]
results_subset = group[density > sorted_densities[i]]
if len(results_subset) > 10:
mean, covariance = se3.gaussian_from_sample(results_subset)
pointcloud_to_vtk(
lie[density > d][:, 0:3],
'points_{}'.format(i),
data={'density': np.ascontiguousarray(density[density > d])})
distribution_to_vtk_ellipsoid(
se3.log(mean)[0:3], covariance[0:3, 0:3],
'ellipsoid_{}'.format(i))
print(covariance[0:3, 0:3])
def apply_mask_cli():
parser = argparse.ArgumentParser(description='Apply as point selection mask on a pair of pointclouds.')
parser.add_argument('database', type=str, help='Location of the registration result database to use.')
parser.add_argument('dataset', type=str)
parser.add_argument('reading', type=int)
parser.add_argument('reference', type=int)
parser.add_argument('--output', type=str, default='.', help='Output directory of the visualization.')
parser.add_argument('--radius', type=float, default=0.1, help='For the overlap mask generator, the max distance between points for them to be neighbors.')
parser.add_argument('--range', type=float, help='For the angle mask generator, the range of angles accepted.', default=0.0)
parser.add_argument('--offset', type=float, help='For the angle mask generator, the offset of angles accepted.', default=0.0)
parser.add_argument('-c', '--config', type=str, help='Path to a json config for the mask')
parser.add_argument('-r', '--rotation', type=float, help='Rotation around the z axis to apply to the cloud pair before computing the descriptor, in radians.', default=0.0)
args = parser.parse_args()
db = RegistrationPairDatabase(args.database)
pair = db.get_registration_pair(args.dataset, args.reading, args.reference)
pair.rotation_around_z = args.rotation
reading = pair.points_of_reading()
reference = pair.points_of_reference()
with open(args.config) as f:
config = json.load(f)
print(config)
mask_generator = mask_factory(config)
reading_masks, reference_masks = mask_generator.compute(pair)
eprint('Transform of pair: ')
eprint(pair.transform())
pointcloud_to_vtk(reference, args.output + '/reference')
pointcloud_to_vtk(transform_points(reading, pair.transform()), args.output + '/reading')
for i in range(len(reading_masks)):
if reference_masks[i].any():
pointcloud_to_vtk(reference[reference_masks[i]], args.output + '/' + '{}_reference_{}'.format(mask_generator.__repr__(), i))
if reading_masks[i].any():
transformed_masked_reading = transform_points(reading[reading_masks[i]], pair.transform())
pointcloud_to_vtk(transformed_masked_reading, args.output + '/' + '{}_reading_{}'.format(mask_generator.__repr__(), i))
def dataset_to_vtk(dataset, filename, dims=(0, 1, 2)):
positions = positions_of_registration_data(dataset)
data = empty_to_none(data_dict_of_registration_data(dataset))
pointcloud_to_vtk(positions[:, dims], filename, data=data)
def save_one_frame(points, output, data_dict, i, pre_transform):
filename = output + '_{0:03d}'.format(i)
print(filename)
data = empty_to_none(data_dict)
pointcloud_to_vtk(points[i], filename, data)