1use crate::{DEPTH_SHAPE, INPUT_HEIGHT, INPUT_WIDTH, VitFly, VitFlyState};
2use candle_core::{Device, Tensor};
3use cu_ahrs::AhrsPose;
4use cu_zed::ZedDepthMap;
5use cu29::bincode::{Decode, Encode};
6use cu29::prelude::*;
7use cu29::units::si::angle::radian;
8use cu29::units::si::f32::Velocity;
9use cu29::units::si::length::meter;
10use cu29::units::si::velocity::meter_per_second;
11
12pub type VitFlyVelocity = [Velocity; 3];
14
15const DEFAULT_MAX_DEPTH_M: f32 = 12.5;
16const DEFAULT_INVALID_DEPTH: f32 = 0.8;
17const RECURRENT_VALUES: usize = 3 * 128;
18
19#[derive(Reflect)]
25#[reflect(from_reflect = false)]
26pub struct VitFlyTask {
27 #[reflect(ignore)]
28 model: VitFly,
29 #[reflect(ignore)]
30 device: Device,
31 #[reflect(ignore)]
32 recurrent: Option<VitFlyState>,
33 #[reflect(ignore)]
34 resized_depth: Vec<f32>,
35 max_depth_m: f32,
36 invalid_depth: f32,
37}
38
39impl VitFlyTask {
40 fn from_config(config: Option<&ComponentConfig>) -> CuResult<Self> {
41 let max_depth_m = config_f32(config, "max_depth_m", DEFAULT_MAX_DEPTH_M)?;
42 if !max_depth_m.is_finite() || max_depth_m <= 0.0 {
43 return Err(CuError::from(
44 "vitfly max_depth_m must be finite and positive",
45 ));
46 }
47 let invalid_depth = config_f32(config, "invalid_depth", DEFAULT_INVALID_DEPTH)?;
48 if !invalid_depth.is_finite() {
49 return Err(CuError::from("vitfly invalid_depth must be finite"));
50 }
51
52 let requested_device = config
53 .map(|cfg| cfg.get::<String>("device"))
54 .transpose()?
55 .flatten();
56 let cuda_ordinal = config
57 .map(|cfg| cfg.get::<u32>("cuda_ordinal"))
58 .transpose()?
59 .flatten()
60 .unwrap_or(0) as usize;
61 let device = select_device(requested_device.as_deref(), cuda_ordinal)?;
62 let model = VitFly::load(&device)
63 .map_err(|err| CuError::new_with_cause("failed to load ViTFly model", err))?;
64
65 Ok(Self {
66 model,
67 device,
68 recurrent: None,
69 resized_depth: vec![0.0; INPUT_HEIGHT * INPUT_WIDTH],
70 max_depth_m,
71 invalid_depth,
72 })
73 }
74
75 fn run_model(
76 &mut self,
77 depth: &ZedDepthMap,
78 pose: &AhrsPose,
79 desired_speed: Velocity,
80 ) -> CuResult<VitFlyVelocity> {
81 let format = depth.format;
82 if format.width == 0
83 || format.height == 0
84 || !format.is_valid()
85 || format.required_elements() == 0
86 {
87 return Err(CuError::from(
88 "vitfly received an invalid ZED raster format",
89 ));
90 }
91
92 depth.with_samples(|samples, format| {
93 resize_and_normalize_depth(
94 samples,
95 format.width as usize,
96 format.height as usize,
97 format.stride as usize,
98 self.max_depth_m,
99 self.invalid_depth,
100 &mut self.resized_depth,
101 );
102 });
103
104 let speed_mps = desired_speed.get::<meter_per_second>();
105 if !speed_mps.is_finite() || speed_mps < 0.0 {
106 return Err(CuError::from(
107 "vitfly desired velocity must be finite and non-negative",
108 ));
109 }
110
111 let quaternion = euler_to_scalar_first_quaternion(
112 pose.roll.get::<radian>(),
113 pose.pitch.get::<radian>(),
114 pose.yaw.get::<radian>(),
115 );
116 let depth = Tensor::from_slice(&self.resized_depth, &DEPTH_SHAPE, &self.device)
117 .map_err(|err| CuError::new_with_cause("failed to upload ViTFly depth", err))?;
118 let desired_velocity = Tensor::new(&[[speed_mps]], &self.device)
119 .map_err(|err| CuError::new_with_cause("failed to upload ViTFly speed", err))?;
120 let attitude = Tensor::new(&[quaternion], &self.device)
121 .map_err(|err| CuError::new_with_cause("failed to upload ViTFly attitude", err))?;
122 let (prediction, recurrent) = self
123 .model
124 .forward(
125 &depth,
126 &desired_velocity,
127 &attitude,
128 self.recurrent.as_ref(),
129 )
130 .map_err(|err| CuError::new_with_cause("ViTFly inference failed", err))?;
131 let prediction = prediction
132 .to_vec2::<f32>()
133 .map_err(|err| CuError::new_with_cause("failed to download ViTFly output", err))?;
134 self.recurrent = Some(recurrent);
135
136 let [forward, left, up]: [f32; 3] = prediction
137 .first()
138 .and_then(|row| row.as_slice().try_into().ok())
139 .ok_or_else(|| CuError::from("ViTFly returned an invalid output shape"))?;
140 Ok([forward, left, up]
141 .map(|component| Velocity::new::<meter_per_second>(component * speed_mps)))
142 }
143}
144
145impl Freezable for VitFlyTask {
146 fn freeze<E: cu29::bincode::enc::Encoder>(
147 &self,
148 encoder: &mut E,
149 ) -> Result<(), cu29::bincode::error::EncodeError> {
150 let snapshot = self
151 .recurrent
152 .as_ref()
153 .map(|state| {
154 let hidden = state.hidden_tensor()?.flatten_all()?.to_vec1::<f32>()?;
155 let cell = state.cell_tensor()?.flatten_all()?.to_vec1::<f32>()?;
156 candle_core::Result::Ok((hidden, cell))
157 })
158 .transpose()
159 .map_err(|err| cu29::bincode::error::EncodeError::OtherString(err.to_string()))?;
160 Encode::encode(&snapshot, encoder)
161 }
162
163 fn thaw<D: cu29::bincode::de::Decoder>(
164 &mut self,
165 decoder: &mut D,
166 ) -> Result<(), cu29::bincode::error::DecodeError> {
167 let snapshot: Option<(Vec<f32>, Vec<f32>)> = Decode::decode(decoder)?;
168 self.recurrent = snapshot
169 .map(|(hidden, cell)| {
170 if hidden.len() != RECURRENT_VALUES || cell.len() != RECURRENT_VALUES {
171 return Err(cu29::bincode::error::DecodeError::Other(
172 "invalid ViTFly recurrent state length",
173 ));
174 }
175 let hidden = Tensor::from_vec(hidden, (3, 128), &self.device)
176 .map_err(candle_decode_error)?;
177 let cell =
178 Tensor::from_vec(cell, (3, 128), &self.device).map_err(candle_decode_error)?;
179 VitFlyState::from_tensors(&hidden, &cell).map_err(candle_decode_error)
180 })
181 .transpose()?;
182 Ok(())
183 }
184}
185
186impl CuTask for VitFlyTask {
187 type Resources<'r> = ();
188 type Input<'m> = input_msg!('m, ZedDepthMap, AhrsPose, Velocity);
189 type Output<'m> = output_msg!(VitFlyVelocity);
190
191 fn new(config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self> {
192 Self::from_config(config)
193 }
194
195 fn process(
196 &mut self,
197 _ctx: &CuContext,
198 input: &Self::Input<'_>,
199 output: &mut Self::Output<'_>,
200 ) -> CuResult<()> {
201 let (depth_msg, pose_msg, speed_msg) = *input;
202 output.tov = depth_msg.tov;
203 let (Some(depth), Some(pose), Some(speed)) = (
204 depth_msg.payload(),
205 pose_msg.payload(),
206 speed_msg.payload().copied(),
207 ) else {
208 output.clear_payload();
209 output.metadata.set_status("missing input");
210 return Ok(());
211 };
212
213 output.set_payload(self.run_model(depth, pose, speed)?);
214 output.metadata.set_status("ok");
215 Ok(())
216 }
217}
218
219fn select_device(requested: Option<&str>, cuda_ordinal: usize) -> CuResult<Device> {
220 let requested = requested.unwrap_or(if cfg!(feature = "cuda") {
221 "cuda"
222 } else {
223 "cpu"
224 });
225 match requested {
226 "cpu" => Ok(Device::Cpu),
227 "cuda" => {
228 #[cfg(feature = "cuda")]
229 {
230 Device::new_cuda(cuda_ordinal)
231 .map_err(|err| CuError::new_with_cause("failed to initialize CUDA", err))
232 }
233 #[cfg(not(feature = "cuda"))]
234 {
235 let _ = cuda_ordinal;
236 Err(CuError::from(
237 "vitfly device=cuda requires the cu-vitfly cuda feature",
238 ))
239 }
240 }
241 _ => Err(CuError::from("vitfly device must be either cpu or cuda")),
242 }
243}
244
245fn config_f32(config: Option<&ComponentConfig>, key: &str, default: f32) -> CuResult<f32> {
246 Ok(config
247 .map(|cfg| cfg.get::<f32>(key))
248 .transpose()?
249 .flatten()
250 .unwrap_or(default))
251}
252
253fn candle_decode_error(err: candle_core::Error) -> cu29::bincode::error::DecodeError {
254 cu29::bincode::error::DecodeError::OtherString(err.to_string())
255}
256
257fn normalize_depth(sample: u16, max_depth_m: f32, invalid_depth: f32) -> f32 {
258 ZedDepthMap::decode_sample(sample)
259 .map(|depth| (depth.get::<meter>() / max_depth_m).clamp(0.0, 1.0))
260 .unwrap_or(invalid_depth)
261}
262
263#[allow(clippy::too_many_arguments)]
264fn resize_and_normalize_depth(
265 source: &[u16],
266 source_width: usize,
267 source_height: usize,
268 source_stride: usize,
269 max_depth_m: f32,
270 invalid_depth: f32,
271 destination: &mut [f32],
272) {
273 debug_assert_eq!(destination.len(), INPUT_HEIGHT * INPUT_WIDTH);
274 let scale_x = source_width as f32 / INPUT_WIDTH as f32;
275 let scale_y = source_height as f32 / INPUT_HEIGHT as f32;
276
277 for output_y in 0..INPUT_HEIGHT {
278 let source_y =
279 ((output_y as f32 + 0.5) * scale_y - 0.5).clamp(0.0, (source_height - 1) as f32);
280 let y0 = source_y.floor() as usize;
281 let y1 = (y0 + 1).min(source_height - 1);
282 let wy = source_y - y0 as f32;
283 for output_x in 0..INPUT_WIDTH {
284 let source_x =
285 ((output_x as f32 + 0.5) * scale_x - 0.5).clamp(0.0, (source_width - 1) as f32);
286 let x0 = source_x.floor() as usize;
287 let x1 = (x0 + 1).min(source_width - 1);
288 let wx = source_x - x0 as f32;
289 let top_left =
290 normalize_depth(source[y0 * source_stride + x0], max_depth_m, invalid_depth);
291 let top_right =
292 normalize_depth(source[y0 * source_stride + x1], max_depth_m, invalid_depth);
293 let bottom_left =
294 normalize_depth(source[y1 * source_stride + x0], max_depth_m, invalid_depth);
295 let bottom_right =
296 normalize_depth(source[y1 * source_stride + x1], max_depth_m, invalid_depth);
297 let top = top_left * (1.0 - wx) + top_right * wx;
298 let bottom = bottom_left * (1.0 - wx) + bottom_right * wx;
299 destination[output_y * INPUT_WIDTH + output_x] = top * (1.0 - wy) + bottom * wy;
300 }
301 }
302}
303
304fn euler_to_scalar_first_quaternion(roll: f32, pitch: f32, yaw: f32) -> [f32; 4] {
305 let (sr, cr) = (0.5 * roll).sin_cos();
306 let (sp, cp) = (0.5 * pitch).sin_cos();
307 let (sy, cy) = (0.5 * yaw).sin_cos();
308 [
309 cr * cp * cy + sr * sp * sy,
310 sr * cp * cy - cr * sp * sy,
311 cr * sp * cy + sr * cp * sy,
312 cr * cp * sy - sr * sp * cy,
313 ]
314}
315
316#[cfg(test)]
317mod tests {
318 use super::*;
319
320 #[test]
321 fn identity_pose_maps_to_scalar_first_identity() {
322 assert_eq!(
323 euler_to_scalar_first_quaternion(0.0, 0.0, 0.0),
324 [1.0, 0.0, 0.0, 0.0]
325 );
326 }
327
328 #[test]
329 fn resize_respects_stride_and_normalizes_meters() {
330 let mut source = vec![u16::MAX; 6 * 2];
331 source[..4].copy_from_slice(&[0, 6_250, 12_500, 25_000]);
332 source[6..10].copy_from_slice(&[0, 6_250, 12_500, 25_000]);
333 let mut destination = vec![0.0; INPUT_HEIGHT * INPUT_WIDTH];
334 resize_and_normalize_depth(&source, 4, 2, 6, 12.5, 0.8, &mut destination);
335 assert_eq!(destination[0], 0.8);
336 assert!((destination[INPUT_WIDTH - 1] - 1.0).abs() < 1.0e-6);
337 }
338}