Struct Camera

Source
pub struct Camera<R: RealField> { /* private fields */ }
Expand description

A calibrated camera with both intrinsic and extrinsic parameters.

This structure represents a complete camera model including:

  • Intrinsic parameters: focal length, principal point, distortion coefficients
  • Extrinsic parameters: position and orientation in 3D space
  • Image dimensions: width and height in pixels

The camera follows the standard computer vision coordinate conventions:

  • Camera frame: X→right, Y→down, Z→forward (optical axis)
  • Image coordinates: origin at top-left, X→right, Y→down

§Mathematical Model

The camera implements the projective camera model:

s[u v 1]ᵀ = K[R|t][X Y Z 1]ᵀ

where:

  • (X,Y,Z) are 3D world coordinates
  • (u,v) are 2D undistorted image coordinates
  • K is the intrinsic matrix
  • [R|t] represents rotation and translation (extrinsics)
  • s is a scaling factor

Lens distortion is supported via the opencv-ros-camera crate.

The parameters for the intrinsic matrix (focal length, principal point, and skew) in addition to the distortion parameters together comprise the intrinsic parameters, or “intrinsics”.

§Example

use braid_mvg::{Camera, extrinsics, make_default_intrinsics};

// Create a camera with default parameters
let extrinsics = extrinsics::make_default_extrinsics::<f64>();
let intrinsics = make_default_intrinsics::<f64>();
let camera = Camera::new(640, 480, extrinsics, intrinsics).unwrap();

// Project a 3D point to 2D
use braid_mvg::PointWorldFrame;
use nalgebra::Point3;
let point_3d = PointWorldFrame { coords: Point3::new(0.0, 0.0, 5.0) };
let pixel = camera.project_3d_to_pixel(&point_3d);

Implementations§

Source§

impl<R: RealField + Copy> Camera<R>

Source

pub fn rr_pinhole_archetype(&self) -> Result<Pinhole, MvgError>

Available on crate feature rerun-io only.

Return a re_types::archetypes::Pinhole

The conversion will not succeed if the camera cannot be represented exactly in re_types.

Source§

impl<R: RealField + Copy> Camera<R>

Source

pub fn new( width: usize, height: usize, extrinsics: ExtrinsicParameters<R>, intrinsics: RosOpenCvIntrinsics<R>, ) -> Result<Self>

Create a new camera from intrinsic and extrinsic parameters.

This constructor creates a complete camera model by combining:

  • Image dimensions (width, height)
  • Extrinsic parameters (camera pose in world coordinates)
  • Intrinsic parameters (focal length, principal point, distortion)
§Arguments
  • width - Image width in pixels
  • height - Image height in pixels
  • extrinsics - Camera position and orientation in world coordinates
  • intrinsics - Camera intrinsic parameters including distortion model
§Returns

A new Camera instance, or MvgError if the parameters are invalid.

§Errors

Returns an error if:

  • The projection matrix cannot be computed
  • The camera parameters are mathematically inconsistent
  • SVD decomposition fails during initialization
§Example
use braid_mvg::{Camera, extrinsics, make_default_intrinsics};

let extrinsics = extrinsics::make_default_extrinsics::<f64>();
let intrinsics = make_default_intrinsics::<f64>();
let camera = Camera::new(640, 480, extrinsics, intrinsics)?;
Source

pub fn new_from_cam_geom( width: usize, height: usize, inner: Camera<R, RosOpenCvIntrinsics<R>>, ) -> Result<Self>

Create a new camera from a cam-geom Camera instance.

This constructor wraps an existing cam-geom Camera with additional image dimension information and caching for performance.

§Arguments
  • width - Image width in pixels
  • height - Image height in pixels
  • inner - A pre-constructed cam-geom Camera instance
§Returns

A new Camera instance, or MvgError if the camera cannot be constructed.

§Example
use braid_mvg::{Camera, extrinsics, make_default_intrinsics};
use cam_geom;

let extrinsics = extrinsics::make_default_extrinsics::<f64>();
let intrinsics = make_default_intrinsics::<f64>();
let cam_geom_camera = cam_geom::Camera::new(intrinsics, extrinsics);
let camera = Camera::new_from_cam_geom(640, 480, cam_geom_camera)?;
Source

pub fn from_pmat( width: usize, height: usize, pmat: &OMatrix<R, U3, U4>, ) -> Result<Self>

Create a camera from a 3×4 projection matrix.

