From 82c5ee21078059025eb76b14704fbb90ce01c345 Mon Sep 17 00:00:00 2001 From: haixuantao Date: Fri, 4 Apr 2025 15:19:16 +0200 Subject: [PATCH] Make camera pitch parametrisation easier --- .../dora-ios-lidar/dora_ios_lidar/main.py | 101 +++++++++++------- node-hub/dora-rerun/src/lib.rs | 18 ++-- 2 files changed, 73 insertions(+), 46 deletions(-) diff --git a/node-hub/dora-ios-lidar/dora_ios_lidar/main.py b/node-hub/dora-ios-lidar/dora_ios_lidar/main.py index af6c9382..2615e61b 100644 --- a/node-hub/dora-ios-lidar/dora_ios_lidar/main.py +++ b/node-hub/dora-ios-lidar/dora_ios_lidar/main.py @@ -7,6 +7,7 @@ import numpy as np import pyarrow as pa from dora import Node from record3d import Record3DStream +from scipy.spatial.transform import Rotation class DemoApp: @@ -54,6 +55,21 @@ class DemoApp: [[coeffs.fx, 0, coeffs.tx], [0, coeffs.fy, coeffs.ty], [0, 0, 1]], ) + def get_camera_pose(self): + """Get Camera Pose.""" + pose = self.session.get_camera_pose() + rot = Rotation.from_quat([pose.qx, pose.qy, pose.qz, pose.qw]) + euler = rot.as_euler("xyz", degrees=False) + + return [ + pose.tx, + pose.ty, + pose.tz, + pose.qx, + euler[1], + euler[2], + ] + def start_processing_stream(self): """TODO: Add docstring.""" node = Node() @@ -61,47 +77,52 @@ class DemoApp: for event in node: if self.stop: break + if event["type"] == "INPUT": - if event["id"] == "TICK": - self.event.wait() # Wait for new frame to arrive - - # Copy the newly arrived RGBD frame - depth = self.session.get_depth_frame() - rgb = self.session.get_rgb_frame() - intrinsic_mat = self.get_intrinsic_mat_from_coeffs( - self.session.get_intrinsic_mat(), - ) - - if depth.shape != rgb.shape: - rgb = cv2.resize(rgb, (depth.shape[1], depth.shape[0])) - - node.send_output( - "image", - pa.array(rgb.ravel()), - metadata={ - "encoding": "rgb8", - "width": rgb.shape[1], - "height": rgb.shape[0], - }, - ) - - node.send_output( - "depth", - pa.array(depth.ravel().astype(np.float64())), - metadata={ - "width": depth.shape[1], - "height": depth.shape[0], - "encoding": "CV_64F", - "focal": [ - int(intrinsic_mat[0, 0]), - int(intrinsic_mat[1, 1]), - ], - "resolution": [ - int(intrinsic_mat[0, 2]), - int(intrinsic_mat[1, 2]), - ], - }, - ) + self.event.wait() # Wait for new frame to arrive + + # Copy the newly arrived RGBD frame + depth = self.session.get_depth_frame() + rgb = self.session.get_rgb_frame() + intrinsic_mat = self.get_intrinsic_mat_from_coeffs( + self.session.get_intrinsic_mat(), + ) + pose = self.get_camera_pose() + + if depth.shape != rgb.shape: + rgb = cv2.resize(rgb, (depth.shape[1], depth.shape[0])) + node.send_output( + "image", + pa.array(rgb.ravel()), + metadata={ + "encoding": "rgb8", + "width": rgb.shape[1], + "height": rgb.shape[0], + }, + ) + + depth = (np.array(depth) * 1_000).astype(np.uint16) + + node.send_output( + "depth", + pa.array(depth.ravel()), + metadata={ + "width": depth.shape[1], + "height": depth.shape[0], + "encoding": "mono16", + "focal": [ + int(intrinsic_mat[0, 0]), + int(intrinsic_mat[1, 1]), + ], + "resolution": [ + int(intrinsic_mat[0, 2]), + int(intrinsic_mat[1, 2]), + ], + "roll": pose[3], + "pitch": pose[4], + "yaw": pose[5], + }, + ) self.event.clear() diff --git a/node-hub/dora-rerun/src/lib.rs b/node-hub/dora-rerun/src/lib.rs index 46ce4fd1..c482e718 100644 --- a/node-hub/dora-rerun/src/lib.rs +++ b/node-hub/dora-rerun/src/lib.rs @@ -95,12 +95,9 @@ pub fn lib_main() -> Result<()> { Err(VarError::NotPresent) => (), }; let camera_pitch = std::env::var("CAMERA_PITCH") - .unwrap_or("2.47".to_string()) + .unwrap_or("0.0".to_string()) .parse::() .unwrap(); - let cos_theta = camera_pitch.cos(); // np.cos(np.deg2rad(180-38)) - let sin_theta = camera_pitch.sin(); // np.sin(np.deg2rad(180-38)) - // (0.32489833, -0.25068134, 0.4761387) while let Some(event) = events.recv() { if let Event::Input { id, data, metadata } = event { @@ -186,6 +183,15 @@ pub fn lib_main() -> Result<()> { } else { vec![640, 480] }; + let pitch = if let Some(Parameter::Float(pitch)) = metadata.parameters.get("pitch") + { + *pitch as f32 + } else { + camera_pitch + }; + let cos_theta = pitch.cos(); // np.cos(np.deg2rad(180-38)) + let sin_theta = pitch.sin(); // np.sin(np.deg2rad(180-38)) + let points = match data.data_type() { dora_node_api::arrow::datatypes::DataType::Float64 => { let buffer: &Float64Array = data.as_any().downcast_ref().unwrap(); @@ -223,8 +229,8 @@ pub fn lib_main() -> Result<()> { let v = i as f32 / *width as f32; // Calculate y-coordinate (v) if let Some(z) = z { - let z = z as f32; - // Skip points that have empty depth or is too far away + let z = z as f32 / 1000.0; // Convert to meters + // Skip points that have empty depth or is too far away if z == 0. || z > 8.0 { points.push((0., 0., 0.)); return;