G1_Lootah/Lidar

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:

  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

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:

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

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

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

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

7. Headless replay / regression

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:

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.