Semantic map fuses multiple instances of the same label into one incorrect pose

[adharshvenkat]

Problem

SemanticMap (src/natural_nav/natural_nav/semantic_map.py) keys observations purely by label string and returns a confidence-weighted mean across all observations for that label. When two physical instances of the same class are perceived (e.g. two doors), their positions average into a single pose that may not correspond to either instance.

Evidence

Observed during GPU validation: a door entry's pose jumped from (-7.69, 0.20) to (-4.17, 3.90) (~15m) the moment a second door came into view, since LabelEntry.pose() averages all buffered observations regardless of spatial proximity.

Impact

• Task execution can navigate to a meaningless midpoint between two objects instead of either one.
• Blocks any scene with more than one instance of a labeled class, which is the common case in a warehouse (multiple pallets, doors, boxes).
• Undermines the planned RViz visualization: a single averaged marker for two doors renders in empty space.

Proposed fix

Cluster observations spatially before fusing (e.g. distance-threshold clustering on x,y), keep one LabelEntry per spatial cluster, and expose instances as door_1, door_2, etc. Reject observations that fall far outside an existing cluster's spread as outliers rather than folding them into the running mean.

Acceptance criteria

• Two instances of the same label, observed from different angles, resolve to two distinct map entries with sane individual poses.
• A single stray/erroneous observation does not measurably shift an established cluster's pose.
• Existing test_projection.py suite and any new test_semantic_map.py pass.