realsense-rust 1.3.0

//! This example demonstrates the colorizer processing block with Rerun visualization
//! It opens a depth stream and applies colorization, then visualizes the results in Rerun

use anyhow::{ensure, Result};
use realsense_rust::{
    config::Config,
    context::Context,
    frame::{DepthFrame, PixelKind},
    kind::{Rs2Format, Rs2StreamKind},
    pipeline::InactivePipeline,
    processing_blocks::colorizer::Colorizer,
};
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_colorizer").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());

    // 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 colorizer processing block
    let mut colorizer = Colorizer::new(5)?;

    println!("Starting depth colorization with Rerun visualization.");
    println!("The depth data will be colorized and displayed in Rerun.");
    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;

            // Log the raw depth data
            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("depth_raw", &rr::DepthImage::try_from(depth_image)?)?;

            // Apply colorizer
            colorizer.queue(depth_frame)?;

            // Wait for colorized frame
            match colorizer.wait(Duration::from_millis(100)) {
                Ok(colorized_frame) => {
                    // Convert colorized frame to RGB data 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]);
                            }
                            _ => {
                                println!("Unexpected pixel format in colorized frame: {:?}", pixel);
                                // Fallback for other formats
                                rgb_data.extend_from_slice(&[0, 0, 0]);
                            }
                        }
                    }

                    // Create Rerun image from raw RGB data
                    let width = colorized_frame.width() as u32;
                    let height = colorized_frame.height() as u32;

                    rec.log(
                        "depth_colorized",
                        &rr::Image::from_rgb24(rgb_data, [width, height]),
                    )?;

                    if frame_count % 60 == 0 {
                        println!("Processed {} frames", frame_count);
                    }
                }
                Err(e) => {
                    eprintln!("Error processing colorized frame: {}", e);
                }
            }
        }
    }
}