# G1 SLAM Stack

Production SLAM for **Unitree G1 Edu** humanoid + **Livox MID-360** head-mounted LiDAR.

KISS-ICP scan matching, voxel persistence filtering, geometric place recognition, pose-graph loop closure, submap mapping with atomic checkpointing, and a Nav2-compatible map export pipeline. Runs as a multi-process worker with a PyQt commander UI.

---

## Hardware

| Item | Spec |
|---|---|
| Robot | Unitree G1 Edu (~1.32 m, ~0.30 m stride) |
| LiDAR | Livox MID-360 (head-mounted, z ≈ 1.30 m) |
| LiDAR rate | 10 Hz, ~200 K pts/s, 360° H-FoV, -7° to +52° V-FoV |
| LiDAR range | 0.10 m – 40 m (10% reflectivity) |
| Compute | Workstation (mapping/UI) + Jetson Orin NX (deploy target) |

---

## Requirements

### System

- Linux x86_64 (workstation) or aarch64 (Jetson Orin NX)
- Python ≥ 3.10
- A wired NIC (Gigabit) on the LiDAR network — typically `enp3s0`

### Native libraries

- **Livox-SDK2** — C++ SDK from Livox. The repo ships with `Livox-SDK2/` as a submodule. Build and install:
  ```bash
  cd Livox-SDK2
  mkdir -p build && cd build
  cmake .. && make -j
  sudo make install
  ```
  This places `liblivox_lidar_sdk_*` under `/usr/local/lib/`.

### Python packages

```bash
pip install numpy scipy open3d kiss-icp PyQt6 pyqtgraph
```

| Package | Why |
|---|---|
| `numpy` | Point-cloud math throughout |
| `scipy` | KD-trees in cleanup / loop closure |
| `open3d` | Point-cloud I/O (.ply read/write) |
| `kiss-icp` | Scan-to-scan ICP backend |
| `PyQt6` | Commander GUI |
| `pyqtgraph` | OpenGL viewport in the GUI |

The Livox driver wrapper (`livox2_python.py`) uses `ctypes` against the installed `liblivox_lidar_sdk_*` — no separate Python binding required.

### Network

LiDAR ships at `192.168.123.120`. The workstation NIC must be on the same `/24`:

```bash
sudo ip addr add 192.168.123.222/24 dev enp3s0
sudo ip link set enp3s0 up
ping 192.168.123.120   # verify
```

UDP ports 56100–56501 must not be in use (close LivoxViewer 2 / other SLAM instances first).

---

## How to run

### 1. Verify the LiDAR is online

```bash
ping 192.168.123.120
```

If no reply: check cable, NIC IP (`ip addr show enp3s0`), and that no other software is bound to the LiDAR's ports.

### 2. Sanity-check the config

Open `SLAM_Config.json` and `mid360_config.json`. Confirm:
- `network.default_host_ip` matches your NIC IP
- `mid360_config.json` host IPs match
- `extrinsic_parameter.z` matches the actual LiDAR mount height

### 3. Launch the GUI

```bash
cd /home/zedx/Robotics_workspace/yslootahtech/G1_Lootah/Lidar
python3 SLAM_GUI.py
```

The **SLAM Commander** window opens.

### 4. Map a new place

1. Click **CONNECT** — the worker spins up and connects to the LiDAR.
2. Click **MAP NEW PLACE** — sets profile to `MAP_NEW` (long decay, fine voxels).
3. Click **START** — mapping begins; points appear in the viewport.
4. Walk the robot through the space.
5. Click **STOP** — if `SAVE ARMED` is checked, the map saves to `DataMap/<name>.ply`.

Use **AUTOSAVE** for long sessions (saves every N seconds).

### 5. Localize / navigate in an existing map

1. **CONNECT**.
2. **NAVIGATE IN MAPPED PLACE** — sets profile to `LOCALIZE_MAP`.
3. **LOAD MAP** — pick a `.ply` from `DataMap/`.
4. **START** — the system runs ICP against the loaded map. The status bar shows `TRACKING` once aligned.
5. Use the mission/waypoint controls to drive goals.

If the robot starts in an unknown spot inside the map, global relocalization runs automatically: session-memory cache → place recognition → brute-force anchor × yaw search.

### 6. Override the config path

```bash
SLAM_CONFIG=/path/to/custom.json python3 SLAM_GUI.py
```

### 7. Headless replay / regression

```bash
python3 SLAM_Replay.py --recording <path>.lvx2
```

---

## File map

