Added pinhole functionality for depth mapping

.gitignore

@@ -19,3 +19,5 @@ Cargo.lock
 /target
 
 *.DS_Store
+.idea/workspace.xml
+.idea/vcs.xml

config/quad.yaml

@@ -44,9 +44,11 @@ maze:
 
 camera:
   resolution: [128, 96] # Camera resolution [width, height] (pixels)
-  fov: 90.0 # Field of view (degrees)
+  fov: 90 # Field of view in height (degrees)
+  fov_x: 106.26 # Field of view in width (degrees)
   near: 0.1 # Near clipping plane (m)
   far: 5.0 # Far clipping plane (m)
+  rotation_transform: [0.0, 0.0, 1.0, -1.0, 0.0, 0.0, 0.0, -1.0, 0.0] # Rotates camera to positive x-axis
 
 mesh:
   division: 7 # Number of divisions in the mesh grid

src/config.rs

@@ -127,12 +127,16 @@ pub struct MazeConfig {
 pub struct CameraConfig {
     /// Camera resolution in pixels (width, height)
     pub resolution: (usize, usize),
-    /// Camera field of view in degrees
+    /// Camera field of view in height in degrees
     pub fov: f32,
+    ///Camera field of view in width in degrees
+    pub fov_x: f32,
     /// Camera near clipping plane in meters
     pub near: f32,
     /// Camera far clipping plane in meters
     pub far: f32,
+    /// Camera transform matrix for depth
+    pub rotation_transform: [f32; 9]
 }
 
 #[derive(serde::Deserialize)]

src/lib.rs

@@ -2582,6 +2582,76 @@ pub fn log_depth_image(
     rec.log("world/quad/cam/depth", &rerun_image)?;
     Ok(())
 }
+/// creates pinhole camera
+/// # Arguments
+/// * `rec` - The rerun::RecordingStream instance
+/// * `width` - The width component of the camera resolution
+/// * `height` - The height component of the camera resolution
+/// * `fov` - The fov of the camera
+/// * `cam_position` - The position vector of the camera (aligns with the quad)
+/// * `cam_orientation` - The orientation quaternion of quad
+/// * `cam_transform` - The transform matrix between quad and camera alignment
+/// * `depth_image` - The depth image data
+/// # Errors
+/// * If the data cannot be logged to the recording stream
+/// # Example
+/// ```no_run
+/// use peng_quad::pinhole_depth;
+/// use nalgebra::{Vector3, UnitQuaternion};
+/// let rec = rerun::RecordingStreamBuilder::new("log.rerun").connect().unwrap();
+/// let depth_image = vec![ 0.0f32 ; 640 * 480];
+/// let cam_position = Vector3::new(0.0,0.0,0.0);
+/// let cam_orientation = UnitQuaternion::identity();
+/// let cam_transform = [0.0, 0.0, 1.0, -1.0, 0.0, 0.0, 0.0, -1.0, 0.0];
+/// pinhole_depth(&rec, 128, 96, 90.0.to_radians(), 106.26.to_radians(), cam_position, cam_orientation, cam_transform, &depth_image).unwrap();
+
+pub fn pinhole_depth(
+    rec: &rerun::RecordingStream,
+    width: usize,
+    height: usize,
+    fov_x: f32,
+    fov_y: f32,
+    cam_position: Vector3<f32>,
+    cam_orientation: UnitQuaternion<f32>,
+    cam_transform: [f32; 9],
+    depth_image: &[f32],
+) -> Result<(), SimulationError> {
+    let horizontal_focal_length = (width as f32 / 2.0) / ((fov_x / 2.0).tan());
+    let vertical_focal_length = (height as f32 / 2.0) / ((fov_y / 2.0).tan());
+    let pinhole_camera = rerun::Pinhole::from_focal_length_and_resolution(
+        (horizontal_focal_length, vertical_focal_length),
+        (width as f32, height as f32),
+    )
+        .with_camera_xyz(rerun::components::ViewCoordinates::RDF)
+        .with_resolution((width as f32, height as f32))
+        .with_principal_point((width as f32 / 2.0, height as f32 / 2.0));
+    let rotated_camera_orientation = UnitQuaternion::from_rotation_matrix(
+        &(cam_orientation.to_rotation_matrix()
+            * Rotation3::from_matrix_unchecked(Matrix3::from_row_slice(&cam_transform))),
+    );
+    let cam_transform = rerun::Transform3D::from_translation_rotation(
+        rerun::Vec3D::new(cam_position.x, cam_position.y, cam_position.z),
+        rerun::Quaternion::from_xyzw([
+            rotated_camera_orientation.i,
+            rotated_camera_orientation.j,
+            rotated_camera_orientation.k,
+            rotated_camera_orientation.w,
+        ]),
+    );
+    rec.log("world/quad/cam", &cam_transform)?;
+    rec.log("world/quad/cam", &pinhole_camera)?;
+    let depth_image_rerun =
+        rerun::external::ndarray::Array::from_shape_vec((height, width), depth_image.to_vec())
+            .unwrap();
+    rec.log(
+        "world/quad/cam/rerun_depth",
+        &rerun::DepthImage::try_from(depth_image_rerun)
+            .unwrap()
+            .with_meter(1.0),
+    )?;
+
+    Ok(())
+}
 /// turbo color map function
 /// # Arguments
 /// * `gray` - The gray value in the range [0, 255]

src/main.rs

@@ -161,6 +161,17 @@ fn main() -> Result<(), SimulationError> {
                         camera.near,
                         camera.far,
                     )?;
+                    pinhole_depth(
+                        rec,
+                        camera.resolution.0,
+                        camera.resolution.1,
+                        config.camera.fov_x.to_radians(),
+                        camera.fov,
+                        quad.position,
+                        quad.orientation,
+                        config.camera.rotation_transform,
+                        &depth_buffer,
+                    )?;
                 }
                 log_maze_obstacles(rec, &maze)?;
             }