tbp.monty.frameworks.utils

tbp.monty.frameworks.utils.communication_utils

get_first_sensory_state(states)

Given a list of states return the first one from a sensory channel.

Returns:

First state from a sensory channel.

get_state_from_channel(states, channel_name)

Given a list of states, return the state of the specified channel.

Parameters:

• states – List of states

• channel_name – The name of the channel to return the state for

Returns:

The state of the specified channel

Raises:

ValueError – If the channel name is not found in the states

tbp.monty.frameworks.utils.dataclass_utils

as_dataclass_dict(obj)

Convert a dataclass instance to a serializable dataclass dict.

Parameters:

obj – The dataclass instance to convert

Returns:

A dictionary with the dataclass fields and values

Raises:

TypeError – If the object is not a dataclass instance

create_dataclass_args(dataclass_name: str, function: Callable, base: Type | None = None)

Creates configuration dataclass args from a given function arguments.

When the function arguments have type annotations these annotations will be passed to the dataclass fields, otherwise the type will be inferred from the argument default value, if any.

For example:

SingleSensorAgentArgs = create_dataclass_args(
        "SingleSensorAgentArgs", SingleSensorAgent.__init__)

# Is equivalent to
@dataclass(frozen=True)
class SingleSensorAgentArgs:
    agent_id: str
    sensor_id: str
    position: Tuple[float. float, float] = (0.0, 1.5, 0.0)
    rotation: Tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0)
    height: float = 0.0
    :

Parameters:

• dataclass_name – The name of the new dataclass

• function – The function used to extract the parameters for the dataclass

• base – Optional base class for newly created dataclass

Returns:

New dataclass with fields defined by the function arguments.

from_dataclass_dict(datadict)

Convert a serializable dataclass dict back into the original dataclass.

Expecting that the serializable dataclass dict was created via asdict().

Parameters:

datadict – The serializable dataclass dict to convert

Returns:

The original dataclass instance

Raises:

TypeError – If the object is not a dict instance

tbp.monty.frameworks.utils.follow_up_configs

tbp.monty.frameworks.utils.graph_matching_utils

add_pose_features_to_tolerances(tolerances, default_tolerances=20)

Add point_normal and curvature_direction default tolerances if not set.

Returns:

Tolerances dictionary with added pose_vectors if not set.

create_exponential_kernel(size, decay_rate)

Create an exponentially decaying kernel.

Used to convolve e.g. evidence history when determining whether we are on a new object.

Parameters:

• size – Size of the kernel.

• decay_rate – Decay rate of the kernel.

Returns:

Exponentially decaying kernel.

detect_new_object_exponential(max_ev_per_step, detection_threshold=-1.0, decay_rate=3)

Detect we’re on a new object using exponentially decaying evidence changes.

Evidence changes from multiple steps into the past are convolved by exponentially decaying constants, such that more recent steps carry more significance.

Parameters:

• max_ev_per_step – List of the maximum evidence (across all locations/poses) for the current MLH object, across all steps of the current episode

• detection_threshold – The total amount of negative evidence in the counter/sum that needs to be exceeded to determine that the LM has moved on to a new object

• decay_rate – The rate of decay that determines how much past evidence -drops contribute to the current estimate of change

Returns:

True if the total amount of negative evidence is less than or equal to the detection threshold, False otherwise.

detect_new_object_k_steps(max_ev_per_step, detection_threshold=-1.0, k=3, reset_at_positive_jump=False)

Detect we’re on a new object using the evidence changes from multiple steps.

Evidence changes from multiple steps into the past are considered. We look at the change in evidence over k discrete steps, weighing these equally.

Parameters:

• max_ev_per_step – List of the maximum evidence (across all locations/poses) for the current MLH object, across all steps of the current episode

• detection_threshold – The total amount of negative evidence in the counter/sum that needs to be exceeded to determine that the LM has moved on to a new object

• k – How many steps into the past to look when summing the negative change in evidence