This method decomposes a camera projection matrix into intrinsic and extrinsic parameters using QR decomposition. It assumes no lens distortion (pinhole model).

§Mathematical Background

The projection matrix P has the form:

P = K[R|t]

where K is the 3×3 intrinsic matrix and [R|t] is the 3×4 extrinsic matrix.

§Arguments
  • width - Image width in pixels
  • height - Image height in pixels
  • pmat - 3×4 projection matrix
§Returns

A new Camera instance with no distortion, or MvgError if decomposition fails.

§Errors

Returns an error if:

  • The projection matrix is singular or ill-conditioned
  • QR decomposition fails
  • The resulting parameters are invalid
§Example
use braid_mvg::Camera;
use nalgebra::{OMatrix, U3, U4};

// Create a simple projection matrix
let pmat = OMatrix::<f64, U3, U4>::new(
    1000.0, 0.0, 320.0, 100.0,
    0.0, 1000.0, 240.0, 200.0,
    0.0, 0.0, 1.0, 0.01
);
let camera = Camera::from_pmat(640, 480, &pmat)?;
Source

pub fn as_pmat(&self) -> Option<&OMatrix<R, U3, U4>>

convert, if possible, into a 3x4 matrix

Source

pub fn linear_part_as_pmat(&self) -> &OMatrix<R, U3, U4>

Get the linear projection matrix (3×4) for this camera.

This returns the cached projection matrix that represents a linearized version of the camera model (without lens distortion). The matrix has the form P = K[R|t] where K is the intrinsic matrix and [R|t] are the extrinsic parameters.

§Returns

Reference to the 3×4 projection matrix

§Example
use braid_mvg::{Camera, extrinsics, make_default_intrinsics};

let camera = Camera::new(640, 480,
    extrinsics::make_default_extrinsics::<f64>(),
    make_default_intrinsics::<f64>())?;
let pmat = camera.linear_part_as_pmat();
println!("Projection matrix shape: {}×{}", pmat.nrows(), pmat.ncols());
Source

pub fn linearize_to_cam_geom( &self, ) -> Camera<R, IntrinsicParametersPerspective<R>>

Return a linearized copy of self.

The returned camera will not have distortion. In other words, the raw projected (“distorted”) pixels are identical with the “undistorted” variant. The camera model is a perfect linear pinhole.

Source

pub fn align(&self, s: R, rot: Matrix3<R>, t: Vector3<R>) -> Result<Self>

Transform this camera using a similarity transformation.

This method applies a similarity transformation (scale, rotation, translation) to align the camera coordinate system. This is commonly used in:

  • Multi-camera system calibration
  • Coordinate system alignment
  • Scale recovery in structure-from-motion
§Mathematical Details

The transformation applies: X' = s*R*X + t where:

  • s is the uniform scale factor
  • R is the 3×3 rotation matrix
  • t is the 3×1 translation vector
  • X are the original 3D points
§Arguments
  • s - Uniform scale factor (positive)
  • rot - 3×3 rotation matrix (must be orthogonal with det=1)
  • t - 3×1 translation vector
§Returns

A new aligned Camera instance, or MvgError if transformation fails.

§Errors

Returns an error if:

  • The rotation matrix is invalid (not orthogonal or det≠1)
  • The scale factor is non-positive
  • Camera reconstruction fails after transformation
§Example
use braid_mvg::{Camera, extrinsics, make_default_intrinsics};
use nalgebra::{Matrix3, Vector3};

let camera = Camera::new(640, 480,
    extrinsics::make_default_extrinsics::<f64>(),
    make_default_intrinsics::<f64>())?;

let scale = 2.0;
let rotation = Matrix3::identity();
let translation = Vector3::new(1.0, 0.0, 0.0);

let aligned_camera = camera.align(scale, rotation, translation)?;
Source

pub fn flip(&self) -> Option<Camera<R>>

return a copy of this camera looking in the opposite direction

The returned camera has the same 3D->2D projection. (The 2D->3D projection results in a vector in the opposite direction.)

Source

pub fn intrinsics(&self) -> &RosOpenCvIntrinsics<R>

Get the camera’s intrinsic parameters.

Source

pub fn extrinsics(&self) -> &ExtrinsicParameters<R>

Get the camera’s extrinsic parameters.

Source

pub fn to_pymvg(&self, name: &str) -> PymvgCamera<R>

Convert this camera to PyMVG format.

