From 076c664063716ddc518b4399e02869e0aadbd896 Mon Sep 17 00:00:00 2001 From: Yang Zhou Date: Tue, 29 Oct 2024 23:36:35 -0400 Subject: [PATCH] 0.5.2 rk4-based quadrotor dynamics update;pinhole camera rerun logging --- Cargo.toml | 4 ++-- config/quad.yaml | 2 +- src/config.rs | 2 +- src/lib.rs | 55 +++++++++++++++++++++++++++--------------------- src/main.rs | 8 +++---- 5 files changed, 38 insertions(+), 33 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index a419bba2..3cfe80bf 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,7 +1,7 @@ [package] exclude = ["assets/", "CONTRIBUTING.md", "CODE_OF_CONDUCT.md", "SECURITY.md"] name = "peng_quad" -version = "0.5.1" +version = "0.5.2" edition = "2021" rust-version = "1.76" authors = ["Yang Zhou "] @@ -30,7 +30,7 @@ lto = "thin" # Do a second optimization pass over the entire program, inclu nalgebra = "0.33.0" rand = { version = "0.8.5", features = ["rand_chacha"] } rand_distr = "0.4.3" -rerun = "0.18.2" +rerun = "0.19.0" thiserror = "1.0.63" serde = { version = "1.0.209", features = ["derive"] } serde_yaml = "0.9.34" diff --git a/config/quad.yaml b/config/quad.yaml index 04fe2741..461dbd7b 100644 --- a/config/quad.yaml +++ b/config/quad.yaml @@ -44,7 +44,7 @@ maze: camera: resolution: [128, 96] # Camera resolution [width, height] (pixels) - fov: 90 # Field of view in height (degrees) + fov_vertical: 90 # Vertical Field of View (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 diff --git a/src/config.rs b/src/config.rs index 15078e0f..6661cc95 100644 --- a/src/config.rs +++ b/src/config.rs @@ -128,7 +128,7 @@ pub struct CameraConfig { /// Camera resolution in pixels (width, height) pub resolution: (usize, usize), /// Camera field of view in height in degrees - pub fov: f32, + pub fov_vertical: f32, /// Camera near clipping plane in meters pub near: f32, /// Camera far clipping plane in meters diff --git a/src/lib.rs b/src/lib.rs index b2107959..b6cfc47b 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -2088,8 +2088,14 @@ impl Maze { pub struct Camera { /// The resolution of the camera pub resolution: (usize, usize), - /// The field of view of the camera - pub fov: f32, + /// The vertical field of view of the camera + pub fov_vertical: f32, + /// The horizontal field of view of the camera + pub fov_horizontal: f32, + /// The vertical focal length of the camera + pub vertical_focal_length: f32, + /// The horizontal focal length of the camera + pub horizontal_focal_length: f32, /// The near clipping plane of the camera pub near: f32, /// The far clipping plane of the camera @@ -2104,7 +2110,7 @@ impl Camera { /// Creates a new camera with the given resolution, field of view, near and far clipping planes /// # Arguments /// * `resolution` - The resolution of the camera - /// * `fov` - The field of view of the camera + /// * `fov_vertical` - The vertical field of view of the camera /// * `near` - The near clipping plane of the camera /// * `far` - The far clipping plane of the camera /// # Returns @@ -2114,9 +2120,10 @@ impl Camera { /// use peng_quad::Camera; /// let camera = Camera::new((800, 600), 60.0, 0.1, 100.0); /// ``` - pub fn new(resolution: (usize, usize), fov: f32, near: f32, far: f32) -> Self { + pub fn new(resolution: (usize, usize), fov_vertical: f32, near: f32, far: f32) -> Self { let (width, height) = resolution; - let (aspect_ratio, tan_half_fov) = (width as f32 / height as f32, (fov / 2.0).tan()); + let (aspect_ratio, tan_half_fov) = + (width as f32 / height as f32, (fov_vertical / 2.0).tan()); let mut ray_directions = Vec::with_capacity(width * height); for y in 0..height { for x in 0..width { @@ -2125,9 +2132,16 @@ impl Camera { ray_directions.push(Vector3::new(1.0, x_ndc, y_ndc).normalize()); } } + let fov_horizontal = + (width as f32 / height as f32 * (fov_vertical / 2.0).tan()).atan() * 2.0; + let horizontal_focal_length = (width as f32 / 2.0) / ((fov_horizontal / 2.0).tan()); + let vertical_focal_length = (height as f32 / 2.0) / ((fov_vertical / 2.0).tan()); Self { resolution, - fov, + fov_vertical, + fov_horizontal, + vertical_focal_length, + horizontal_focal_length, near, far, aspect_ratio, @@ -2536,8 +2550,7 @@ pub fn log_mesh( /// # Arguments /// * `rec` - The rerun::RecordingStream instance /// * `depth_image` - The depth image data -/// * `width` - The width of the depth image -/// * `height` - The height of the depth image +/// * `resolution` - The width and height of the depth image /// * `min_depth` - The minimum depth value /// * `max_depth` - The maximum depth value /// # Errors @@ -2547,16 +2560,16 @@ pub fn log_mesh( /// use peng_quad::log_depth_image; /// let rec = rerun::RecordingStreamBuilder::new("log.rerun").connect().unwrap(); /// let depth_image = vec![0.0; 640 * 480]; -/// log_depth_image(&rec, &depth_image, 640, 480, 0.0, 1.0).unwrap(); +/// log_depth_image(&rec, &depth_image, (640usize, 480usize), 0.0, 1.0).unwrap(); /// ``` pub fn log_depth_image( rec: &rerun::RecordingStream, depth_image: &[f32], - width: usize, - height: usize, + resolution: (usize, usize), min_depth: f32, max_depth: f32, ) -> Result<(), SimulationError> { + let (width, height) = resolution; let mut image = rerun::external::ndarray::Array::zeros((height, width, 3)); let depth_range = max_depth - min_depth; image @@ -2585,9 +2598,7 @@ pub fn log_depth_image( /// 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` - The camera object /// * `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 @@ -2596,31 +2607,27 @@ pub fn log_depth_image( /// * If the data cannot be logged to the recording stream /// # Example /// ```no_run -/// use peng_quad::pinhole_depth; +/// use peng_quad::{pinhole_depth, Camera}; /// 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, (128usize, 96usize), 90.0, cam_position, cam_orientation, cam_transform, &depth_image).unwrap(); +/// let camera = Camera::new((800, 600), 60.0, 0.1, 100.0); +/// pinhole_depth(&rec, &camera, cam_position, cam_orientation, cam_transform, &depth_image).unwrap(); pub fn pinhole_depth( rec: &rerun::RecordingStream, - resolution: (usize, usize), - fov: f32, + cam: &Camera, cam_position: Vector3, cam_orientation: UnitQuaternion, cam_transform: [f32; 9], depth_image: &[f32], ) -> Result<(), SimulationError> { - let width = resolution.0; - let height = resolution.1; - let fov_x = (width as f32 / height as f32 * (fov / 2.0).tan()).atan() * 2.0; - let horizontal_focal_length = (width as f32 / 2.0) / ((fov_x / 2.0).tan()); - let vertical_focal_length = (height as f32 / 2.0) / ((fov / 2.0).tan()); + let (width, height) = cam.resolution; let pinhole_camera = rerun::Pinhole::from_focal_length_and_resolution( - (horizontal_focal_length, vertical_focal_length), + (cam.horizontal_focal_length, cam.vertical_focal_length), (width as f32, height as f32), ) .with_camera_xyz(rerun::components::ViewCoordinates::RDF) diff --git a/src/main.rs b/src/main.rs index 712ef54a..090476d0 100644 --- a/src/main.rs +++ b/src/main.rs @@ -47,7 +47,7 @@ fn main() -> Result<(), SimulationError> { ); let camera = Camera::new( config.camera.resolution, - config.camera.fov.to_radians(), + config.camera.fov_vertical.to_radians(), config.camera.near, config.camera.far, ); @@ -156,15 +156,13 @@ fn main() -> Result<(), SimulationError> { log_depth_image( rec, &depth_buffer, - camera.resolution.0, - camera.resolution.1, + camera.resolution, camera.near, camera.far, )?; pinhole_depth( rec, - camera.resolution, - camera.fov, + &camera, quad.position, quad.orientation, config.camera.rotation_transform,