The bookkeeping around matching: which k, which poses, which distances.
Mirrors tbp/monty/frameworks/utils/graph_matching_utils.py. The functions
fall into four groups — building a graph (how many neighbours are there to
have), seeding hypotheses (which object poses to try), scoring them (custom
distances, scaled evidence) and knowing when to give up (the LM has moved off
the object it was tracking).
Shapes
Hypotheses are rows: nearest_node_locations is one list of neighbour
locations per hypothesis, search_locations and search_normals one vector
per hypothesis. Results keep the hypotheses' order, so a distance can always
be read back as "this hypothesis, that neighbour".
Where the numbers come from
The uniform pose sampling, the dual-pose filter and the 512 → 256 reduction are Monty's, reproduced rather than improved: an experiment that samples a different 256 poses is a different experiment.
Summary
Functions
Makes every input channel tolerate pose vectors, in radians.
The neighbour count a graph can actually support.
The distance from one search point to one node, flattened along the normal.
Distances from each hypothesis' search point to its nearest graph nodes, flattened along the surface normal.
The step-to-step changes in evidence, with the rises clipped away, and where evidence last rose.
Whether the evidence has been falling for long enough to say the sensor has moved onto a different object.
Whether the last k evidence changes add up to a drop of at least
:detection_threshold.
Distances from each hypothesis' search point to its nearest graph nodes.
Weights that fall off exponentially into the past, one per step.
The step at which the sensor first spent n_steps_off_primary_target
consecutive steps off the primary target, or nil if it never did.
How far off its tangent plane a curved surface lets a search stray.
Whether every value lies in its own range.
The initial poses an experiment asks for.
The directions a node may be sensing from, one pose vector set per rotation.
How strongly the surface curves where the sensor is, as one number.
Evidence rescaled into [-1, 1] so that one module's longer history cannot
outvote another's.
A spread of object poses to try before anything is known.
The paths that are not a repeat of an earlier one.
Types
@type locations() :: [[float()]]
Functions
Makes every input channel tolerate pose vectors, in radians.
pose_vectors are degrees everywhere else in Monty's configs and radians
everywhere in its arithmetic, so this is the one place the conversion
happens. Channels that already declare a tolerance keep it; the rest get
default_tolerances degrees on all three vectors.
@spec correct_k_n(pos_integer(), non_neg_integer()) :: {:ok, pos_integer()} | {:error, :not_enough_observations}
The neighbour count a graph can actually support.
k is asked for as "edges I want", so the count includes the node itself and
comes back one larger. A graph with too few observations cannot honour it: it
falls back to "everything except me", and with two or fewer points there is
nothing to fall back to.
@spec custom_distance( Monty.Frameworks.Utils.SpatialArithmetics.vector(), Monty.Frameworks.Utils.SpatialArithmetics.vector(), Monty.Frameworks.Utils.SpatialArithmetics.vector(), number() ) :: float()
The distance from one search point to one node, flattened along the normal.
The three products subtract, magnitude and dot would make, without the
difference they would build in between: this runs once per neighbour of every
hypothesis, which at Monty's own hypothesis counts is millions of times a step.
@spec custom_distances( [locations()], [Monty.Frameworks.Utils.SpatialArithmetics.vector()], [Monty.Frameworks.Utils.SpatialArithmetics.vector()], number() ) :: [[float()]]
Distances from each hypothesis' search point to its nearest graph nodes, flattened along the surface normal.
A plain Euclidean distance treats the search sphere as a ball; the surface a
sensor is looking at is not one. Adding |dot(difference, normal)| penalises
candidates that sit off the sensed tangent plane, and dividing by
|curvature| + 0.5 lets a curved surface search further off that plane than
a flat one — while the 0.5 keeps a flat surface from collapsing the sphere to
a disc (and the division from being by zero).
This is for a channel whose pose is a surface pose — a normal and two
curvature directions. A channel whose pose is an object orientation has no
tangent plane to flatten against, and takes euclidean_distances/2 instead
(Monty's #1266).
@spec delta_evidence_values([number()]) :: {[float()], non_neg_integer() | nil}
The step-to-step changes in evidence, with the rises clipped away, and where evidence last rose.
Only drops suggest movement onto a new object, so the rises are clipped to zero; the position of the last rise is returned so the caller can decide whether to count from there.
Whether the evidence has been falling for long enough to say the sensor has moved onto a different object.
Each step's drop is weighted by how recent it is, so a sharp fall this step
counts for more than a slow decline over many; the sum of those weighted
drops is compared with :detection_threshold.
Whether the last k evidence changes add up to a drop of at least
:detection_threshold.
:reset_at_positive_jump counts only the steps since evidence last rose:
a module that just got more confident is not on its way off the object, no
matter what it did before.
Distances from each hypothesis' search point to its nearest graph nodes.
Monty's euclidean_distances: the plain distance, for a channel whose pose is
an object orientation rather than a surface. There is no sensed normal to
flatten against, so the surface version's penalty has nothing to act on.
@spec exponential_kernel(pos_integer(), number()) :: [float()]
Weights that fall off exponentially into the past, one per step.
The most recent step is weighted 1.0.
@spec find_step_on_new_object([String.t()], String.t(), pos_integer()) :: non_neg_integer() | nil
The step at which the sensor first spent n_steps_off_primary_target
consecutive steps off the primary target, or nil if it never did.
A single step elsewhere is not "on a new object" — the sensor may simply have looked past an edge — so the run of steps is what counts.
How far off its tangent plane a curved surface lets a search stray.
1 / (|curvature| + @minimum_curvature): a curved surface searches further off
the plane than a flat one, and the minimum keeps a flat surface from collapsing
the sphere to a disc (and the division from being by zero). One number per
channel, handed to custom_distance/4 for every neighbour of every hypothesis.
Whether every value lies in its own range.
A range whose minimum exceeds its maximum wraps around: {350, 20} covers
everything except the 20-odd degrees in between, which is how a circular
feature like an orientation is tolerated.
@spec initial_possible_poses( :uniform | :informed | {:euler_degrees, [Monty.Frameworks.Utils.SpatialArithmetics.vector()]} ) :: {:ok, [Monty.Geometry.Rotation.t()] | :first_observation}
The initial poses an experiment asks for.
:uniform samples the whole rotation space, :informed starts from the first
observation instead (there is nothing to assume yet), and a list of Euler
poses means "try exactly these" — inverted, because they describe how the
object sits rather than how it must be turned.
@spec possible_sensed_directions(Monty.Geometry.SurfacePoseVectors.t(), pos_integer()) :: [locations()]
The directions a node may be sensing from, one pose vector set per rotation.
With num_hyps_per_node == 2 the pose is well defined (the two principal
curvatures differ), and a well-defined pose still cannot tell an object from
its upside-down twin: the curvature directions may point either way, so both
are offered. Otherwise the directions are sampled around the normal.
How strongly the surface curves where the sensor is, as one number.
The maximum absolute principal curvature, or a plain curvature value, or — when the sensor reports none — a large default, which widens the search sphere until it is nearly circular. Searching a wide sphere is slow but finds the object; searching a tight one around a curvature that was never measured finds nothing.
@spec scaled_evidences(%{optional(String.t()) => [number()]}, keyword()) :: %{ optional(String.t()) => [float()] }
Evidence rescaled into [-1, 1] so that one module's longer history cannot
outvote another's.
By default the scale is shared: the widest and narrowest evidence over all objects set the range, so the objects stay comparable. An evidence set whose largest value is below 1 is not rescaled at all — it is only clipped, because stretching it would inflate evidence that was never strong.
per_object: true scales each object against itself. Monty notes this can
hand a weakly supported object too much of a boost; it is kept for the
experiments that ask for it.
When every evidence is equal there is no scale to speak of: Monty's division yields NaN there, and zeros here, which vote neither way.
@spec uniform_initial_possible_poses(pos_integer()) :: [Monty.Geometry.Rotation.t()]
A spread of object poses to try before anything is known.
Each axis is sampled at n_degrees_sampled angles; every combination is
generated in lexicographic order, then halved: a rotation and its dual
(a + 180, -b + 180, c + 180) describe the same pose, so only the first of
each pair is kept. Nine samples give 512 combinations and 256 poses, which is
what a uniform experiment means by "tried every rotation".
The paths that are not a repeat of an earlier one.
Two paths are the same visit if the points line up: the summed distance
between corresponding points is below threshold. Summing rather than taking
the maximum is Monty's choice, and it means two paths that both stay close to
a longer path are the same path.