• reset_at_positive_jump – Boolean to “reset” the accumulated changes when there is a positive increase in evidence, i.e. k is further limited by this history

Returns:

True if the total evidence change is less than or equal to the detection threshold, False otherwise.

find_step_on_new_object(stepwise_targets, primary_target, n_steps_off_primary_target)

Returns the episode step at which we’ve moved off the primary target object.

The returned episode step is the first step at which we’ve been off the primary target for a total of n_steps_off_primary_target consecutive steps.

get_correct_k_n(k_n, num_datapoints)

Determine k_n given the number of datapoints.

k_n specified in hyperparameter may not be possible to achive with the number of data points collected. This function checks for that and adjusts k_n.

Parameters:

• k_n – current number k neareast neighbors specified for graph building

• num_datapoints – number of observations available to build the graph.

Returns:

adjusted k_n

get_custom_distances(nearest_node_locs, search_locs, search_pns, search_curvature)

Calculate custom distances modulated by point normal and curvature.

Parameters:

• nearest_node_locs – locations of nearest nodes to search_locs. shape=(num_hyp, max_nneighbors, 3)

• search_locs – search locations for each hypothesis. shape=(num_hyp, 3)

• search_pns – sensed point normal rotated by hypothesis pose. shape=(num_hyp, 3)

• search_curvature – magnitude of sensed curvature (maximum if using two principal curvatures). Is used to modulate the search spheres thickness in the direction of the point normal. shape=1

Returns:

custom distances of each nearest location

from its search location taking into account the hypothesis point normal and sensed curvature. shape=(num_hyp, max_nneighbors)

Return type:

custom_nearest_node_dists

get_initial_possible_poses(initial_possible_pose_type)

Initialize initial_possible_poses to test based on initial_possible_pose_type.

Parameters:

initial_possible_pose_type –

How to sample initial possible poses. Options are: - “uniform”: Sample uniformly from the space of possible poses. - “informed”: Sample poses that are likely to be possible based on

the object’s geometry and the first observation.

• list of euler angles: Use a list of predefiende poses to test (useful for

  debugging).

Returns:

List of initial possible poses to test.

get_relevant_curvature(features)

Get the relevant curvature from features. Used to scale search sphere.

In the case of principal_curvatures and principal_curvatures_log we use the maximum absolute curvature between the two values. Otherwise we just return the curvature value.

Note

Not sure if other curvatures work as well as log curvatures since they may have too big of a range.

Returns:

Magnitude of sensed curvature (maximum if using two principal curvatures).

get_scaled_evidences(evidences, per_object=False)

Scale evidences to be in range [-1, 1] for voting.

This is useful so that the absolute evidence values don’t distort the votes (for example if one LM has already had a lot more matching steps than another and the evidence is not bounded). It is also useful to keep the evidence added from a single voting step in a reasonable range.

By default we normalize using the maximum and minimum evidence over all objects. There is also an option to scale the evidence for each object independently but that might give low evidence objects too much of a boost. We could probably remove this option.

Returns:

Scaled evidences.

get_uniform_initial_possible_poses(n_degrees_sampled=9)

Get initial list of possible poses.

Parameters:

n_degrees_sampled – Number of degrees sampled for each axis. Default = 9 Which means tested degrees are in [ 0., 45., 90., 135., 180., 225., 270., 315.] This results in 512 unique pose combinations.