PyMVG is a Python library for multiple view geometry. This method converts the camera parameters to the PyMVG JSON schema format for interoperability.

§Arguments
  • name - Name to assign to the camera in the PyMVG format
§Returns

A PymvgCamera struct containing the camera parameters in PyMVG format

§Example
use braid_mvg::{Camera, extrinsics, make_default_intrinsics};

let camera = Camera::new(640, 480,
    extrinsics::make_default_extrinsics::<f64>(),
    make_default_intrinsics::<f64>())?;
let pymvg_camera = camera.to_pymvg("camera1");
Source

pub fn width(&self) -> usize

Get the image width in pixels.

Source

pub fn height(&self) -> usize

Get the image height in pixels.

Source

pub fn project_3d_to_pixel( &self, pt3d: &PointWorldFrame<R>, ) -> UndistortedPixel<R>

Project a 3D world point to undistorted 2D image coordinates.

This method performs the core camera projection operation, transforming a 3D point in world coordinates to 2D pixel coordinates. The result represents the undistorted pixel coordinates (as if using a perfect pinhole camera model).

Source

pub fn project_3d_to_distorted_pixel( &self, pt3d: &PointWorldFrame<R>, ) -> DistortedPixel<R>

Project a 3D world point to distorted 2D image coordinates.

This method projects a 3D point to 2D image coordinates and then applies lens distortion to get the actual pixel coordinates as they would appear in the raw camera image.

§Arguments
  • pt3d - 3D point in world coordinates
§Returns

DistortedPixel containing the 2D image coordinates with distortion applied

§Example
use braid_mvg::{Camera, PointWorldFrame, extrinsics, make_default_intrinsics};
use nalgebra::Point3;

let camera = Camera::new(640, 480,
    extrinsics::make_default_extrinsics::<f64>(),
    make_default_intrinsics::<f64>())?;
let point_3d = PointWorldFrame { coords: Point3::new(0.0, 0.0, 5.0) };
let distorted_pixel = camera.project_3d_to_distorted_pixel(&point_3d);
Source

pub fn project_pixel_to_3d_with_dist( &self, pt2d: &UndistortedPixel<R>, dist: R, ) -> PointWorldFrame<R>

Back-project a 2D undistorted pixel to a 3D point at a given distance.

This method performs the inverse camera projection, taking a 2D pixel coordinate and a distance to compute the corresponding 3D world point. This is useful for depth-based reconstruction and ray casting.

§Arguments
  • pt2d - 2D pixel coordinates (undistorted)
  • dist - Distance from camera center to the 3D point
§Returns

PointWorldFrame containing the 3D world coordinates

§Example
use braid_mvg::{Camera, UndistortedPixel, extrinsics, make_default_intrinsics};
use nalgebra::Point2;

let camera = Camera::new(640, 480,
    extrinsics::make_default_extrinsics::<f64>(),
    make_default_intrinsics::<f64>())?;
let pixel = UndistortedPixel { coords: Point2::new(320.0, 240.0) };
let point_3d = camera.project_pixel_to_3d_with_dist(&pixel, 5.0);
Source

pub fn project_distorted_pixel_to_3d_with_dist( &self, pt2d: &DistortedPixel<R>, dist: R, ) -> PointWorldFrame<R>

Back-project a 2D distorted pixel to a 3D point at a given distance.

This method first removes lens distortion from the pixel coordinates, then back-projects to 3D space at the specified distance from the camera.

§Arguments
  • pt2d - 2D pixel coordinates (with distortion)
  • dist - Distance from camera center to the 3D point
§Returns

PointWorldFrame containing the 3D world coordinates

§Example
use braid_mvg::{Camera, DistortedPixel, extrinsics, make_default_intrinsics};
use nalgebra::Point2;

let camera = Camera::new(640, 480,
    extrinsics::make_default_extrinsics::<f64>(),
    make_default_intrinsics::<f64>())?;
let pixel = DistortedPixel { coords: Point2::new(320.0, 240.0) };
let point_3d = camera.project_distorted_pixel_to_3d_with_dist(&pixel, 5.0);

Trait Implementations§

Source§

impl<R: RealField + Copy> AsRef<Camera<R, RosOpenCvIntrinsics<R>>> for Camera<R>

Source§

fn as_ref(&self) -> &Camera<R, RosOpenCvIntrinsics<R>>

Converts this type into a shared reference of the (usually inferred) input type.
Source§

impl<R: Clone + RealField> Clone for Camera<R>

