Add an Iridescence live viewer alongside RVIZ with support for performance metrics.

[DethCubeHax]

Overview

This PR adds an optional Iridescence live viewer (fastlivo_viewer) besides the vanilla RVIZ viewer.

The viewer shows the registered cloud in RGB or as an XYZ heatmap, places the camera image in the HUD, and plots LIO/VIO timings, residual, and EKF 1-sigma from quantities the estimator already produces. On a GTX 1650 Ti, over 180 s on the provided HKisland03 dataset, visualizer CPU was about 55% lower and RAM about 65% lower than stock RViz.

Install steps are in README section 2.3.

Changes

viewer:=true starts fastlivo_viewer. The viewer draws /cloud_registered in 50k-point chunks, which keeps draw cost bounded as the map grows. Colour is RGB when the cloud carries colour, or a turbo heatmap on X, Y, or Z whose range is set by Tukey inliers, so the scale follows the bulk of the samples. Green /path and pose axes can follow the vehicle, with orbit as left drag to rotate, middle or right to pan, and scroll to zoom. /rgb_img sits in the HUD under the host stats, with a scale slider. CPU, GPU, GPU memory, host memory, and FPS are shown as numbers, and a plots window holds LIO/VIO time, residual, feature counts, and position / rotation / velocity 1-sigma. Those series are published on /fast_livo/diagnostics after every LIO and VIO update.

Task	Control
Stock RViz	default roslaunch
Iridescence	viewer:=true
Coloured reconstruction	RGB
Height or axis extent	Heatmap on X, Y, or Z
Follow the pose and orbit	Track on; mouse as above
LIO/VIO cost while mapping	Plots window
Estimator 1-sigma already in the EKF	Position / rotation / velocity series
Headless mapping	rviz:=false

Screenshots

RGB:

Heatmap:

Comparison

Factor	Stock RViz (default)	Iridescence (viewer:=true)
Visualizer	nice rviz -d official .rviz	fastlivo_viewer
Launch flag	rviz:=true (default)	viewer:=true
Map colour	PointCloud2 settings	RGB or XYZ heatmap, axis selectable
Heatmap range	Display min/max	Tukey inliers, then min/max
Trajectory	Path display	Green path, toggle
Camera image	Image display	HUD under host stats, scale slider
Timings / residual / features	Terminal tables	Resizable plots
Path noise	—	EKF 1-sigma from existing covariance
Host stats	—	CPU, GPU, mem, FPS
Camera	RViz orbit / FPS / XY	Orbit plus optional track
Headless	rviz:=false	rviz:=false
Graphics	RViz / Ogre	OpenGL 3.3 (Iridescence)

Below is a comparison between the two visualizers, and both used the same bag and hover skip (rosbag play --clock -s 72 --duration 180). Both runs were Docker --gpus all on a GTX 1650 Ti (driver 595.84), one run each after the bag cache was already warm.

System Specifications

Item	Spec
Machine	Lenovo Legion 5i
OS	Ubuntu 22.04.5 LTS, Linux 6.8.0-138-generic x86_64
CPU	Intel Core i5-10300H, 4 cores / 8 threads, 2.50–4.50 GHz, 8 MB L3
Memory	64 GB
GPU	NVIDIA GeForce GTX 1650 Ti, 4 GB, compute 7.5, graphics clock to 2100 MHz
Driver / CUDA	595.84 / 13.2
Docker	29.1.3, NVIDIA Container Toolkit 1.20.0

Results

Metric	Iridescence	Stock RViz	vs stock
Viz CPU mean / max (%)	15.5 / 20.7	34.4 / 51.2	−55% mean
Viz RSS mean / max (MB)	563 / 873	1589 / 2753	−65% mean
Viz FPS mean / min / max	50.6 / 25.0 / 75.4	61.6 / 41.5 / 63.9	−18% mean
LIO average, last (s)	0.0776	0.0811	−4%
LIO / VIO updates	1689 / 1689	1593 / 1593	+6%
Mapping CPU mean (%)	291	287	+1%
GPU util mean (%)	64.3	39.2	+64%
GPU mem mean (MB)	934	746	+25%

Please note that although the FPS dropped by 18% in case of Iridescence, chances are it's because my laptop is about 6 years old and running at 87°C lol, it's barely holding up these days. The results should be reproduceable on any system to validate.

[DethCubeHax]

Please feel free to suggest anything :))

I just added a few things in the diagnostics that I thought would help analyze things like potential drift, timings and performance.