realsense-rust 1.3.0

//! This example demonstrates how to configure processing block options
//! It shows various processing blocks with different configuration options

use anyhow::{ensure, Result};
use realsense_rust::{
    config::Config,
    context::Context,
    frame::{DepthFrame, PixelKind},
    kind::{Rs2Format, Rs2StreamKind},
    pipeline::InactivePipeline,
    processing_blocks::{
        colorizer::Colorizer, decimation::Decimation, hole_filling::HoleFillingFilter, options::*,
        spatial_filter::SpatialFilter, temporal_filter::TemporalFilter, threshold::ThresholdFilter,
    },
};
use rerun::{self as rr, external::image};
use std::{collections::HashSet, convert::TryFrom, time::Duration};

fn main() -> Result<()> {
    // Initialize Rerun
    let rec = rr::RecordingStreamBuilder::new("realsense_processing_options").spawn()?;

    // Create RealSense context and check for available devices
    let context = Context::new()?;
    let devices = context.query_devices(HashSet::new());
    ensure!(!devices.is_empty(), "No devices found");

    println!("Found {} device(s), using the first one", devices.len());
    println!("This example demonstrates configurable processing block options.");

    // Create pipeline and configuration
    let pipeline = InactivePipeline::try_from(&context)?;
    let mut config = Config::new();
    config.enable_stream(Rs2StreamKind::Depth, Some(0), 640, 480, Rs2Format::Z16, 30)?;

    // Start pipeline
    let mut active_pipeline = pipeline.start(Some(config))?;

    // Create processing blocks with custom configurations
    println!("\nConfiguring processing blocks with custom options...");

    // 1. Decimation Filter - reduce resolution
    let mut decimation = Decimation::new(5)?;
    let decimation_opts = DecimationOptions {
        filter_magnitude: Some(4.0), // 4x decimation (640x480 -> 160x120)
    };
    decimation.apply_options(&decimation_opts)?;
    println!("✓ Decimation: 4x magnitude reduction");

    // 2. Spatial Filter - noise reduction with aggressive settings
    let mut spatial_filter = SpatialFilter::new(5)?;
    let spatial_opts = SpatialFilterOptions {
        smooth_alpha: Some(0.8),  // High smoothing
        smooth_delta: Some(10.0), // Moderate edge preservation
        magnitude: Some(3.0),     // Strong filtering
        holes_fill: Some(2.0),    // Fill holes aggressively
    };
    spatial_filter.apply_options(&spatial_opts)?;
    println!("✓ Spatial Filter: Aggressive noise reduction");

    // 3. Temporal Filter - reduce temporal noise
    let mut temporal_filter = TemporalFilter::new(5)?;
    let temporal_opts = TemporalFilterOptions {
        smooth_alpha: Some(0.4),        // Moderate temporal smoothing
        smooth_delta: Some(20.0),       // Medium sensitivity
        persistence_control: Some(3.0), // Medium persistence
    };
    temporal_filter.apply_options(&temporal_opts)?;
    println!("✓ Temporal Filter: Moderate temporal smoothing");

    // 4. Hole Filling Filter
    let mut hole_filling = HoleFillingFilter::new(5)?;
    let hole_filling_opts = HoleFillingOptions {
        holes_fill: Some(2.0), // Nearest from around
    };
    hole_filling.apply_options(&hole_filling_opts)?;
    println!("✓ Hole Filling: Nearest neighbor interpolation");

    // 5. Threshold Filter - limit depth range
    let mut threshold_filter = ThresholdFilter::new(5)?;
    let threshold_opts = ThresholdOptions {
        min_distance: Some(0.3), // 30cm minimum
        max_distance: Some(3.0), // 3m maximum
    };
    threshold_filter.apply_options(&threshold_opts)?;
    println!("✓ Threshold Filter: 0.3m - 3.0m range");

    // 6. Colorizer - custom color scheme
    let mut colorizer = Colorizer::new(5)?;
    let colorizer_opts = ColorizerOptions {
        color_scheme: Some(2.0),           // Jet color scheme
        histogram_equalization: Some(1.0), // Enable histogram equalization
        min_distance: Some(0.0),           // Auto-range min
        max_distance: Some(4.0),           // 4m max for color mapping
    };
    colorizer.apply_options(&colorizer_opts)?;
    println!("✓ Colorizer: Jet color scheme with histogram equalization");

    println!("\nStarting processing pipeline...");
    println!("Processing: Raw -> Decimation -> Spatial -> Temporal -> Hole Fill -> Threshold -> Colorize");
    println!("Press Ctrl+C to quit.");

    let mut frame_count = 0;

    loop {
        let frames = active_pipeline.wait(Some(Duration::from_millis(1000)))?;

        // Get depth frame
        let depth_frames: Vec<DepthFrame> = frames.frames_of_type();
        if let Some(depth_frame) = depth_frames.into_iter().next() {
            frame_count += 1;

            let depth_data: Vec<u16> = depth_frame
                .iter()
                .map(|pixel| match pixel {
                    PixelKind::Z16 { depth } => *depth,
                    _ => 0,
                })
                .collect();

            let depth_image: image::ImageBuffer<image::Luma<u16>, Vec<u16>> =
                image::ImageBuffer::from_raw(
                    depth_frame.width() as u32,
                    depth_frame.height() as u32,
                    depth_data,
                )
                .unwrap();

            rec.log(
                "pipeline/00_original",
                &rr::DepthImage::try_from(depth_image)?,
            )?;

            // Apply processing pipeline
            decimation.queue(depth_frame)?;
            let frame = decimation.wait(Duration::from_millis(100))?;

            // Log after decimation

            let depth_data: Vec<u16> = frame
                .iter()
                .map(|pixel| match pixel {
                    PixelKind::Z16 { depth } => *depth,
                    _ => 0,
                })
                .collect();

            let depth_image: image::ImageBuffer<image::Luma<u16>, Vec<u16>> =
                image::ImageBuffer::from_raw(
                    frame.width() as u32,
                    frame.height() as u32,
                    depth_data,
                )
                .unwrap();

            rec.log(
                "pipeline/01_decimated",
                &rr::DepthImage::try_from(depth_image)?,
            )?;

            spatial_filter.queue(frame)?;
            let frame = spatial_filter.wait(Duration::from_millis(100))?;

            temporal_filter.queue(frame)?;
            let frame = temporal_filter.wait(Duration::from_millis(100))?;

            hole_filling.queue(frame)?;
            let frame = hole_filling.wait(Duration::from_millis(100))?;

            threshold_filter.queue(frame)?;
            let frame = threshold_filter.wait(Duration::from_millis(100))?;

            // Log after filtering

            let depth_data: Vec<u16> = frame
                .iter()
                .map(|pixel| match pixel {
                    PixelKind::Z16 { depth } => *depth,
                    _ => 0,
                })
                .collect();

            let depth_image: image::ImageBuffer<image::Luma<u16>, Vec<u16>> =
                image::ImageBuffer::from_raw(
                    frame.width() as u32,
                    frame.height() as u32,
                    depth_data,
                )
                .unwrap();

            rec.log(
                "pipeline/02_filtered",
                &rr::DepthImage::try_from(depth_image)?,
            )?;

            colorizer.queue(frame)?;
            let colorized_frame = colorizer.wait(Duration::from_millis(100))?;

            // Convert colorized frame to RGB for Rerun
            let mut rgb_data = Vec::new();
            for pixel in colorized_frame.iter() {
                match pixel {
                    PixelKind::Bgr8 { b, g, r } => {
                        rgb_data.extend_from_slice(&[*r, *g, *b]);
                    }
                    PixelKind::Rgb8 { r, g, b } => {
                        rgb_data.extend_from_slice(&[*r, *g, *b]);
                    }
                    _ => {
                        rgb_data.extend_from_slice(&[0, 0, 0]);
                    }
                }
            }

            rec.log(
                "pipeline/03_final_colorized",
                &rr::Image::from_rgb24(
                    rgb_data,
                    [
                        colorized_frame.width() as u32,
                        colorized_frame.height() as u32,
                    ],
                ),
            )?;

            if frame_count % 60 == 0 {
                println!(
                    "Processed {} frames - Final resolution: {}x{}",
                    frame_count,
                    colorized_frame.width(),
                    colorized_frame.height()
                );
            }
        }
    }
}