1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
//! This example demonstrates the align processing block with Rerun visualization
//! It aligns depth and color streams and visualizes the alignment results
use anyhow::{ensure, Result};
use realsense_rust::{
config::Config,
context::Context,
frame::{ColorFrame, DepthFrame, PixelKind},
kind::{Rs2Format, Rs2StreamKind},
pipeline::InactivePipeline,
processing_blocks::align::Align,
};
use rerun::{self as rr};
use std::{collections::HashSet, convert::TryFrom, time::Duration};
fn main() -> Result<()> {
// Initialize Rerun
let rec = rr::RecordingStreamBuilder::new("realsense_align").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();
// Enable both depth and color streams
config.enable_stream(Rs2StreamKind::Depth, Some(0), 640, 480, Rs2Format::Z16, 30)?;
config.enable_stream(Rs2StreamKind::Color, Some(0), 640, 480, Rs2Format::Rgb8, 30)?;
// Start pipeline
let mut active_pipeline = pipeline.start(Some(config))?;
// Create align processing block - align depth to color
let mut align = Align::new(Rs2StreamKind::Color, 10)?;
println!("Starting alignment processing with Rerun visualization.");
println!("Aligning depth stream to color stream coordinates.");
println!("Press Ctrl+C to quit.");
let mut frame_count = 0;
loop {
let frames = active_pipeline.wait(Some(Duration::from_millis(1000)))?;
frame_count += 1;
// Apply alignment processing
align.queue(frames)?;
// Wait for aligned frames
match align.wait(Duration::from_millis(100)) {
Ok(aligned_frames) => {
// Get aligned depth and color frames
let aligned_depth_frames: Vec<DepthFrame> = aligned_frames.frames_of_type();
let aligned_color_frames: Vec<ColorFrame> = aligned_frames.frames_of_type();
// Create blended frame if we have both depth and color
if let (Some(aligned_depth), Some(aligned_color)) =
(aligned_depth_frames.first(), aligned_color_frames.first())
{
// Ensure both frames have the same dimensions
let width = aligned_depth.width() as u32;
let height = aligned_depth.height() as u32;
if width == aligned_color.width() as u32
&& height == aligned_color.height() as u32
{
// Create depth data vector
let depth_data: Vec<u16> = aligned_depth
.iter()
.map(|pixel| match pixel {
PixelKind::Z16 { depth } => *depth,
_ => 0,
})
.collect();
// Create color data vector
let mut color_data = Vec::new();
for pixel in aligned_color.iter() {
match pixel {
PixelKind::Rgb8 { r, g, b } => {
color_data.push([*r, *g, *b]);
}
PixelKind::Bgr8 { b, g, r } => {
color_data.push([*r, *g, *b]);
}
_ => {
color_data.push([0, 0, 0]);
}
}
}
// Create blended RGB data
let mut blended_rgb = Vec::new();
for (i, &depth_value) in depth_data.iter().enumerate() {
if i < color_data.len() {
let color_pixel = color_data[i];
if depth_value > 0 {
// Normalize depth to 0-1 range (assuming max depth ~5000mm)
let depth_normalized = (depth_value as f32 / 5000.0).min(1.0);
let alpha = 0.3; // Blend factor for depth overlay
// Create depth color (blue to red gradient)
let depth_r = (depth_normalized * 255.0) as u8;
let depth_g =
((1.0 - depth_normalized) * depth_normalized * 4.0 * 255.0)
as u8;
let depth_b = ((1.0 - depth_normalized) * 255.0) as u8;
// Blend color and depth
let blended_r = ((1.0 - alpha) * color_pixel[0] as f32
+ alpha * depth_r as f32)
as u8;
let blended_g = ((1.0 - alpha) * color_pixel[1] as f32
+ alpha * depth_g as f32)
as u8;
let blended_b = ((1.0 - alpha) * color_pixel[2] as f32
+ alpha * depth_b as f32)
as u8;
blended_rgb
.extend_from_slice(&[blended_r, blended_g, blended_b]);
} else {
// No depth data, use original color
blended_rgb.extend_from_slice(&color_pixel);
}
} else {
blended_rgb.extend_from_slice(&[0, 0, 0]);
}
}
// Log the blended frame
rec.log(
"frames/depth_color_blend",
&rr::Image::from_rgb24(blended_rgb, [width, height]),
)?;
if frame_count % 60 == 0 {
println!(
"Processed {} frames - Blended aligned frames: {}x{}",
frame_count, width, height
);
}
} else {
eprintln!(
"Frame size mismatch - Depth: {}x{}, Color: {}x{}",
aligned_depth.width(),
aligned_depth.height(),
aligned_color.width(),
aligned_color.height()
);
}
}
}
Err(e) => {
eprintln!("Error processing aligned frames: {}", e);
}
}
}
}