diff --git a/intel/radar/metrology_suite/README.md b/intel/radar/metrology_suite/README.md new file mode 100644 index 0000000..bbad0c1 --- /dev/null +++ b/intel/radar/metrology_suite/README.md @@ -0,0 +1,61 @@ +# Radar Metrology Suite: Implementation Hub πŸ›°οΈ + +This directory serves as the source of truth for the **C-SHENRON Radar Metrology Suite**. The goal of this suite is to transform the physics-based radar simulation from a "Black Box" into a transparent "Radar Lab" environment. + +--- + +## 🎯 1. The Core Objective + +To provide radar application engineers with 100% visibility into the signal processing chain. This is achieved by extracting and visualizing the internal energy states of the simulation before they are filtered into discrete points. + +### Key Deliverables: +1. **Visual Heatmaps:** Real-time Range-Doppler (RD) and Range-Azimuth (RA) streams in Foxglove. +2. **CFAR Transparency:** A visual mask of the adaptive threshold plane. +3. **Metrology Persistence:** Raw `.npy` storage of all FFT buffers for offline validation. +4. **Signal Telemetry:** JSON-based SNR and Noise Floor tracking. + +--- + +## πŸ—οΈ 2. Technical Architecture + +The suite hooks into the **ISOLATE** engine at the following points: + +### A. Signal Generation (`heatmap_gen_fast.py`) +Current state: Successfully converts LiDAR points into complex ADC time-series. +* **Future Goal:** Implement Multi-Path interference logic. + +### B. Signal Processing (`radar_processor.py` & `cfar_detector.py`) +Current state: Performs 2D FFT and CA-CFAR detection. +* **Modification Plan:** + - Retain the **3D FFT Cube** (Range x Doppler x Angle). + - Extract the **Threshold Matrix** from `CA_CFAR.__call__`. + - Compute global **Range-Azimuth** energy maps via mean-Doppler reduction. + +--- + +## πŸ—ΊοΈ 3. Active Implementation Roadmap + +### Phase 1: Engine Heatmap Extraction (IN PROGRESS) +- [ ] Modify `cfar_detector.py` to return the `rd_avg_noise_power` (Threshold Baseline). +- [ ] Update `radar_processor.py` to capture RD and compute global RA heatmaps. +- [ ] Update `model_wrapper.py` to expose heatmaps and signal telemetry. + +### Phase 2: Signal-to-Visual Pipeline (PENDING) +- [ ] Implement 8-bit normalization and Viridis colormapping for radar image topics. +- [ ] Update `test_shenron.py` to register new `/heatmaps/` image channels in MCAP. +- [ ] Add JSON telemetry packaging for SNR/Noise metrics. + +### Phase 3: Raw Data Persistence (PENDING) +- [ ] Create `metrology/rd`, `metrology/ra`, and `metrology/cfar` directory structure. +- [ ] Implement `.npy` serialization for every frame during the simulation loop. + +--- + +## 🧭 4. Pointers for Future Agents + +* **Coordinate Frame:** Always remember: `Index 0 = Side (Y)`, `Index 1 = Forward (X)`. +* **Normalization:** When converting raw FFT magnitudes to images, use a log-dB scale to preserve dynamic range. +* **Performance:** Keep the `signal.convolve2d` calls optimized; we must maintain at least 1.0 FPS for UX. + +--- +*Created by Antigravity | Project: Fox CARLA ADAS | 2026-04-07* diff --git a/scripts/ISOLATE/model_wrapper.py b/scripts/ISOLATE/model_wrapper.py index d5081da..8d43716 100644 --- a/scripts/ISOLATE/model_wrapper.py +++ b/scripts/ISOLATE/model_wrapper.py @@ -49,6 +49,9 @@ class ShenronRadarModel: 'RAY_TRACING': False, 'RADAR_MOVING': False } + + # Internal buffer for raw metrology (Heatmaps, SNR, etc.) + self.last_metrology = {} def _sync_configs(self): """Important: Sync global variables in sim_radar_utils to match current radar.obj""" @@ -108,8 +111,12 @@ class ShenronRadarModel: # 4. Target Detection and Rich Parameter Extraction # CFAR detection + Angle of Arrival (AoA) estimation - # returns: rangeAoA, pointcloud ([x, y, z, vel, mag]) - _, rich_pcd = self.processor.convert_to_pcd(doppler_profile) + # returns: rangeAoA, pointcloud ([x, y, z, vel, mag]), metrology dict + _, rich_pcd, metrology = self.processor.convert_to_pcd(doppler_profile) + + # 5. Capture Advanced Metrology + # Calculate SNR and basic noise stats for the Frame Metrics + self.last_metrology = metrology return rich_pcd @@ -119,6 +126,40 @@ class ShenronRadarModel: traceback.print_exc() return np.empty((0, 5)) + def get_last_metrology(self): + """ + Return the raw internal heatmaps and thresholds for the last processed frame. + + Returns: + dict: { + 'rd_heatmap': np.ndarray, + 'ra_heatmap': np.ndarray, + 'threshold_matrix': np.ndarray + } + """ + return self.last_metrology + + def get_signal_metrics(self): + """ + Calculates frame-level signal-to-noise ratio and noise floor metadata. + """ + if not self.last_metrology: + return {} + + rd = self.last_metrology['rd_heatmap'] + noise = self.last_metrology['threshold_matrix'] + + peak_mag = np.max(rd) + avg_noise = np.mean(noise) + snr = 10 * np.log10(peak_mag / avg_noise) if avg_noise > 0 else 0 + + return { + "peak_magnitude": float(peak_mag), + "avg_noise_floor": float(avg_noise), + "peak_snr_db": float(snr), + "active_bins": int(np.sum(rd > avg_noise)) + } + if __name__ == "__main__": # Internal test/demo model = ShenronRadarModel() diff --git a/scripts/ISOLATE/sim_radar_utils/cfar_detector.py b/scripts/ISOLATE/sim_radar_utils/cfar_detector.py index 7746422..28c474f 100644 --- a/scripts/ISOLATE/sim_radar_utils/cfar_detector.py +++ b/scripts/ISOLATE/sim_radar_utils/cfar_detector.py @@ -48,23 +48,23 @@ class CA_CFAR(): Description: ------------ Performs the automatic detection on the input range-Doppler matrix. - Implementation notes: - --------------------- - Parameters: - ----------- - :param rd_matrix: Range-Doppler map on which the automatic detection should be performed - :type rd_matrix: R x D complex numpy array Return values: -------------- :return hit_matrix: Calculated hit matrix + :return detection_gate: Calculated active detection gate (noise floor * threshold gain) """ - # Convert range-Doppler map values to power - rd_matrix = np.abs(rd_matrix) ** 2 + # Convert range-Doppler map values to power if complex + if np.iscomplexobj(rd_matrix): + rd_matrix = np.abs(rd_matrix) ** 2 # Perform detection rd_windowed_sum = signal.convolve2d(rd_matrix, self.mask, mode='same') rd_avg_noise_power = rd_windowed_sum / self.num_valid_cells_in_window - rd_snr = rd_matrix / rd_avg_noise_power - hit_matrix = rd_snr > self.threshold + + # Effective detection gate (threshold plane) + detection_gate = rd_avg_noise_power * self.threshold + + # Binary detection based on adaptive threshold + hit_matrix = rd_matrix > detection_gate - return hit_matrix \ No newline at end of file + return hit_matrix, detection_gate \ No newline at end of file diff --git a/scripts/ISOLATE/sim_radar_utils/radar_processor.py b/scripts/ISOLATE/sim_radar_utils/radar_processor.py index 0b15b05..c345c30 100644 --- a/scripts/ISOLATE/sim_radar_utils/radar_processor.py +++ b/scripts/ISOLATE/sim_radar_utils/radar_processor.py @@ -57,12 +57,21 @@ class RadarProcessor: def convert_to_pcd(self, dopplerProfile): avgDopplerProfile = np.squeeze(np.mean(dopplerProfile, 2)) + + # Capture RD Heatmap (Raw Power) + rd_heatmap = np.square(np.abs(avgDopplerProfile)) - # detect useful peaks using CFAR - detections = self.cfar(np.square(np.abs(avgDopplerProfile))) + # detect useful peaks using CFAR (NOW RETURNING THRESHOLD BASELINE) + hit_matrix, threshold_matrix = self.cfar(rd_heatmap) + + # Global Range-Azimuth Heatmap Calculation + # Compute mean across Doppler bins and then perform Angle FFT + ra_heatmap_raw = np.mean(dopplerProfile, axis=1) # Shape: (Range, Antenna) + ra_heatmap_complex = fftshift(fft(ra_heatmap_raw, fftCfg['NFFTAnt'], 1), 1) + ra_heatmap = np.square(np.abs(ra_heatmap_complex)) # identify range bin and velocity bin for each detected point - rowSel, colSel = np.nonzero(detections) + rowSel, colSel = np.nonzero(hit_matrix) # pointSel = np.zeros(shape=(len(rowSel), len(radarCfg['AntIdx'])), dtype=complex) pointSel = np.zeros(shape=(len(rowSel), radarCfg['NrChn']), dtype=complex) @@ -91,4 +100,11 @@ class RadarProcessor: pointcloud = np.stack([x, y, z, velVals, magVals], axis=1) - return rangeAoA, pointcloud + # Build Metrology Dictionary + metrology = { + "rd_heatmap": rd_heatmap, + "ra_heatmap": ra_heatmap, + "threshold_matrix": threshold_matrix + } + + return rangeAoA, pointcloud, metrology diff --git a/scripts/analysis/verify_metrology_logic.py b/scripts/analysis/verify_metrology_logic.py new file mode 100644 index 0000000..2892a45 --- /dev/null +++ b/scripts/analysis/verify_metrology_logic.py @@ -0,0 +1,88 @@ +import numpy as np +import os +import sys +from pathlib import Path + +# Add project root and ISOLATE paths +project_root = Path(__file__).parent.parent.parent +sys.path.append(str(project_root)) +sys.path.append(str(project_root / 'scripts' / 'ISOLATE')) + +try: + from scripts.ISOLATE.model_wrapper import ShenronRadarModel + print("βœ… Model Import: SUCCESS") +except Exception as e: + print(f"❌ Model Import: FAILED ({e})") + sys.exit(1) + +def verify_signal_flow(frame_idx=190): + print(f"\n--- πŸ›°οΈ Radar Lab Verification (Frame {frame_idx}) ---") + + # 1. Initialize Engine + try: + model = ShenronRadarModel(radar_type='awrl1432') + print(f"βœ… Engine Init: awrl1432 (Gain: {model.radar_obj.gain}dB)") + except Exception as e: + print(f"❌ Engine Init: FAILED ({e})") + return + + # 2. Load Real LiDAR Data + lidar_path = project_root / 'Shenron_debug' / 'logs' / 'lidar' / f"frame_{frame_idx:06d}.npy" + if not lidar_path.exists(): + print(f"❌ Lidar Data: NOT FOUND at {lidar_path}") + return + + data = np.load(lidar_path) + # Pad to 7-col [x,y,z,int,cos,obj,tag] if old 6-col format + if data.shape[1] == 6: + padded = np.zeros((data.shape[0], 7), dtype=np.float32) + padded[:, 0:3] = data[:, 0:3] + padded[:, 4:7] = data[:, 3:6] + data = padded + print(f"βœ… Lidar Data Loaded: {data.shape[0]} points") + + # 3. Process Frame + try: + pcd = model.process(data) + print(f"βœ… Signal Process: SUCCESS ({len(pcd)} detections)") + except Exception as e: + print(f"❌ Signal Process: FAILED ({e})") + return + + # 4. Verify Metrology Extraction + try: + met = model.get_last_metrology() + keys = met.keys() + expected = ["rd_heatmap", "ra_heatmap", "threshold_matrix"] + + status = True + for k in expected: + if k not in keys: + print(f"❌ Metrology Key Missing: {k}") + status = False + else: + val = met[k] + if np.max(val) <= 0: + print(f"❌ Metrology Data Null: {k} (Max: {np.max(val)})") + status = False + + if status: + print("βœ… Metrology Extraction: ALL KEYS VALID") + print(f" -> RD Heatmap: {met['rd_heatmap'].shape} (Peak Power: {np.max(met['rd_heatmap']):.2e})") + print(f" -> RA Heatmap: {met['ra_heatmap'].shape} (Peak Power: {np.max(met['ra_heatmap']):.2e})") + print(f" -> CFAR Threshold: {met['threshold_matrix'].shape} (Mean: {np.mean(met['threshold_matrix']):.2e})") + + except Exception as e: + print(f"❌ Metrology Extraction: FAILED ({e})") + + # 5. Verify Signal Metrics + try: + metrics = model.get_signal_metrics() + print(f"βœ… Signal Metrics: {metrics}") + if metrics['peak_snr_db'] <= 0: + print("⚠️ WARNING: Very low SNR detected (< 0dB). Check gain.") + except Exception as e: + print(f"❌ Signal Metrics: FAILED ({e})") + +if __name__ == "__main__": + verify_signal_flow(190) diff --git a/scripts/test_shenron.py b/scripts/test_shenron.py index 7487a76..29d2a29 100644 --- a/scripts/test_shenron.py +++ b/scripts/test_shenron.py @@ -6,6 +6,11 @@ from pathlib import Path import json import base64 import argparse +import matplotlib +import matplotlib.pyplot as plt +import matplotlib.cm as cm +import io +from PIL import Image from mcap.writer import Writer # Add project root and ISOLATE paths @@ -59,6 +64,43 @@ FOXGLOVE_PCL_SCHEMA = { "data": {"type": "string", "contentEncoding": "base64"} } } +FOXGLOVE_METRICS_SCHEMA = { + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "foxglove.Telemetry", + "title": "foxglove.Telemetry", + "type": "object", + "properties": { + "timestamp": {"type": "object", "properties": {"sec": {"type": "integer"}, "nsec": {"type": "integer"}}}, + "frame_id": {"type": "string"}, + "metrics": {"type": "object", "additionalProperties": {"type": "number"}} + } +} + +def render_heatmap(data, vmin=None, vmax=None, cmap='viridis'): + """Converts a 2D numpy array to a colormapped PNG base64 string.""" + if data is None or data.size == 0: + return None + + # Simple log scaling if needed? For now we assume input is power or magnitude + # Normalize to 0-1 + if vmin is None: vmin = np.min(data) + if vmax is None: vmax = np.max(data) + + if vmax > vmin: + norm_data = (data - vmin) / (vmax - vmin) + else: + norm_data = np.zeros_like(data) + + # Apply colormap (Updated to use modern matplotlib.colormaps API) + color_mapped = matplotlib.colormaps[cmap](norm_data) # [H, W, 4] + + # Convert to 8-bit RGB + rgb = (color_mapped[:, :, :3] * 255).astype(np.uint8) + + img = Image.fromarray(rgb) + buffered = io.BytesIO() + img.save(buffered, format="PNG") + return base64.b64encode(buffered.getvalue()).decode("ascii") def load_frames(folder_path): with open(os.path.join(folder_path, "frames.jsonl")) as f: @@ -95,6 +137,12 @@ def run_testbench(iter_name): print(f" -> Initializing {r_type} engine...") models[r_type] = ShenronRadarModel(radar_type=r_type) (iter_dir / r_type).mkdir(exist_ok=True) + + # Create Metrology folders + met_base = iter_dir / r_type / "metrology" + for sub in ["rd", "ra", "cfar"]: + (met_base / sub).mkdir(parents=True, exist_ok=True) + except Exception as e: print(f" -> [WARNING] Failed to init {r_type}: {e}") @@ -113,6 +161,20 @@ def run_testbench(iter_name): rich_pcd = model.process(data) out_path = iter_dir / r_type / lidar_file.name np.save(out_path, rich_pcd) + + # --- PHASES 1 & 3: Save Raw Metrology (.npy) --- + met = model.get_last_metrology() + if met: + frame_name = lidar_file.stem # e.g., frame_000200 + np.save(iter_dir / r_type / "metrology" / "rd" / f"{frame_name}.npy", met['rd_heatmap']) + np.save(iter_dir / r_type / "metrology" / "ra" / f"{frame_name}.npy", met['ra_heatmap']) + np.save(iter_dir / r_type / "metrology" / "cfar" / f"{frame_name}.npy", met['threshold_matrix']) + + # Log Metrics + metrics = model.get_signal_metrics() + with open(iter_dir / r_type / "metrology" / "metrics.jsonl", "a") as mf: + mf.write(json.dumps({"frame": frame_name, **metrics}) + "\n") + except Exception as e: print(f"[ERROR] Frame {lidar_file.name} failed for {r_type}: {e}") @@ -139,8 +201,17 @@ def run_testbench(iter_name): } shenron_channels = {} + metrology_channels = {} for r_type in radar_types: shenron_channels[r_type] = writer.register_channel(topic=f"/radar/{r_type}", message_encoding="json", schema_id=lidar_schema_id) + + # Register Metrology Channels + metrology_channels[r_type] = { + "rd": writer.register_channel(topic=f"/radar/{r_type}/heatmaps/range_doppler", message_encoding="json", schema_id=camera_schema_id), + "ra": writer.register_channel(topic=f"/radar/{r_type}/heatmaps/range_azimuth", message_encoding="json", schema_id=camera_schema_id), + "cfar": writer.register_channel(topic=f"/radar/{r_type}/heatmaps/cfar_mask", message_encoding="json", schema_id=camera_schema_id), + "telemetry": writer.register_channel(topic=f"/radar/{r_type}/metrics", message_encoding="json", schema_id=writer.register_schema(name="foxglove.Telemetry", encoding="jsonschema", data=json.dumps(FOXGLOVE_METRICS_SCHEMA).encode())) + } try: frames_gen = load_frames(logs_dir) @@ -258,6 +329,33 @@ def run_testbench(iter_name): } writer.add_message(shenron_channels[r_type], log_time=ts_ns, data=json.dumps(shenron_msg).encode(), publish_time=ts_ns) + # --- PHASE 2: Stream Metrology Visuals --- + met_folder = iter_dir / r_type / "metrology" + rd_p = met_folder / "rd" / shenron_fname + ra_p = met_folder / "ra" / shenron_fname + cf_p = met_folder / "cfar" / shenron_fname + + if rd_p.exists(): + rd_data = np.load(rd_p) + b64 = render_heatmap(np.log10(rd_data + 1e-9), cmap='viridis') + if b64: + msg = {"timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", "format": "png", "data": b64} + writer.add_message(metrology_channels[r_type]["rd"], log_time=ts_ns, data=json.dumps(msg).encode(), publish_time=ts_ns) + + if ra_p.exists(): + ra_data = np.load(ra_p) + b64 = render_heatmap(np.log10(ra_data + 1e-9), cmap='magma') # Magma for top-down contrast + if b64: + msg = {"timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", "format": "png", "data": b64} + writer.add_message(metrology_channels[r_type]["ra"], log_time=ts_ns, data=json.dumps(msg).encode(), publish_time=ts_ns) + + if cf_p.exists(): + cf_data = np.load(cf_p) + b64 = render_heatmap(np.log10(cf_data + 1e-9), cmap='plasma') # Plasma for threshold mask + if b64: + msg = {"timestamp": {"sec": ts_sec, "nsec": ts_nsec}, "frame_id": "ego_vehicle", "format": "png", "data": b64} + writer.add_message(metrology_channels[r_type]["cfar"], log_time=ts_ns, data=json.dumps(msg).encode(), publish_time=ts_ns) + writer.finish() print(f"\n[SUCCESS] Iteration packaged to: {output_mcap}") print(f"File size: {os.path.getsize(output_mcap)/1024/1024:.2f} MB")