diff --git a/examples/reachy2-remote/dataflow_reachy.yml b/examples/reachy2-remote/dataflow_reachy.yml index 069e8b24..e3ec824c 100644 --- a/examples/reachy2-remote/dataflow_reachy.yml +++ b/examples/reachy2-remote/dataflow_reachy.yml @@ -226,7 +226,7 @@ nodes: machine: encoder inputs: action_base: parse_point/action - action_whipser: parse_whisper/action + action_whisper: parse_whisper/action outputs: - response_base env: diff --git a/node-hub/dora-object-to-pose/src/lib.rs b/node-hub/dora-object-to-pose/src/lib.rs index f8f7e0f0..652dec78 100644 --- a/node-hub/dora-object-to-pose/src/lib.rs +++ b/node-hub/dora-object-to-pose/src/lib.rs @@ -1,7 +1,7 @@ use core::f32; use dora_node_api::{ arrow::{ - array::{AsArray, Float64Array, UInt16Array, UInt8Array}, + array::{AsArray, UInt16Array, UInt8Array}, datatypes::{Float32Type, Int64Type}, }, dora_core::config::DataId, @@ -11,50 +11,62 @@ use eyre::Result; use std::collections::HashMap; fn points_to_pose(points: &[(f32, f32, f32)]) -> Vec { - let (sum_x, sum_y, sum_z, sum_xy, sum_x2, sum_y2, n, x_min, x_max, y_min, y_max, z_min, z_max) = - points.iter().fold( + let ( + _sum_x, + _sum_y, + _sum_z, + sum_xy, + sum_x2, + sum_y2, + n, + x_min, + x_max, + y_min, + y_max, + z_min, + z_max, + ) = points.iter().fold( + ( + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, -10.0, 10.0, -10.0, 10., -10.0, + ), + |( + acc_x, + acc_y, + acc_z, + acc_xy, + acc_x2, + acc_y2, + acc_n, + acc_x_min, + acc_x_max, + acc_y_min, + acc_y_max, + acc_z_min, + acc_z_max, + ), + (x, y, z)| { ( - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, -10.0, 10.0, -10.0, 10., -10.0, - ), - |( - acc_x, - acc_y, - acc_z, - acc_xy, - acc_x2, - acc_y2, - acc_n, - acc_x_min, - acc_x_max, - acc_y_min, - acc_y_max, - acc_z_min, - acc_z_max, - ), - (x, y, z)| { - ( - acc_x + x, - acc_y + y, - acc_z + z, - acc_xy + x * y, - acc_x2 + x * x, - acc_y2 + y * y, - acc_n + 1., - f32::min(acc_x_min, *x), - f32::max(acc_x_max, *x), - f32::min(acc_y_min, *y), - f32::max(acc_y_max, *y), - f32::min(acc_z_min, *z), - f32::max(acc_z_max, *z), - ) - }, - ); + acc_x + x, + acc_y + y, + acc_z + z, + acc_xy + x * y, + acc_x2 + x * x, + acc_y2 + y * y, + acc_n + 1., + f32::min(acc_x_min, *x), + f32::max(acc_x_max, *x), + f32::min(acc_y_min, *y), + f32::max(acc_y_max, *y), + f32::min(acc_z_min, *z), + f32::max(acc_z_max, *z), + ) + }, + ); let (mean_x, mean_y, mean_z) = ( (x_max + x_min) / 2., (y_max + y_min) / 2., (z_max + z_min) / 2., ); - // Compute covariance and standard deviations let cov = sum_xy / n - mean_x * mean_y; @@ -139,8 +151,6 @@ pub fn lib_main() -> Result<()> { continue; }; - let mut z_2 = 0.0; - let mut z_1 = 0.0; let outputs: Vec> = masks .chunks(height as usize * width as usize) .filter_map(|data| { @@ -159,31 +169,19 @@ pub fn lib_main() -> Result<()> { if z == 0. || z > 20.0 { return; } - if z_2 == 0. && z_1 == 0. { - z_1 = z; - } else if z_1 == 0. { - z_2 = z_1; - z_1 = z; - } else if (z - z_2).abs() < 0.1 && (z - z_1).abs() < 0.1 { - z_2 = z_1; - z_1 = z; - if data[i] { - let y = (u - resolution[0] as f32) * z - / focal_length[0] as f32; - let x = (v - resolution[1] as f32) * z - / focal_length[1] as f32; - let new_x = sin_theta * z + cos_theta * x; - let new_y = -y; - let new_z = cos_theta * z - sin_theta * x; + if data[i] { + let y = (u - resolution[0] as f32) * z + / focal_length[0] as f32; + let x = (v - resolution[1] as f32) * z + / focal_length[1] as f32; + let new_x = sin_theta * z + cos_theta * x; + let new_y = -y; + let new_z = cos_theta * z - sin_theta * x; - points.push((new_x, new_y, new_z)); - z_total += new_z; - n += 1.; - } - } else { - z_2 = z_1; - z_1 = z; + points.push((new_x, new_y, new_z)); + z_total += new_z; + n += 1.; } } });