| File | Purpose |
|---|---|
| `SLAM_GUI.py` | PyQt commander UI |
| `SLAM_engine.py` | Config dataclasses, worker bootstrap, `EngineConfig` |
| `SLAM_worker.py` | Main SLAM loop: ingest, ICP, filter, map, checkpoint, save, navigate |
| `SLAM_LocalizationService.py` | `odom`/`map`/`ref` frame transforms |
| `SLAM_LoopClosure.py` | Keyframe loop closure with SE3-slerp error distribution |
| `SLAM_PlaceRecognition.py` | Geometric anchor descriptors for global relocalization |
| `SLAM_StateMachine.py` | TRACKING / DEGRADED / LOST / RECOVERY |
| `SLAM_Submap.py` | Local + global submap mapper, atomic `.pkl` checkpointing |
| `SLAM_Filter.py` | Voxel persistence filter + indoor map quality filter |
| `SLAM_MAP.py` | Stable-map layer, `.ply` export via Open3D |
| `SLAM_Navigation.py` | Nav2-compatible YAML/PGM export, A* planner |
| `SLAM_NavRuntime.py` | Live cost-map for runtime planning |
| `SLAM_Mission.py` | Waypoint missions |
| `SLAM_Fusion.py` | LiDAR + IMU + (wheel/vision) pose fusion |
| `SLAM_Safety.py` | E-stop / stale-localization watchdog |
| `SLAM_Session.py` | Session memory (cached `slam→ref` alignments per map) |
| `SLAM_Replay.py` | Offline replay / regression harness |
| `SLAM_Validation.py` | Startup self-check |
| `SLAM_Diagnostics.py` | Crash logging, performance snapshots |
| `SLAM_Transforms.py` | SE3 utilities, pose deltas, slerp |
| `livox2_python.py` | Livox-SDK2 ctypes wrapper, with tag-byte noise filter |
| `SLAM_Config.json` | Single source of truth for tuning |
| `mid360_config.json` | Livox driver config (IPs, ports, extrinsics) |

---

## Configuration reference

### Network — IPs & Interfaces

| Source | Setting | Value |
|---|---|---|
| `SLAM_Config.json` | `network.default_interface` | `enp3s0` |
| `SLAM_Config.json` | `network.default_host_ip` | `192.168.123.222` (workstation) |
| `mid360_config.json` | `lidar_configs[0].ip` | `192.168.123.120` (LiDAR — Unitree default) |
| `mid360_config.json` | `host_net_info.*_ip` | `192.168.123.222` (matches workstation) |

> Reference (Unitree docs): default LiDAR IP `192.168.123.120`, host on `192.168.123.x/24`, gateway `192.168.123.1`.

### LiDAR UDP ports — `mid360_config.json`

| Direction | Channel | LiDAR side | Host side |
|---|---|---|---|
| Control | `cmd_data` | 56100 | 56101 |
| Push messages | `push_msg` | 56200 | 56201 |
| Point cloud | `point_data` | 56300 | 56301 |
| IMU data | `imu_data` | 56400 | 56401 |
| Logs | `log_data` | 56500 | 56501 |

### LiDAR extrinsics — `mid360_config.json` (head mount, G1)

| Param | Value | Note |
|---|---|---|
| `roll` | 0.0° | |
| `pitch` | 0.0° | head mount, level (Unitree default) |
| `yaw` | 0.0° | |
| `x` | 0.0 m | |
| `y` | 0.0 m | |
| `z` | 1.30 m | LiDAR atop head, ≈ floor + 1.30 m |
| `pcl_data_type` | 1 | Cartesian High |
| `pattern_mode` | 0 | non-repetitive |

### SLAM core tuning — `SLAM_Config.json` highlights

| Section | Key | Value | Purpose |
|---|---|---|---|
| **slam** | `slam_voxel_size` | 0.12 m | ICP voxel |
| | `max_range` | 50.0 m | LiDAR clip |
| **filter** | `voxel_size` | 0.20 m | persistence base |
| | `hits_threshold` | 4 | (overridden by profiles) |
| | `persistence.decay_seconds` | 3.0 s | (overridden by profiles) |
| | `persistence.max_voxels` | 2,000,000 | mem cap |
| **map** | `display_voxel` / `save_voxel` | 0.08 / 0.05 m | GUI / .ply resolution |
| | `min_points_to_save` | 550 | guard |
| **map_quality** | `near_min_range_m` | 0.15 m | hardware min is 0.10 |
| | `body_exclusion` | x[-0.20, 0.25] · y[±0.25] · z[-1.40, -0.10] | head-mount G1 body box |
| **map_cleanup** | `keep_largest_n` / `period_sec` | 2 / 6.0 s | islands removed every 6 s |
| **submap_mapping** | `local_voxel_m` / `global_voxel_m` | 0.08 / 0.14 m | |
| | `checkpoint_interval_sec` | 60.0 s | atomic .pkl save |
| **navigation_export** | `z_min_m` / `z_max_m` | 0.05 / 1.40 m | floor+5 cm to 1.40 m |
| | `resolution_m` | 0.05 m | Nav2 grid |
| **state_machine** | `min_good_to_recover` | 3 | RECOVERY → TRACKING |
| **mission** | `waypoint_tolerance_m` | 0.55 m | G1 ~0.30 m stride margin |
| **safety** | `stop_radius_m` | 0.50 m | |
| | `stale_localization_sec` | 1.5 s | |
| **fusion** | `enabled` | `true` | LiDAR + IMU pose fusion |
| | `imu_weight` / `lidar_weight` | 0.25 / 1.0 | |
| **runtime** | `publish_hz` | 12.0 | GUI/status update rate |
| **livox** | `tag_filter` | `true` | Unitree-recommended noise drop |
| **loop_closure** | `enabled` | `false` | optimizer is real, but disabled |