Of those, depending on the displacement vector, some are equivalent eg. [0,0,0] and [180,180,180] or [ 0, 45, 90] and [180, 135, 270] (a, b, c) == (a + 180, -b + 180, c + 180) (see https://books.google.gr/books?id=rn3OBQAAQBAJ p.267)

Returns:

List of poses to test.

get_unique_paths(possible_paths, threshold=0.01)

Get all unique paths in a list of possible paths.

Parameters:

• possible_paths – List of possible paths (locations)

• threshold – minimun distance between two parts to be considered different. defaults to 0.01

Returns:

List of unique paths (locations)

is_in_ranges(array, ranges)

Check for each element in an array whether it is in its specified range.

Each element can have a different tolerance range.

Returns:

True if all elements are in their respective ranges, False otherwise.

process_delta_evidence_values(max_ev_per_step)

Pre-process the max evidence values to get the change in evidence across steps.

Clip the values to be less than or equal to 0.

Also returns the index of the most recent positive change in evidence

Returns:

Clipped change in evidence across steps. postive_jump_loc: Index of the most recent positive change in evidence.

Return type:

clipped_ev_changes

tbp.monty.frameworks.utils.logging_utils

add_evidence_lm_episode_stats(lm, stats)

add_policy_episode_stats(lm, stats)

add_pose_lm_episode_stats(lm, stats)

Add possible poses of lm to episode stats.

Parameters:

• lm – LM istance from which to add the statistics.

• stats – Statistics dictionary to update.

Returns:

Updated stats dictionary.

calculate_fpr(fp, tn)

Calculate False Positive Rate, aka specificity.

Parameters:

• fp – false positives

• tn – true negatives

Returns:

False Positive Rate

calculate_performance(stats, performance_type, lm, target_object)

Calculate performance of an LM on a given target object.

Parameters:

• stats – Statistics dictionary to update.

• performance_type – performance type index into stats

• lm – Learning module for which to generate stats.

• target_object – target (primary or stepwise) object for the LM to have converged to

Returns:

Updated stats dictionary.

calculate_tpr(tp, fn)

Calculate True Positive Rate, aka sensitivity.

Parameters:

• tp – true positives

• fn – false negatives

Returns:

True Positive Rate

check_detection_accuracy_at_step(stats, last_n_step=1)

check_rotation_accuracy(stats, last_n_step=1)

compute_unsupervised_stats(possible_matches, target, graph_to_target, target_to_graph)

Compute statistics like how many graphs are built per object.

Parameters:

• possible_matches – container of str that key into graph_to_target

• target – str ground truth name of object being presented

• graph_to_target – dict mapping each graph to the set of objects used to build

• target_to_graph – dict mapping each object to the set of graphs that used it

Returns:

?

Return type:

dict

deserialize_json_chunks(json_file, start=0, stop=None, episodes=None)

Deserialize one episode at a time from json file.

Only get episodes specified by arguments, which follow list / numpy like semantics.

Note

assumes line counter is exactly in line with episode keys

Parameters:

• json_file – full path to the json file to load

• start – int, get data starting at this episode

• stop – int, get data ending at this episode, not inclussive as usual in python

• episodes – iterable of ints with episodes to pull

Returns:

dict containing contents of file_handle

Return type:

detailed_json

format_columns_for_wandb(lm_dict)

Various columns break wandb because we are playing fast and loose with types.

Put any standardizations here.

Parameters:

lm_dict – dict, part of a larger dict ~ {LM_0: lm_dict, LM_1: lm_dict}

Returns:

formatted lm_dict

Return type:

dict

get_graph_lm_episode_stats(lm)

Populate stats dictionary for one episode for a lm.

Parameters:

lm – Learning module for which to generate stats.

Returns:

dict with stats of one episode.

get_object_graph_stats(graph_to_target, target_to_graph)

get_overall_pose_error(stats, lm_id='LM_0')

Get mean pose error over all episodes.

Note

This can now be obtained easier from the .csv stats.

Parameters:

• stats (dict) – detailed stats

• lm_id – id of learning module

Returns:

mean pose error

get_pose_error(detected_pose, target_pose)

get_reverse_rotation(rotation)

get_rgba_frames_single_sm(observations)

Convert a time series of rgba observations into format for wandb.Video.

Parameters:

observations – episode_stats[sm][___observations]

Returns:

formatted observations

get_stats_per_lm(model, target)

Loop through lms and get stats.

Parameters:

• model – model instance

• target – target object

Returns:

dict with stats per lm

Return type:

performance_dict

get_time_stats(all_ds, all_conditions)

Get summary of run times in a dataframe for each condition.

Parameters:

• all_ds – detailed stats (dict) for each condition

• all_conditions – name of each condition

Returns:

pd.DataFrame with runtime stats

get_unique_euler_poses(poses)

Get unique poses for an object from possible poses per path.

Returns:

array of unique poses

Return type:

unique_poses

lm_stats_to_dataframe(stats, format_for_wandb=False)

Take in a dictionary and format into a dataframe.

Example:

{0: {LM_0: stats, LM_1: stats...}, 1:...} --> dataframe

Currently we are reporting once per episode, so the loop over episodes is only over a singel key, value pair, but leaving it here because it is backward compatible.

Returns:

dataframe

load_models_from_dir(exp_path, pretrained_dict=None)

load_stats(exp_path, load_train=True, load_eval=True, load_detailed=True, load_models=True, pretrained_dict=None)

Load experiment statistics from an experiment for analysis.

Returns:

pandas DataFrame with training statistics eval_stats: pandas DataFrame with evaluation statistics detailed_stats: dict with detailed statistics lm_models: dict with loaded language models

Return type:

train_stats

matches_to_target_str(possible_matches, graph_to_target)

Get the possible target objects associated with each possible match.

Targets are concatenated into a single string name for easy saving in a csv.

Returns:

?

Return type:

dict

maybe_rename_existing_directory(path, report_count)

maybe_rename_existing_file(log_file, extension, report_count)

Check if this run has already been executed.

If so, change name of existing log file by appending _old to it.

Parameters:

• log_file – full path to the file, e.g. ~/…/detailed_run_stats.json

• extension – str name of file type

• report_count –

  ?

print_overall_stats(stats)

print_unsupervised_stats(stats, epoch_len)

Print stats of unsupervised learning experiment.

target_data_to_dict(target)

Format target params to dict.

Parameters:

target – target params

Returns:

dict with target params

Return type:

output_dict

total_size(o)

Returns the approximate memory footprint an object and all of its contents.

Automatically finds the contents of the following builtin containers and their subclasses: tuple, list, deque, dict, set and frozenset. To search other containers, add handlers to iterate over their contents:

handlers = {SomeContainerClass: iter,

OtherContainerClass: OtherContainerClass.get_elements}

The recursive recipe universally cited on stack exchange and blogs for gauging the size of python objets in memory.

See also

https://code.activestate.com/recipes/577504/

tbp.monty.frameworks.utils.object_model_utils

class NumpyGraph(my_dict)

Bases: object

Alternative way to represent graphs without using torch.

Speeds up runtime significantly.

already_in_list(existing_points, new_point, features, clean_ids, query_id, graph_delta_thresholds)

Check if a given point is already in a list of points.

Parameters:

• existing_points – List of x,y,z locations

• new_point – new location

• features – all features (both existing and candidate points)

• clean_ids – indices (w.r.t “features”) that have been accepted into the graph and are compared to

• query_id – index (w.r.t “features”) that is currently being considered

• graph_delta_thresholds – Dictionary of thresholds used to determine whether a point should be considered sufficiently different so as to be included in the graph

Returns:

Whether the point is already in the list

Return type:

bool

build_point_cloud_graph(locations, features, feature_mapping)

Build a graph from observations without edges.

Parameters:

• locations – array of x, y, z positions in space

• features – dictionary of features at locations

• feature_mapping –

  ?

Returns:

A NumpyGraph containing the observed features at locations.

circular_mean(values)

Calculate the mean of a circular value such as hue where 0==1.

Returns:

Mean value.

expand_index_dims(indices_3d, last_dim_size)

Expand 3d indices to 4d indices by adding a 4th dimension with size.

Parameters:

• indices_3d – 3d indices that should be comverted to 4d

• last_dim_size – desired size of the 4th dimension (will be filled with arange indices from 0 to last_dim_size-1)

Returns:

Tensor of 4d indices.

get_cubic_patches(arr_shape, centers, size)

Cut a cubic patch around a center id out of a 3d array.

NOTE: Currently not used. Was implemented for draft of nn search in grid.

Returns:

New centers and mask.

get_most_common_bool(booleans)

Get most common value out of a list of boolean values.

Returns:

True when we have equally many True as False entries.

get_values_from_dense_last_dim(tensor, index_3d)

Get values from 4d tensor at indices in last dimension.

This function assumes that the entries in the last dimension are dense. This is the case in all our sparse tensors where the first 3 dimensions represent the 3d location (sparse) and the 4th represents values at this location (dense).

Returns:

List of values.

increment_sparse_tensor_by_count(old_tensor, indices)

pose_vector_mean(pose_vecs, pose_fully_defined)

Calculate mean of pose vectors.

This takes into account that point normals may contain observations from two surface sides and curvature directions have an ambiguous direction. It also enforces them to stay orthogonal.

If not pose_fully_defined, the curvature directions are meaningless and we just return the first observation. Theoretically this shouldn’t matter but it can save some computation time.

Returns:

?

remove_close_points(point_cloud, features, graph_delta_thresholds, old_graph_index)

Remove points from a point cloud unless sufficiently far away.

Points are removed unless sufficiently far away either by Euclidean distance, or feature-space.

Parameters:

• point_cloud – List of 3D points

• features –

  ?

• graph_delta_thresholds – dictionary of thresholds; if the L-2 distance between the locations of two observations (or other feature-distance measure) is below all of the given thresholds, then a point will be considered insufficiently interesting to be added

• old_graph_index – If the graph is not new, the index associated with the final point in the old graph; we will skip this when checking for sameness, as they will already have been compared in the past to one-another, saving computation.

Returns:

List of 3D points that are sufficiently novel w.r.t one-another, along with their associated indices.

torch_graph_to_numpy(torch_graph)

Turn torch geometric data structure into dict with numpy arrays.

Parameters:

torch_graph – Torch geometric data structure.

Returns:

NumpyGraph.

tbp.monty.frameworks.utils.plot_utils

tbp.monty.frameworks.utils.profile_utils

bar_chart_cumtime(df, n_functions=None)

bar_chart_tottime(df, n_functions=None)

drop_filename(string)

Drop filename for shorter strings and easier viz.

We do this because strings for code calls are long.

Returns:

String without filename.

get_data_from_df(df, sortby='cumtime')

get_total_time(df)

linebreak_long_strings(string, chars_per_line=40)

Strings with filename are long, try to get them more readable in bar plots.

Parameters:

• string – String to format.

• chars_per_line – Number of characters per line. Defaults to 40.

Returns:

Formatted string.

print_top_k_functions(func_names, k=20)

sort_by_cumtime(df)

sort_by_tottime(df)

tbp.monty.frameworks.utils.sensor_processing

get_center_neighbors(point_cloud, center_id, neighbor_patch_frac)

Get neighbors within a given neighborhood of the patch center.

Returns:

Locations and semantic id of all points within a given neighborhood of the patch center which lie on an object.

get_curvature_at_point(point_cloud, center_id, normal)

Compute principal curvatures from point cloud.

Computes the two principal curvatures of a 2D surface and corresponding principal directions

Parameters:

• point_cloud – point cloud (2d numpy array) based on which the 2D surface is approximated

• center_id – center point around which the local curvature is estimated

• normal – surface normal at the center point

Returns:

first principal curvature k2: second principal curvature dir1: first principal direction dir2: second principal direction

Return type:

k1

get_pixel_dist_to_center(n_points, patch_width, center_id)

Extracts the relative distance of each pixel to patch center (in pixel space).

Returns:

Relative distance of each pixel to patch center (in pixel space)

get_point_normal_naive(point_cloud, patch_radius_frac=2.5)

Estimate point normal.

This is a very simplified alternative to open3d’s estimate_normals where we make use of several assumptions specific to our case: - we know which locations are neighboring locations from the camera patch

arrangement

• we only need the point normal at the center of the patch

TODO: Calculate point normal from multiple points at different distances (tan_len

values) and then take the average of them. Test if this improves robustness to raw sensor noise.

Parameters:

• point_cloud – list of 3d coordinates and whether the points are on the object or not. shape = [n, 4]

• patch_radius_frac – Fraction of observation size to use for PN calculation. Default of 2.5 means that we look half_obs_dim//2.5 to the left, right, up and down. With a resolution of 64x64 that would be 12 pixels. The calculated tan_len (in this example 12) describes the distance of pixels used to span up the two tangent vectors to calculate the point normals. These two vectors are then used to calculate the point normal by taking the cross product. If we set tan_len to a larger value the point normal is more influenced by the global shape of the patch.

Returns:

Estimated point normal at center of patch valid_pn: Boolean for whether the point-normal was valid or not (True by

default); an invalid point-normal means there were not enough points in the patch to make any estimate of the point-normal

Return type:

norm

get_point_normal_ordinary_least_squares(sensor_frame_data, world_camera, center_id, neighbor_patch_frac=3.2)

Extracts the point-normal direction from a noisy point-cloud.

Uses ordinary least-square fitting with error minimization along the view direction.

Parameters:

• sensor_frame_data – point-cloud in sensor coordinates (assumes full patch is provided i.e. no preliminary filtering of off-object points).

• world_camera – matrix defining sensor-to-world frame transformation.

• center_id – id of the center point in point_cloud.

• neighbor_patch_frac – fraction of the patch width that defines the local neighborhood within which to perform the least-squares fitting.

Returns:

Estimated point normal at center of patch valid_pn: Boolean for whether the point-normal was valid or not. Defaults

to True. An invalid point-normal means there were not enough points in the patch to make any estimate of the point-normal

Return type:

point_normal

get_point_normal_total_least_squares(point_cloud_base, center_id, view_dir, neighbor_patch_frac=3.2)

Extracts the point-normal direction from a noisy point-cloud.

Uses total least-square fitting. Error minimization is independent of view direction.

Parameters:

• point_cloud_base – point-cloud in world coordinates (assumes full patch is provided i.e. no preliminary filtering of off-object points).

• center_id – id of the center point in point_cloud.

• view_dir – viewing direction used to adjust the sign of the estimated point-normal.

• neighbor_patch_frac – fraction of the patch width that defines the local neighborhood within which to perform the least-squares fitting.

Returns:

Estimated point normal at center of patch valid_pn: Boolean for whether the point-normal was valid or not. Defaults

to True. An invalid point-normal means there were not enough points in the patch to make any estimate of the point-normal

Return type:

norm

get_principal_curvatures(point_cloud_base, center_id, n_dir, neighbor_patch_frac=2.13, weighted=True, fit_intercept=True)

Compute principal curvatures from point cloud.

Computes the two principal curvatures of a 2D surface and corresponding principal directions

Parameters:

• point_cloud_base – point cloud (2d numpy array) based on which the 2D surface is approximated

• center_id – center point around which the local curvature is estimated

• n_dir – surface normal at the center point

• neighbor_patch_frac – fraction of the patch width that defines the std of the gaussian distribution used to sample the weights. Defines a local neighborhood for principal curvature computation.

• weighted – boolean flag that determines if regression is weighted or not. Weighting scheme is defined in get_weight_matrix.

• fit_intercept – boolean flag that determines whether to fit an intercept term for the regression.

Returns:

first principal curvature k2: second principal curvature dir1: first principal direction dir2: second principal direction

Return type:

k1

get_weight_matrix(n_points, center_id, neighbor_patch_frac=2.13)

Extracts individual pixel weights for least-squares fitting.

Weight for each pixel is sampled from a gaussian distribution based on its distance to the patch center.

Parameters:

• n_points – total number of points in the full RGB-D square patch.

• center_id – id of the center point in point_cloud.

• neighbor_patch_frac – fraction of the patch width that defines the std of the gaussian distribution used to sample the weights.

Returns:

w_diag

log_sign(to_scale)

Apply symlog to input array, preserving sign.

This implementation makes sure to preserve the sign of the input values and to avoid extreme outputs when values are close to 0.

Parameters:

to_scale – array to scale.

Returns:

Scaled values of array.

point_pair_features(pos_i, pos_j, normal_i, normal_j)

Get point pair features between two points.

Parameters:

• pos_i – Location of point 1

• pos_j – Location of point 2

• normal_i – Point normal of point 1

• normal_j – Point normal of point 2

Returns:

Point pair feature

scale_clip(to_scale, clip)

Clip values into range and scale with sqrt.

This can be used to get gaussian and mean curvatures into a reasonable range and remove outliers. Makes it easier to deal with noise. Preserves sign before applying sqrt.

Parameters:

• to_scale – array where each element should be scaled.

• clip – range to which the array values should be clipped.

Returns:

scaled values of array.

tbp.monty.frameworks.utils.spatial_arithmetics

align_multiple_orthonormal_vectors(ms1, ms2, as_scipy=True)

Calculate rotations between multiple orthonormal vector sets.

Parameters:

• ms1 – multiple orthonormal vectors. shape = (N, 3, 3)

• ms2 – orthonormal vectors to align with. shape = (3, 3)

• as_scipy – Whether to return a list of N scipy.Rotation objects or a np.array of rotation matrices (N, 3, 3).

Returns:

List of N Rotations that align ms2 with each element in ms1.

align_orthonormal_vectors(m1, m2, as_scipy=True)

Calculate the rotation that aligns two sets of orthonormal vectors.

Parameters:

• m1 – First set of orthonormal vectors.

• m2 – Second set of orthonormal vectors to align with.

• as_scipy – Whether to return a scipy rotation object or a rotation matrix. Defaults to True.

Returns:

?

apply_rf_transform_to_points(locations, features, location_rel_model, object_location_rel_body, object_rotation, object_scale=1)

Apply location and rotation transform to locations and features.

These transforms tell us how to transform new observations into the existing model reference frame. They are calculated from the detected object pose.

Parameters:

• locations – Locations to transform (in body reference frame). Shape (N, 3)

• features – Features to transform (in body reference frame). Shape (N, F)

• location_rel_model – Detected location of the sensor on the object (object reference frame).

• object_location_rel_body – Location of the sensor in the body reference frame.

• object_rotation – Rotation of the object in the world relative to the learned model of the object. Expresses how the object model needs to be rotated to be consistent with the observations. To transfor the observed locations (rel. body) into the models reference frame, the inverse of this rotation is applied.

• object_scale – Scale of the object relative to the model. Not used yet.

Note

Function can also be used in different contexts besides transforming points from body to object centric reference frame.

Returns:

Transformed locations features: Transformed features

Return type:

transformed_locations

check_orthonormal(matrix)

euler_to_quats(euler_rots, invert=False)

Convert euler rotations to rotation matrices.

Parameters:

• euler_rots – Euler rotations

• invert – Whether to invert the rotation. Defaults to False.

Returns:

Quaternions

get_angle(vec1, vec2)

Get angle between two vectors.

NOTE: for efficiency reasons we assume vec1 and vec2 are already normalized (which is the case for point normals and curvature directions).

Parameters:

• vec1 – Vector 1

• vec2 – Vector 2

Returns:

angle in radians

get_angle_beefed_up(v1, v2)

Returns the angle in radians between vectors ‘v1’ and ‘v2’.

If one of the vectors is undefined, return arbitrarily large distance

If one of the vectors is the zero vector, return arbitrarily large distance

Also enforces that vectors are unit vectors (therefore less efficient than the standard get_angle)

>>> angle_between_vecs((1, 0, 0), (0, 1, 0))
1.5707963267948966
>>> angle_between_vecs((1, 0, 0), (1, 0, 0))
0.0
>>> angle_between_vecs((1, 0, 0), (-1, 0, 0))
3.141592653589793

get_angle_torch(v1, v2)

Get angle between two torch vectors.

Parameters:

• v1 – Vector 1

• v2 – Vector 2

Returns:

angle in radians

get_angles_for_all_hypotheses(hyp_f, query_f)

Get all angles for hypotheses and their neighbors at once.

hyp_f shape = (num_hyp, num_nn, 3) query_f shape = (num_hyp, 3)

for each hypothesis we want to get num_nn angles.

return shape = (num_hyp, num_nn)

Parameters:

• hyp_f (num_hyp, num_nn, 3) –

  ?

• query_f (num_hyp, 3) –

  ?

Returns:

?

get_more_directions_in_plane(vecs, n_poses)

Get a list of unit vectors, evenly spaced in a plane orthogonal to vecs[0].

This is used to sample possible poses orthogonal to the point normal when the curvature directions are undefined (like on a flat surface).

Parameters:

• vecs – Vector to get more directions in plane for

• n_poses – Number of poses to get

Returns:

List of vectors evenly spaced in a plane orthogonal to vecs[0]

Return type:

list

get_right_hand_angle(v1, v2, pn)

get_unique_rotations(poses, similarity_th, get_reverse_r=True)

Get unique scipy.Rotations out of a list, given a similarity threshold.

Parameters:

• poses – List of poses to get unique rotations from

• similarity_th – Similarity threshold

• get_reverse_r – Whether to get the reverse rotation. Defaults to True.

Returns:

Unique euler poses r_poses: Unique rotations corresponding to euler_poses

Return type:

euler_poses

non_singular_mat(a)

Return True if a matrix is non-singular, i.e. can be inverted.

Uses the condition number of the matrix, which will approach a very large value, given by (1 / sys.float_info.epsilon) (where epsilon is the smallest possible floating-point difference)

pose_is_new(all_poses, new_pose, similarity_th)

Check if a pose is different from a list of poses.

Use the magnitude of the difference between quaternions as a measure for similarity and check that it is below pose_similarity_threshold.

Returns:

True if the pose is new, False otherwise

Return type:

bool

rot_mats_to_quats(rot_mats, invert=False)

Convert rotation matrices to quaternions.

Parameters:

• rot_mats – Rotation matrices

• invert – Whether to invert the rotation. Defaults to False.

Returns:

Quaternions

rotate_multiple_pose_dependent_features(features, ref_frame_rot)

Rotate point normal and curv dirs given a rotation matrix.

Parameters:

• features – dict of features with pose vectors to rotate. Pose vectors have shape (N, 9)

• ref_frame_rot – scipy rotation to rotate pose vectors with.

Returns:

Features with rotated pose vectors

Return type:

dict

rotate_pose_dependent_features(features, ref_frame_rots)

Rotate pose_vectors given a list of rotation matrices.

Parameters:

• features – dict of features with pose vectors to rotate. pose vectors have shape (3, 3)

• ref_frame_rots – Rotation matrices to rotate pose features by. Can either be - A single scipy rotation (as used in FeatureGraphLM) - An array of rotation matrices of shape (N, 3, 3) or (3, 3) (as used in EvidenceGraphLM).

Returns:

Original features but with the pose_vectors rotated. If multiple

rotations were given, pose_vectors entry will now contain multiple entries of shape (N, 3, 3).

Return type:

dict

rotations_to_quats(rotations, invert=False)

tbp.monty.frameworks.utils.transform_utils