vision-calibration-optim 0.3.0

Non-linear optimization problems and solvers for calibration-rs
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44

//! SE(3) parameter conversions for solver backends.
//!
//! The storage order is `[qx, qy, qz, qw, tx, ty, tz]`.

use crate::Error;
use nalgebra::{DVector, DVectorView, Quaternion, UnitQuaternion, Vector3};
use vision_calibration_core::Iso3;

/// Convert an `Iso3` into a 7D SE(3) parameter vector `[qx, qy, qz, qw, tx, ty, tz]`.
///
/// The quaternion is stored in `(x, y, z, w)` order to match tiny-solver.
pub fn iso3_to_se3_dvec(pose: &Iso3) -> DVector<f64> {
    let q = pose.rotation.into_inner();
    let t = pose.translation.vector;
    nalgebra::dvector![
        q.coords[0],
        q.coords[1],
        q.coords[2],
        q.coords[3],
        t.x,
        t.y,
        t.z
    ]
}

/// Convert a 7D SE(3) vector `[qx, qy, qz, qw, tx, ty, tz]` into an `Iso3`.
///
/// The input quaternion is not renormalized; callers should provide a unit quaternion.
///
/// # Errors
///
/// Returns [`Error::InvalidInput`] if the vector length is not 7.
pub fn se3_dvec_to_iso3(v: DVectorView<'_, f64>) -> Result<Iso3, Error> {
    if v.len() != 7 {
        return Err(Error::invalid_input(format!(
            "expected se3 vector of length 7, got {}",
            v.len()
        )));
    }
    let quat = Quaternion::new(v[3], v[0], v[1], v[2]);
    let rot = UnitQuaternion::from_quaternion(quat);
    let trans = Vector3::new(v[4], v[5], v[6]);
    Ok(Iso3::from_parts(trans.into(), rot))
}