### Stability profiles — hardcoded in `SLAM_worker.py:1957-1961`

| Profile | hit_threshold | decay_seconds | voxel_size | density |
|---|---|---|---|---|
| `MAP_NEW` | 2 | 1800.0 | 0.10 | MEDIUM |
| `LOCALIZE_MAP` | 3 | 45.0 | 0.20 | MEDIUM |
| `LIVE_NAV_MAP` | 2 | 18.0 | 0.20 | MEDIUM |
| `LIVE_NAV_NO_MAP` | 2 | 14.0 | 0.18 | HIGH |
| `QUICK_DEMO` | 2 | 8.0 | 0.20 | HIGH |
| `BALANCED` | (from config) | (from config) | (from config) | MEDIUM |

### File locations

```
G1_Lootah/Lidar/
├── SLAM_Config.json          # main config (single source of truth)
├── mid360_config.json        # Livox driver (read by SDK2)
└── DataMap/                  # maps + submap_checkpoint.pkl + session memory
```

Override path: `SLAM_CONFIG=/path/to/other.json`.

---

## Workflows

| Workflow | Profile | When to use |
|---|---|---|
| Map a new place | `MAP_NEW` | Building a fresh map. Long decay (30 min), 0.10 m voxels. |
| Navigate in mapped place | `LOCALIZE_MAP` | Localize against an existing `.ply`. |
| Live nav with map | `LIVE_NAV_MAP` | Navigation + slow map updates. |
| Live nav (no map) | `LIVE_NAV_NO_MAP` | Pure obstacle-avoidance, no persistent map. |
| Quick demo | `QUICK_DEMO` | Short-window mapping for demos. |

The current profile shows in the GUI status bar.

---

## Outputs

```
DataMap/
├── *.ply                       # saved point cloud maps
├── *.yaml + *.pgm              # Nav2-compatible map_server bundles
├── submap_checkpoint.pkl       # atomic submap snapshot (auto-restored on next start)
└── SLAM_session_memory.json    # cached relocalization alignments per map
```

---

## Frame conventions

- World z = 0 is floor.
- LiDAR origin at sensor frame's z = 0; extrinsic places it at body-frame z ≈ 1.30 m.
- Body exclusion box in **sensor frame**: z = -1.40 (floor) to z = -0.10 (just below LiDAR).
- ICP yields `T_world_lidar`; localization service maintains `odom_to_map` and `odom_to_ref` separately.

---

## Deploy

Edit on the **workstation only**. Push to Jetson via `scp`:

```bash
scp -r /home/zedx/Robotics_workspace/yslootahtech/G1_Lootah/Lidar/ \
    unitree@<jetson-ip>:~/G1_Lootah/
```

Do not edit on the Jetson directly.

---

## Common issues

- **Connect fails / `Address already in use`** — another Livox app holds ports 56101–56501. Close LivoxViewer 2 / other SLAM instances.
- **No map / empty viewport** — verify `ping 192.168.123.120` works, NIC IP matches `default_host_ip`, and the LiDAR LED is solid.
- **Map ignores furniture** — head-mount + -7° lower FoV creates a blind cone underneath; close-floor area is unscannable < ~10 m. Move the robot closer or accept the limitation.
- **Stray points in saved map** — cleanup pass runs every 6 s and once more on save; if islands persist, lower `map_cleanup.keep_largest_n`.
- **Loop closure off** — set `loop_closure.enabled: true` in config; the optimizer (`SLAM_LoopClosure._optimize`) is fully wired.
- **`SDKINIT failed`** — Livox-SDK2 not installed; rebuild `Livox-SDK2/` and `sudo make install`.