Multi template tracking features/add multi template tracking map

[dcolli23]

High-Level Description

Adds the ability for CDCPD to run on multiple deformable object configurations by storing a map of configurations by unique ID (ID unique to each deformable object configuration). This isn't particularly important now but opens the possibility for tracking multiple templates in the future.

This does NOT implement multiple template tracking in the sense of:

• Learning separate LLE weights for separate templates given different initial template states.
• Separating multiple deformable object configurations in the CPD step, meaning that the masked points that belong to another template still impact the CPD score of another, separate deformable object configuration.
• Using masked point association history to disambiguate multi-template (non-instance) segmentation, meaning that if CDCPD receives a mask indicating there are two deformable objects in the segmentation that are interacting, there's no way to tell them apart in the tracking's use of the segmented mask.

Key Changes

• Adds a std::map to keep track of deformable object configurations (how tracked templates are stored) by unique IDs.
• Changes max segment length to be per edge so that edges can have different maximum lengths.
  • Something of this flavor is necessary to specify max edge lengths based on each template's (potentially different) edge lengths compared to any other template's edge lengths.
  • Though, it's up for debate as to whether this is the best way to handle this.
• Updates the RVIZ edge visualization to only connect tracked points that belong to the same template.
  • Accomplishes this by publishing a MarkerArray message instead of a single Marker message.
  • TODO: Connect points based on the edge list. This would make the visualization respect the actual edges and thus correctly visualize cloth.

Miscellaneous Changes

• Formats CDCPD and Optimizer function signatures as they could have been considered hard to read.