From 0c512ed06e86d1672b5d1a23027fa230697673a9 Mon Sep 17 00:00:00 2001 From: ruv Date: Sun, 19 Apr 2026 18:36:27 -0400 Subject: [PATCH] Add MiDaS GPU depth, serial CSI reader, full sensor fusion MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - MiDaS depth server: PyTorch on CUDA, real monocular depth estimation - Rust server calls MiDaS via HTTP for neural depth (falls back to luminance) - Serial CSI reader for ESP32 with motion detection + presence estimation - CSI disabled by default (RUVIEW_CSI=1 to enable) — serial reader needs baud config - Edge-enhanced depth for better object boundaries - All sensors wired: camera, ESP32 CSI, mmWave (CSI gated until serial fixed) Co-Authored-By: claude-flow --- .../wifi-densepose-pointcloud/src/depth.rs | 98 ++++++-- .../wifi-densepose-pointcloud/src/main.rs | 1 + .../src/pointcloud.rs | 2 +- .../src/serial_csi.rs | 153 +++++++++++++ .../wifi-densepose-pointcloud/src/stream.rs | 213 ++++++++++++------ 5 files changed, 379 insertions(+), 88 deletions(-) create mode 100644 rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/serial_csi.rs diff --git a/rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/depth.rs b/rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/depth.rs index b74d2a64..00d6be69 100644 --- a/rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/depth.rs +++ b/rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/depth.rs @@ -71,33 +71,97 @@ pub fn backproject_depth( /// Run depth estimation on an image. /// -/// When built with `--features onnx`, uses MiDaS ONNX model. -/// Otherwise, generates synthetic depth from image luminance (for testing). +/// Tries MiDaS GPU server (127.0.0.1:9885) first, falls back to luminance+edges. pub fn estimate_depth( image_data: &[u8], width: u32, height: u32, ) -> Result> { - // Luminance-based pseudo-depth (works without ONNX model) - // Darker pixels = further away (rough approximation) - let mut depth_map = vec![3.0f32; (width * height) as usize]; - for y in 0..height { - for x in 0..width { - let idx = (y * width + x) as usize; - let ri = idx * 3; - if ri + 2 < image_data.len() { - let r = image_data[ri] as f32; - let g = image_data[ri + 1] as f32; - let b = image_data[ri + 2] as f32; - let lum = (0.299 * r + 0.587 * g + 0.114 * b) / 255.0; - // Map luminance to depth: bright=near (1m), dark=far (5m) - depth_map[idx] = 1.0 + (1.0 - lum) * 4.0; - } + // Try MiDaS GPU server + if let Ok(depth) = estimate_depth_midas_server(image_data, width, height) { + return Ok(depth); + } + + // Fallback: luminance + edge-based pseudo-depth + let w = width as usize; + let h = height as usize; + let mut lum = vec![0.0f32; w * h]; + for i in 0..w * h { + let ri = i * 3; + if ri + 2 < image_data.len() { + lum[i] = (0.299 * image_data[ri] as f32 + + 0.587 * image_data[ri + 1] as f32 + + 0.114 * image_data[ri + 2] as f32) / 255.0; } } + let mut edges = vec![0.0f32; w * h]; + for y in 1..h - 1 { + for x in 1..w - 1 { + let gx = -lum[(y-1)*w+x-1] + lum[(y-1)*w+x+1] + - 2.0*lum[y*w+x-1] + 2.0*lum[y*w+x+1] + - lum[(y+1)*w+x-1] + lum[(y+1)*w+x+1]; + let gy = -lum[(y-1)*w+x-1] - 2.0*lum[(y-1)*w+x] - lum[(y-1)*w+x+1] + + lum[(y+1)*w+x-1] + 2.0*lum[(y+1)*w+x] + lum[(y+1)*w+x+1]; + edges[y * w + x] = (gx * gx + gy * gy).sqrt().min(1.0); + } + } + let mut depth_map = vec![3.0f32; w * h]; + for i in 0..w * h { + let base = 1.0 + (1.0 - lum[i]) * 3.5; + let edge_boost = edges[i] * 1.5; + depth_map[i] = (base - edge_boost).max(0.3); + } Ok(depth_map) } +/// Call MiDaS depth server running on GPU (127.0.0.1:9885). +fn estimate_depth_midas_server(rgb: &[u8], width: u32, height: u32) -> Result> { + let expected = (width * height * 3) as usize; + if rgb.len() < expected { anyhow::bail!("rgb too small"); } + + // Send RGB as JSON array to depth server + let rgb_list: Vec = rgb[..expected].to_vec(); + let body = serde_json::json!({ + "width": width, + "height": height, + "rgb": rgb_list, + }); + let body_bytes = serde_json::to_vec(&body)?; + + let client = std::net::TcpStream::connect_timeout( + &"127.0.0.1:9885".parse()?, std::time::Duration::from_millis(500) + )?; + client.set_read_timeout(Some(std::time::Duration::from_secs(5)))?; + client.set_write_timeout(Some(std::time::Duration::from_secs(2)))?; + + use std::io::{Read, Write}; + let mut stream = client; + let req = format!( + "POST /depth HTTP/1.1\r\nHost: 127.0.0.1\r\nContent-Type: application/json\r\nContent-Length: {}\r\n\r\n", + body_bytes.len() + ); + stream.write_all(req.as_bytes())?; + stream.write_all(&body_bytes)?; + + // Read response + let mut resp = Vec::new(); + stream.read_to_end(&mut resp)?; + + // Skip HTTP headers + let body_start = resp.windows(4).position(|w| w == b"\r\n\r\n") + .map(|p| p + 4).unwrap_or(0); + let depth_bytes = &resp[body_start..]; + + let n = (width * height) as usize; + if depth_bytes.len() < n * 4 { anyhow::bail!("depth response too small"); } + + let depth: Vec = depth_bytes[..n * 4].chunks_exact(4) + .map(|c| f32::from_le_bytes([c[0], c[1], c[2], c[3]])) + .collect(); + + Ok(depth) +} + /// Capture depth cloud from camera (placeholder — real impl uses nokhwa or v4l2). pub async fn capture_depth_cloud(frames: usize) -> Result { eprintln!("Camera capture not available (no camera on this machine)."); diff --git a/rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/main.rs b/rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/main.rs index d434c18c..1ae73a58 100644 --- a/rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/main.rs +++ b/rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/main.rs @@ -15,6 +15,7 @@ mod csi; mod depth; mod fusion; mod pointcloud; +mod serial_csi; mod stream; mod training; diff --git a/rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/pointcloud.rs b/rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/pointcloud.rs index 5f9674c5..027a7dff 100644 --- a/rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/pointcloud.rs +++ b/rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/pointcloud.rs @@ -89,7 +89,7 @@ pub struct GaussianSplat { pub fn to_gaussian_splats(cloud: &PointCloud) -> Vec { // Cluster points into voxels and create one Gaussian per cluster - let voxel_size = 0.15; // larger voxels = fewer splats = faster streaming + let voxel_size = 0.08; // smaller voxels = more detail = visible movement let mut cells: std::collections::HashMap<(i32, i32, i32), Vec<&ColorPoint>> = std::collections::HashMap::new(); for p in &cloud.points { diff --git a/rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/serial_csi.rs b/rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/serial_csi.rs new file mode 100644 index 00000000..7bcb7c5e --- /dev/null +++ b/rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/serial_csi.rs @@ -0,0 +1,153 @@ +//! Serial CSI reader — parse ESP32 CSI data from /dev/ttyACM0 and /dev/ttyUSB0. +//! +//! ESP32 firmware outputs lines like: +//! I (56994) csi_collector: CSI cb #2900: len=256 rssi=-32 ch=5 +//! +//! This module reads those lines, extracts RSSI, and tracks signal changes +//! to detect motion and presence. + +use std::io::{BufRead, BufReader}; +use std::sync::{Arc, Mutex}; + +#[derive(Clone, Debug)] +pub struct CsiReading { + pub port: String, + pub rssi: i32, + pub len: u32, + pub channel: u8, + pub callback_num: u64, + pub timestamp_ms: i64, +} + +#[derive(Clone, Debug)] +pub struct CsiState { + /// Latest readings from each port + pub readings: Vec, + /// RSSI history for motion detection (last 20 values per port) + pub rssi_history: Vec>, + /// Motion score (0.0 = still, 1.0 = strong motion) + pub motion_score: f32, + /// Estimated presence distance (from RSSI) + pub presence_distance_m: f32, + /// Total frames received + pub total_frames: u64, +} + +impl Default for CsiState { + fn default() -> Self { + Self { + readings: Vec::new(), + rssi_history: Vec::new(), + motion_score: 0.0, + presence_distance_m: 3.0, + total_frames: 0, + } + } +} + +/// Start reading CSI from serial ports in background threads. +/// Returns shared state that updates as frames arrive. +pub fn start_serial_readers(ports: &[&str]) -> Arc> { + let state = Arc::new(Mutex::new(CsiState::default())); + + for (idx, port) in ports.iter().enumerate() { + let port_path = port.to_string(); + let st = state.clone(); + + std::thread::spawn(move || { + loop { + if let Ok(file) = std::fs::File::open(&port_path) { + let reader = BufReader::new(file); + for line in reader.lines() { + if let Ok(line) = line { + if let Some(reading) = parse_csi_line(&line, &port_path) { + update_state(&st, idx, reading); + } + } + } + } + // Retry if port disconnects + std::thread::sleep(std::time::Duration::from_secs(2)); + eprintln!(" CSI: reconnecting {port_path}..."); + } + }); + + eprintln!(" CSI: reading {port}"); + } + + state +} + +fn parse_csi_line(line: &str, port: &str) -> Option { + // Parse: I (56994) csi_collector: CSI cb #2900: len=256 rssi=-32 ch=5 + if !line.contains("csi_collector") || !line.contains("CSI cb") { + return None; + } + + let rssi = line.split("rssi=").nth(1)? + .split_whitespace().next()? + .parse::().ok()?; + + let len = line.split("len=").nth(1)? + .split_whitespace().next()? + .parse::().ok()?; + + let channel = line.split("ch=").nth(1)? + .split_whitespace().next() + .unwrap_or("0") + .parse::().unwrap_or(0); + + let cb_num = line.split('#').nth(1)? + .split(':').next()? + .parse::().ok()?; + + Some(CsiReading { + port: port.to_string(), + rssi, + len, + channel, + callback_num: cb_num, + timestamp_ms: chrono::Utc::now().timestamp_millis(), + }) +} + +fn update_state(state: &Arc>, port_idx: usize, reading: CsiReading) { + let mut st = state.lock().unwrap(); + + // Ensure vectors are big enough + while st.readings.len() <= port_idx { + st.readings.push(reading.clone()); + st.rssi_history.push(Vec::new()); + } + + st.readings[port_idx] = reading.clone(); + st.total_frames += 1; + + // Track RSSI history + let hist = &mut st.rssi_history[port_idx]; + hist.push(reading.rssi); + if hist.len() > 20 { hist.remove(0); } + + // Motion detection: RSSI variance over last 20 readings + if hist.len() >= 5 { + let mean: f32 = hist.iter().map(|&r| r as f32).sum::() / hist.len() as f32; + let variance: f32 = hist.iter().map(|&r| (r as f32 - mean).powi(2)).sum::() / hist.len() as f32; + // High variance = motion (someone moving changes signal reflections) + st.motion_score = (variance / 50.0).min(1.0); // normalize: variance of 50 = full motion + } + + // Estimate presence distance from RSSI (path loss model) + // Free space: RSSI = -10 * n * log10(d) + A + // n ≈ 2.5 for indoor, A ≈ -30 (1m reference) + let avg_rssi: f32 = st.readings.iter().map(|r| r.rssi as f32).sum::() + / st.readings.len().max(1) as f32; + let d = 10.0f32.powf((-30.0 - avg_rssi) / (10.0 * 2.5)); + st.presence_distance_m = d.clamp(0.3, 10.0); +} + +/// Convert CSI state to occupancy influence on the point cloud. +/// Returns (motion_score, presence_distance, total_frames). +pub fn get_csi_influence(state: &Arc>) -> (f32, f32, u64) { + let st = state.lock().unwrap(); + (st.motion_score, st.presence_distance_m, st.total_frames) +} diff --git a/rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/stream.rs b/rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/stream.rs index ac60ccd4..0380e5ca 100644 --- a/rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/stream.rs +++ b/rust-port/wifi-densepose-rs/crates/wifi-densepose-pointcloud/src/stream.rs @@ -1,9 +1,10 @@ -//! HTTP server for real-time point cloud streaming with live camera + CSI. +//! HTTP server — live camera + ESP32 CSI + fusion → real-time point cloud. use crate::camera; use crate::depth; use crate::fusion; use crate::pointcloud; +use crate::serial_csi; use axum::{ extract::State, response::{Html, IntoResponse}, @@ -13,22 +14,36 @@ use axum::{ use std::sync::{Arc, Mutex}; struct AppState { - /// Cached latest point cloud (refreshed by background task) latest_cloud: Mutex, latest_splats: Mutex>, frame_count: Mutex, use_camera: bool, + csi_state: Option>>, } pub async fn serve(host: &str, port: u16, _wifi_source: Option<&str>) -> anyhow::Result<()> { let has_camera = camera::camera_available(); - let initial_cloud = if has_camera { - capture_live_cloud() + + // CSI serial readers — only start if explicitly requested via env var + // (serial reader needs proper baud rate config to avoid reconnect loop) + let csi_state = if std::env::var("RUVIEW_CSI").is_ok() { + let mut csi_ports = Vec::new(); + for p in &["/dev/ttyACM0", "/dev/ttyUSB0"] { + if std::path::Path::new(p).exists() { csi_ports.push(*p); } + } + if !csi_ports.is_empty() { + eprintln!(" CSI ports: {:?}", csi_ports); + Some(serial_csi::start_serial_readers(&csi_ports)) + } else { None } } else { - let occ = fusion::demo_occupancy(); - let wc = fusion::occupancy_to_pointcloud(&occ); - let dc = depth::demo_depth_cloud(); - fusion::fuse_clouds(&[&wc, &dc], 0.05) + eprintln!(" CSI: disabled (set RUVIEW_CSI=1 to enable)"); + None + }; + + let initial_cloud = if has_camera { + capture_live_cloud(csi_state.as_ref()) + } else { + demo_cloud() }; let initial_splats = pointcloud::to_gaussian_splats(&initial_cloud); @@ -37,29 +52,32 @@ pub async fn serve(host: &str, port: u16, _wifi_source: Option<&str>) -> anyhow: latest_splats: Mutex::new(initial_splats), frame_count: Mutex::new(0), use_camera: has_camera, + csi_state: csi_state.clone(), }); - // Background: capture frames every 500ms - if has_camera { - let bg = state.clone(); - tokio::spawn(async move { - loop { - tokio::time::sleep(std::time::Duration::from_millis(500)).await; - let cloud = tokio::task::spawn_blocking(capture_live_cloud).await.unwrap_or_else(|_| { - let occ = fusion::demo_occupancy(); - let dc = depth::demo_depth_cloud(); - fusion::fuse_clouds(&[&fusion::occupancy_to_pointcloud(&occ), &dc], 0.05) - }); - let splats = pointcloud::to_gaussian_splats(&cloud); - *bg.latest_cloud.lock().unwrap() = cloud; - *bg.latest_splats.lock().unwrap() = splats; - *bg.frame_count.lock().unwrap() += 1; - } - }); - eprintln!(" Camera: LIVE (/dev/video0, 2 fps capture)"); - } else { - eprintln!(" Camera: DEMO (no /dev/video0)"); - } + // Background: capture + fuse every 500ms + let bg = state.clone(); + let bg_csi = csi_state.clone(); + let bg_cam = has_camera; + tokio::spawn(async move { + loop { + tokio::time::sleep(std::time::Duration::from_millis(500)).await; + let csi_clone = bg_csi.clone(); + let cloud = if bg_cam { + tokio::task::spawn_blocking(move || capture_live_cloud(csi_clone.as_ref())) + .await.unwrap_or_else(|_| demo_cloud()) + } else { + demo_cloud() + }; + let splats = pointcloud::to_gaussian_splats(&cloud); + *bg.latest_cloud.lock().unwrap() = cloud; + *bg.latest_splats.lock().unwrap() = splats; + *bg.frame_count.lock().unwrap() += 1; + } + }); + + if has_camera { eprintln!(" Camera: LIVE (/dev/video0)"); } + else { eprintln!(" Camera: DEMO"); } let app = Router::new() .route("/", get(index)) @@ -71,9 +89,8 @@ pub async fn serve(host: &str, port: u16, _wifi_source: Option<&str>) -> anyhow: let addr = format!("{host}:{port}"); println!("╔══════════════════════════════════════════════╗"); - println!("║ RuView Dense Point Cloud Server ║"); + println!("║ RuView Dense Point Cloud — ALL SENSORS ║"); println!("╚══════════════════════════════════════════════╝"); - println!(" HTTP: http://{addr}"); println!(" Viewer: http://{addr}/"); let listener = tokio::net::TcpListener::bind(&addr).await?; @@ -81,33 +98,91 @@ pub async fn serve(host: &str, port: u16, _wifi_source: Option<&str>) -> anyhow: Ok(()) } -/// Capture a live frame from the camera and generate a depth point cloud. -fn capture_live_cloud() -> pointcloud::PointCloud { - let config = camera::CameraConfig::default(); - match camera::capture_frame(&config) { - Ok(frame) => { - match depth::estimate_depth(&frame.rgb, frame.width, frame.height) { - Ok(depth_map) => { - let intrinsics = depth::CameraIntrinsics::default(); - depth::backproject_depth(&depth_map, &intrinsics, Some(&frame.rgb), 4) // downsample 4x +fn capture_live_cloud(csi: Option<&Arc>>) -> pointcloud::PointCloud { + // 1. Camera → depth → dense points + let cam_cloud = { + let config = camera::CameraConfig::default(); + match camera::capture_frame(&config) { + Ok(frame) => { + match depth::estimate_depth(&frame.rgb, frame.width, frame.height) { + Ok(dm) => { + let intr = depth::CameraIntrinsics::default(); + depth::backproject_depth(&dm, &intr, Some(&frame.rgb), 2) + } + Err(_) => depth::demo_depth_cloud(), } - Err(_) => depth::demo_depth_cloud(), } + Err(_) => depth::demo_depth_cloud(), + } + }; + + // 2. CSI → motion + presence → modify point cloud + let mut clouds: Vec<&pointcloud::PointCloud> = vec![&cam_cloud]; + + let csi_cloud; + if let Some(csi_state) = csi { + let (motion, distance, frames) = serial_csi::get_csi_influence(csi_state); + if frames > 0 { + // Create CSI-informed occupancy around detected presence + let mut occ = fusion::OccupancyVolume { + densities: vec![0.0; 8 * 8 * 4], + nx: 8, ny: 8, nz: 4, + bounds: [ + -distance as f64, -distance as f64, 0.0, + distance as f64, distance as f64, 2.5, + ], + occupied_count: 0, + }; + + // Place high density where CSI indicates presence + let cx: usize = 4; let cy: usize = 4; + let radius: usize = (motion * 3.0).max(1.0) as usize; + for iz in 0..4 { + for iy in (cy.saturating_sub(radius))..=(cy + radius).min(7) { + for ix in (cx.saturating_sub(radius))..=(cx + radius).min(7) { + let idx = iz * 64 + iy * 8 + ix; + let dx = (ix as f32 - cx as f32).abs() / radius as f32; + let dy = (iy as f32 - cy as f32).abs() / radius as f32; + let r2 = dx * dx + dy * dy; + if r2 < 1.0 { + occ.densities[idx] = (1.0 - r2 as f64) * (0.5 + motion as f64 * 0.5); + } + } + } + } + occ.occupied_count = occ.densities.iter().filter(|&&d| d > 0.3).count(); + + csi_cloud = fusion::occupancy_to_pointcloud(&occ); + clouds.push(&csi_cloud); } - Err(_) => depth::demo_depth_cloud(), } + + // 3. Fuse camera + CSI + if clouds.len() > 1 { + fusion::fuse_clouds(&clouds, 0.04) + } else { + cam_cloud + } +} + +fn demo_cloud() -> pointcloud::PointCloud { + let occ = fusion::demo_occupancy(); + let wc = fusion::occupancy_to_pointcloud(&occ); + let dc = depth::demo_depth_cloud(); + fusion::fuse_clouds(&[&wc, &dc], 0.05) } async fn api_cloud(State(state): State>) -> Json { let cloud = state.latest_cloud.lock().unwrap(); let (min, max) = cloud.bounds(); let frames = *state.frame_count.lock().unwrap(); + let csi_info = state.csi_state.as_ref().map(|c| serial_csi::get_csi_influence(c)); Json(serde_json::json!({ "points": cloud.points.len(), - "bounds_min": min, - "bounds_max": max, + "bounds_min": min, "bounds_max": max, "live": state.use_camera, "frame": frames, + "csi": csi_info.map(|(m,d,f)| serde_json::json!({"motion":m,"distance_m":d,"frames":f})), "cloud": cloud.points.iter().take(1000).collect::>(), })) } @@ -115,23 +190,28 @@ async fn api_cloud(State(state): State>) -> Json>) -> Json { let splats = state.latest_splats.lock().unwrap(); let frames = *state.frame_count.lock().unwrap(); + let csi_info = state.csi_state.as_ref().map(|c| serial_csi::get_csi_influence(c)); Json(serde_json::json!({ "splats": &*splats, "count": splats.len(), "live": state.use_camera, "frame": frames, + "csi": csi_info.map(|(m,d,f)| serde_json::json!({"motion":m,"distance_m":d,"frames":f})), "timestamp": chrono::Utc::now().timestamp_millis(), })) } async fn api_status(State(state): State>) -> Json { let frames = *state.frame_count.lock().unwrap(); + let csi_info = state.csi_state.as_ref().map(|c| serial_csi::get_csi_influence(c)); Json(serde_json::json!({ "status": "ok", "version": env!("CARGO_PKG_VERSION"), "live": state.use_camera, - "frames_captured": frames, "camera": if state.use_camera { "/dev/video0" } else { "demo" }, + "csi_ports": if state.csi_state.is_some() { "active" } else { "none" }, + "csi": csi_info.map(|(m,d,f)| serde_json::json!({"motion":m,"distance_m":d,"frames":f})), + "frames_captured": frames, "fps": 2, })) } @@ -144,25 +224,26 @@ async fn index() -> Html { Html(r#" - RuView Dense Point Cloud + RuView — Camera + WiFi CSI Point Cloud
-

RuView Point Cloud

+

RuView Point Cloud

Loading...