Source§

fn clone(&self) -> Camera<R>

Returns a duplicate of the value. Read more
1.0.0 · Source§

fn clone_from(&mut self, source: &Self)

Performs copy-assignment from source. Read more
Source§

impl<R: RealField + Copy> Debug for Camera<R>

Source§

fn fmt(&self, f: &mut Formatter<'_>) -> Result

Formats the value using the given formatter. Read more
Source§

impl<R: RealField + Copy> Default for Camera<R>

Source§

fn default() -> Camera<R>

Returns the “default value” for a type. Read more
Source§

impl<'de, R: RealField + Deserialize<'de> + Copy> Deserialize<'de> for Camera<R>

Source§

fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
where D: Deserializer<'de>,

Deserialize this value from the given Serde deserializer. Read more
Source§

impl<R: PartialEq + RealField> PartialEq for Camera<R>

Source§

fn eq(&self, other: &Camera<R>) -> bool

Tests for self and other values to be equal, and is used by ==.
1.0.0 · Source§

fn ne(&self, other: &Rhs) -> bool

Tests for !=. The default implementation is almost always sufficient, and should not be overridden without very good reason.
Source§

impl<R: RealField + Serialize + Copy> Serialize for Camera<R>

Source§

fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
where S: Serializer,

Serialize this value into the given Serde serializer. Read more
Source§

impl<R: RealField> StructuralPartialEq for Camera<R>

Auto Trait Implementations§

§

impl<R> Freeze for Camera<R>
where R: Freeze,

§

impl<R> RefUnwindSafe for Camera<R>
where R: RefUnwindSafe,

§

impl<R> Send for Camera<R>

§

impl<R> Sync for Camera<R>

§

impl<R> Unpin for Camera<R>
where R: Unpin,

§

impl<R> UnwindSafe for Camera<R>
where R: UnwindSafe,

Blanket Implementations§

Source§

impl<T> Any for T
where T: 'static + ?Sized,

Source§

fn type_id(&self) -> TypeId

Gets the TypeId of self. Read more
Source§

impl<T> Az for T

Source§

fn az<Dst>(self) -> Dst
where T: Cast<Dst>,

Casts the value.
Source§

impl<T> Borrow<T> for T
where T: ?Sized,

Source§

fn borrow(&self) -> &T

Immutably borrows from an owned value. Read more
Source§

impl<T> BorrowMut<T> for T
where T: ?Sized,

Source§

fn borrow_mut(&mut self) -> &mut T

Mutably borrows from an owned value. Read more
Source§

impl<Src, Dst> CastFrom<Src> for Dst
where Src: Cast<Dst>,

Source§

fn cast_from(src: Src) -> Dst

Casts the value.
Source§

impl<T> CheckedAs for T

Source§

fn checked_as<Dst>(self) -> Option<Dst>
where T: CheckedCast<Dst>,

Casts the value.
Source§

impl<Src, Dst> CheckedCastFrom<Src> for Dst
where Src: CheckedCast<Dst>,

Source§

fn checked_cast_from(src: Src) -> Option<Dst>

Casts the value.
Source§

impl<T> CloneToUninit for T
where T: Clone,

Source§

unsafe fn clone_to_uninit(&self, dest: *mut u8)

🔬This is a nightly-only experimental API. (clone_to_uninit)
Performs copy-assignment from self to dest. Read more
Source§

impl<T> From<T> for T

Source§

fn from(t: T) -> T

Returns the argument unchanged.

Source§

impl<T> Instrument for T

Source§

fn instrument(self, span: Span) -> Instrumented<Self>

Instruments this type with the provided Span, returning an Instrumented wrapper. Read more
Source§

fn in_current_span(self) -> Instrumented<Self>

Instruments this type with the current Span, returning an Instrumented wrapper. Read more
Source§

impl<T, U> Into<U> for T
where U: From<T>,

Source§

fn into(self) -> U

Calls U::from(self).

That is, this conversion is whatever the implementation of From<T> for U chooses to do.

Source§

impl<T> IntoEither for T

Source§

fn into_either(self, into_left: bool) -> Either<Self, Self>

Converts self into a Left variant of Either<Self, Self> if into_left is true. Converts self into a Right variant of Either<Self, Self> otherwise. Read more
Source§

fn into_either_with<F>(self, into_left: F) -> Either<Self, Self>
where F: FnOnce(&Self) -> bool,

Converts self into a Left variant of Either<Self, Self> if into_left(&self) returns true. Converts self into a Right variant of Either<Self, Self> otherwise. Read more
Source§

impl<Src, Dst> LosslessTryInto<Dst> for Src
where Dst: LosslessTryFrom<Src>,

Source§

fn lossless_try_into(self) -> Option<Dst>

Performs the conversion.
Source§

impl<Src, Dst> LossyInto<Dst> for Src
where Dst: LossyFrom<Src>,

Source§

fn lossy_into(self) -> Dst

Performs the conversion.
Source§

impl<T> OverflowingAs for T

Source§

fn overflowing_as<Dst>(self) -> (Dst, bool)
where T: OverflowingCast<Dst>,

Casts the value.
Source§

impl<Src, Dst> OverflowingCastFrom<Src> for Dst
where Src: OverflowingCast<Dst>,

Source§

fn overflowing_cast_from(src: Src) -> (Dst, bool)

Casts the value.
Source§

impl<T> Same for T

Source§

type Output = T

Should always be Self
Source§

impl<T> SaturatingAs for T

Source§

fn saturating_as<Dst>(self) -> Dst
where T: SaturatingCast<Dst>,

Casts the value.
Source§

impl<Src, Dst> SaturatingCastFrom<Src> for Dst
where Src: SaturatingCast<Dst>,

Source§

fn saturating_cast_from(src: Src) -> Dst

Casts the value.
Source§

impl<SS, SP> SupersetOf<SS> for SP
where SS: SubsetOf<SP>,

Source§

fn to_subset(&self) -> Option<SS>

The inverse inclusion map: attempts to construct self from the equivalent element of its superset. Read more
Source§

fn is_in_subset(&self) -> bool

Checks if self is actually part of its subset T (and can be converted to it).
Source§

fn to_subset_unchecked(&self) -> SS

Use with care! Same as self.to_subset but without any property checks. Always succeeds.
Source§

fn from_subset(element: &SS) -> SP

The inclusion map: converts self to the equivalent element of its superset.
Source§

impl<T> ToOwned for T
where T: Clone,

Source§

type Owned = T

The resulting type after obtaining ownership.
Source§

fn to_owned(&self) -> T

Creates owned data from borrowed data, usually by cloning. Read more
Source§

fn clone_into(&self, target: &mut T)

Uses borrowed data to replace owned data, usually by cloning. Read more
Source§

impl<T, U> TryFrom<U> for T
where U: Into<T>,

Source§

type Error = Infallible

The type returned in the event of a conversion error.
Source§

fn try_from(value: U) -> Result<T, <T as TryFrom<U>>::Error>

Performs the conversion.
Source§

impl<T, U> TryInto<U> for T
where U: TryFrom<T>,

Source§

type Error = <U as TryFrom<T>>::Error

The type returned in the event of a conversion error.
Source§

fn try_into(self) -> Result<U, <U as TryFrom<T>>::Error>

Performs the conversion.
Source§

impl<T> UnwrappedAs for T

Source§

fn unwrapped_as<Dst>(self) -> Dst
where T: UnwrappedCast<Dst>,

Casts the value.
Source§

impl<Src, Dst> UnwrappedCastFrom<Src> for Dst
where Src: UnwrappedCast<Dst>,

Source§

fn unwrapped_cast_from(src: Src) -> Dst

Casts the value.
Source§

impl<T> WithSubscriber for T

Source§

fn with_subscriber<S>(self, subscriber: S) -> WithDispatch<Self>
where S: Into<Dispatch>,

Attaches the provided Subscriber to this type, returning a WithDispatch wrapper. Read more
Source§

fn with_current_subscriber(self) -> WithDispatch<Self>

Attaches the current default Subscriber to this type, returning a WithDispatch wrapper. Read more
Source§

impl<T> WrappingAs for T

Source§

fn wrapping_as<Dst>(self) -> Dst
where T: WrappingCast<Dst>,

Casts the value.
Source§

impl<Src, Dst> WrappingCastFrom<Src> for Dst
where Src: WrappingCast<Dst>,

Source§

fn wrapping_cast_from(src: Src) -> Dst

Casts the value.
Source§

impl<T> Allocation for T
where T: RefUnwindSafe + Send + Sync,

Source§

impl<T> DeserializeOwned for T
where T: for<'de> Deserialize<'de>,

Source§

impl<T> Scalar for T
where T: 'static + Clone + PartialEq + Debug,