libredr/camera/
perspective.rs

1use std::cmp::max;
2use nalgebra as na;
3use pyo3::prelude::*;
4use ndarray::{Axis, Array1, Array3};
5use anyhow::{Result, anyhow, ensure};
6use numpy::{PyReadonlyArray2, PyArray3, IntoPyArray, PyUntypedArrayMethods};
7use super::from_ray_corner;
8
9/// Construct input ray for perspective camera model
10/// # Arguments
11/// * `resolution`: tuple of 2 usize [`width`, `height`]
12/// * `intrinsic`: 3 * 3 matrix
13/// * `extrinsic`: 4 * 4 matrix
14/// # Return
15/// * 18 * `height` * `width`
16/// Camera model use x-right, y-down, z-forward axis scheme
17pub fn perspective_ray(
18    resolution: &[usize; 2],
19    intrinsic: na::MatrixView<f32, na::Const<3>, na::Const<3>, na::Dyn, na::Dyn>,
20    extrinsic: na::MatrixView<f32, na::Const<4>, na::Const<4>, na::Dyn, na::Dyn>) -> Result<Array3<f32>> {
21  ensure!(intrinsic.fold(true, |acc, v| { acc && v.is_finite() }),
22          "camera::perspective_ray: `intrinsic` is not finite");
23  ensure!(extrinsic.fold(true, |acc, v| { acc && v.is_finite() }),
24          "camera::perspective_ray: `extrinsic` is not finite");
25  let intrinsic_inv = intrinsic.try_inverse().ok_or(anyhow!("camera::perspective_ray: `intrinsic` is not invertible"))?;
26  let extrinsic_inv = extrinsic.try_inverse().ok_or(anyhow!("camera::perspective_ray: `extrinsic` is not invertible"))?;
27  let resolution_max = max(resolution[0], resolution[1]);
28  let axis_range = [resolution[0] as f32 / resolution_max as f32, resolution[1] as f32 / resolution_max as f32];
29  let axis_x = Array1::linspace(0.5 - 0.5 * axis_range[0], 0.5 + 0.5 * axis_range[0], resolution[0] + 1);
30  let axis_y = Array1::linspace(0.5 - 0.5 * axis_range[1], 0.5 + 0.5 * axis_range[1], resolution[1] + 1);
31  let ret_r = extrinsic_inv * na::Vector4::new(0., 0., 0., 1.);
32  let mut ray_corner = Array3::zeros((6, resolution[1] + 1, resolution[0] + 1));
33  ray_corner.axis_iter_mut(Axis(2)).enumerate().for_each(|(x, mut ray_corner)| {
34    ray_corner.axis_iter_mut(Axis(1)).enumerate().for_each(|(y, mut ray_corner)| {
35      let axis_xyz = intrinsic_inv * na::Vector3::new(axis_x[x], axis_y[y], 1.);
36      let ret_rd = extrinsic_inv * na::Vector4::new(axis_xyz[0], axis_xyz[1], axis_xyz[2], 0.);
37      for i in 0..3 {
38        ray_corner[i] = ret_r[i];
39        ray_corner[i + 3] = ret_rd[i];
40      }
41    });
42  });
43  from_ray_corner(ray_corner)
44}
45
46/// Python version [`perspective_ray`]
47#[pyfunction]
48#[pyo3(name = "perspective_ray")]
49pub fn py_perspective_ray<'py>(
50    py: Python<'py>,
51    resolution: [usize; 2],
52    intrinsic: PyReadonlyArray2<f32>,
53    extrinsic: PyReadonlyArray2<f32>) -> Result<Bound<'py, PyArray3<f32>>> {
54  ensure!(intrinsic.shape() == [3, 3],
55    "camera::perspective_ray: `intrinsic` expected shape {:?}, found {:?}", [3, 3], intrinsic.shape());
56  let intrinsic = intrinsic.try_as_matrix().expect("nalgebra dynamic stride");
57  ensure!(extrinsic.shape() == [4, 4],
58    "camera::perspective_ray: `extrinsic` expected shape {:?}, found {:?}", [4, 4], extrinsic.shape());
59  let extrinsic = extrinsic.try_as_matrix().expect("nalgebra dynamic stride");
60  let ray = perspective_ray(&resolution, intrinsic, extrinsic)?;
61  Ok(ray.into_pyarray(py))
62}
libredr/camera/perspective.rs

libredr/camera/
perspective.rs
