Make camera pitch parametrisation easier

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()
 

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::<f32>()
         .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;