Navigation Next Steps (Semantic Scene Graph and Room segmentation)

Navigation Spec: Semantic Scene Graph + Room Segmentation

Purpose

Define the navigation-relevant semantic scene graph and the room segmentation
pipeline that produces room IDs, door connectivity, and room nodes. This spec is
intentionally limited to:

• Semantic scene graph (rooms, viewpoints, objects, door adjacency).
• Room segmentation inputs/outputs and how they update the graph.
• Two example instruction flows that use the graph for door-based navigation.

Non-Goals

• VLM prompt design, query queues, or response parsing.
• Object mapping internals (voxel fusion, detection).
• Global/local path planning details beyond door-to-door transitions.

Components

1) Room Segmentation Node

Room segmentation runs as a standalone node in the stack.

Inputs

• Registered LiDAR scans
• State estimation / odometry
• Occupied-space point cloud
• Free-space point cloud

Outputs

• Room mask grid (room IDs).
• Room nodes list (polygon, centroid, neighbors, area, mask crop).
• Door candidate points tagged with adjacent room IDs.
• Current room boundary polygon and room boundary visualization.
• Debug and map clouds (optional).

Key Parameters

• room_resolution, room_x, room_y, room_z
• rolling_occupancy_grid.resolution_x
• ceilingHeight_, wall_thres_height_
• region_growing_radius, min_room_size
• normal_search_num, normal_search_radius

Behavior

• Maintains a voxel grid in map frame, builds a 2D room mask.
• Produces room node entries with polygon, centroid, neighbors, and cropped
  room mask.
• Emits door points where each point encodes two adjacent rooms (room IDs are
  1-based). Door points with the navigable label are used for transitions.

2) Semantic Scene Graph (Representation)

Graph owner: the planner’s representation layer.

Node types

• Room node
  • ID, polygon, centroid, neighbors, area
  • label votes, anchor point, room mask crop
  • viewpoint membership, object membership
• Viewpoint node
  • ID, position, timestamp
  • object visibility (direct + inferred)
  • room membership
• Object node
  • ID(s), label, position, geometry, timestamp
  • visible viewpoints
  • room membership

Edge types

• Room <-> Room (door adjacency)
  • Derived from door candidates to an adjacency matrix.
• Room <-> Viewpoint (membership)
  • Updated by room-mask lookup of viewpoint position.
• Room <-> Object (membership)
  • Assigned by room-mask lookup of object centroid.
• Viewpoint <-> Object (visibility)
  • Populated by line-of-sight checks and direct observation links.

Invariants

• Room IDs are 1-based; room_mask value 0 means unknown.
• All graph positions are in map frame.

Update Flow

A) Room Mask Updates

1. Room mask arrives.
2. Planner resizes to configured voxel grid.
3. Updates the room mask in coverage and viewpoint modules, then refreshes
   viewpoint-room memberships.

B) Room Nodes List Updates

1. Room node list arrives.
2. Create or update room nodes in the graph.
3. Remove stale rooms.
4. Validate labeled room anchor points against the current room mask.

C) Door Candidate Updates

1. Door candidates arrive.
2. Filter out doors in collision.
3. Build adjacency using the two room IDs attached to each door.

D) Object Node Updates

1. Object nodes arrive.
2. Update or insert object nodes in the graph.
3. Record new object IDs for visibility updates.

Navigation Semantics (Graph Usage)

Room-to-Room Transitions

Goal: find a door sequence from current room to target room.

1. Resolve target room
   • Use room label votes to match the requested label.
   • If a modifier is present (e.g., "near lobby"), prefer rooms whose
     adjacency includes the modifier room.
2. Compute path
   • BFS on the adjacency matrix from the current room to the target room.
3. Select door
   • For the next hop, find door points where (r, g) match the two rooms and
     label == 0.
   • Use the door centroid as the interim navigation target.
4. Traverse
   • Publish door position and door normal marker.
   • When the room mask indicates a room change, repeat BFS for the remaining path.

Room Membership Updates

Current room ID is derived from the room mask at the robot position and
updated on every room-mask refresh. Merge/split detection uses overlaps between
the new and previous masks to decide whether to accept the new ID immediately.

Example Commands and Graph Execution

Example 1: "Go to the reception area near the lobby."

Interpretation

• Target label: reception
• Proximity constraint: near lobby (adjacent to a room labeled lobby)

Graph steps

1. Find all rooms labeled lobby.
2. Among rooms labeled reception, select one whose neighbor set contains a
   lobby room. If multiple, pick the one with the shortest BFS distance from
   the current room ID.
3. Run BFS on the door adjacency to get a room path.
4. For the first hop, pick the door connecting the next room in the path.
5. Navigate to the door centroid; once the robot enters the next room (room mask
   updates), repeat until in the reception room.
6. Use the room's anchor point (if set) as the final local goal.

Example 2: "Go to the kitchen from the bathroom."

Interpretation

• Current room label: bathroom
• Target label: kitchen

Graph steps

1. Verify the robot is in a room labeled bathroom (from the room mask lookup).
2. Resolve kitchen to a target room ID.
3. BFS over the adjacency matrix to get the room sequence.
4. For each hop, set the door centroid as the interim goal.
5. After each door crossing, update the current room from the room mask and
   continue until the kitchen room is reached.
6. Use the kitchen room anchor point as the final local goal.

The issue describes a navigation specification but names no files, tests, or concrete entry points. Start by locating the planner's representation layer and the standalone room segmentation node, then compare their existing interfaces with the specified graph updates and invariants. Done means the scoped semantic scene graph, segmentation behavior, and example door-navigation flows are documented consistently.