Generalize odometry inspector

[miguelriemoliveira]

Because of #970 we now seem to have problems in the initial transforms for another sensor.

That means we have to generalize the odometry inspector to run be able to cope with any transformation.

Ultimately we will remove the set initial estimate working from the bagfile, and create a dataset_playbag 2.0 that will allow us to correct annotations and manually set any transformations we'd like.

[miguelriemoliveira]

Working with multiple transforms.

3d / 2d mode operational.

Revised the transform tree not to replicate a branch for each collection but rather to have a single branch + the reference collection branch. Much lighter like this.

[miguelriemoliveira]

Now we need to detect if the collection is fixed or dynamic so that when the marker is moved we affect only the selected collection (if the transform is dynamic) or affect all the transformations (if the transform is static).

@Kazadhum you were doing this just yesterday, right?

[Kazadhum]

Hi @miguelriemoliveira!

Indeed, I wrote a check for if a given TF was fixed! It's not a separate function, but its here:

atom/atom_evaluation/scripts/other_calibrations/copy_tfs_from_dataset.py

Lines 71 to 89 in 3d8558f

	# Organize the -pll and -cll args into a list of tuples [(parent1,child1), ...]
	parent_child_list = [(args["parent_link_list"][i], args["child_link_list"][i]) for i in range(len(args["parent_link_list"]))]
	# Check if TF exists and if it is fixed; we do this by comparing the TF from one of the collections to each other
	# I think it only makes sense to do this for the sensor dataset
	for parent_child_tuple in parent_child_list:
	tf_name = parent_child_tuple[0] + '-' + parent_child_tuple[1]
	selected_collection_quat = source_dataset["collections"][selected_collection_key]["transforms"][tf_name]["quat"]
	selected_collection_trans = source_dataset["collections"][selected_collection_key]["transforms"][tf_name]["trans"]
	print("Selected collection quat (" + tf_name + "):")
	print(selected_collection_quat)
	for collection_key, collection in source_dataset["collections"].items():
	if (selected_collection_quat == collection["transforms"][tf_name]["quat"] and selected_collection_trans == collection["transforms"][tf_name]["trans"]):
	continue
	else:
	atomError("Indicated transform " + tf_name + "is not fixed!")

Keep in mind this comes in the context of the copy_tfs_from_dataset.py script, where the child links and corresponding parent links are given as input arguments. (which are then reorganized into (parent, child) tuples for organization).

I can reorganize it into a function quickly if needed!

[miguelriemoliveira]

This is were I have done it before

atom/atom_calibration/src/atom_calibration/calibration/visualization.py

Line 496 in b1f0a42

if args['all_joints_fixed']: # assume the robot is static

[miguelriemoliveira]

I think all is done for now. I have use the dataset['transforms'] that exist in newer datasets.

Perhaps we will need to create a dataset migrate tool ... opened #973 for this.

[miguelriemoliveira]

Missing the creation of a transform_corrector.launch ... should be done in the configure_calibration_package.
Opened #974 for this.

[miguelriemoliveira]

Closing.