Adding Spatial Constraints

It's time to put in our spatial and temporal constraints. These describe the relationships of our components in... well, space and time!

We'll start with spatial constraints.

RustPythonC++

// rotation in radians; translation in meters
// convention: (+X: right, +Y: down, +Z: forward)
let beta_from_alpha = Extrinsics::from(Isometry3::from_parts(
    Translation3::new(0.2, 0.0, -0.2),
    UnitQuaternion::from_euler_angles(std::f32::consts::FRAC_PI_4, 0.0, 0.0)
));

# rotation in radians
rotation = [math.pi/4, 0.0, 0.0]
# translation in meters
# convention: (+X: right, +Y: down, +Z: forward)
translation = [0.2, 0.0, -0.2]
extrinsics = plex.Extrinsics(rotation, translation)

// rotation in radians
std::vector<double> rotation{M_PI / 4, 0.0, 0.0};
// translation in meters
// convention: (+X: right, +Y: down, +Z: forward)
std::vector<double> translation{0.02, 0.0, -0.2};
tangviz::Extrinsics extrinsics{rotation, translation};

If you have mechanical drawings of your system, there's a possibility that you know the machining tolerances involved. If this is the case, you can keep track of these using the extrinsic covariance.

RustPythonC++

// Angular standard deviation in radians.
let d_theta = (constants::ROTATION_STD_DEV).to_radians();

let std_devs = Extrinsics::from(Isometry3::from_parts(
    // Assume all our translations are ±0.010m (±1cm)
    Translation3::new(
        constants::TRANSLATION_STD_DEV, // X
        constants::TRANSLATION_STD_DEV, // Y
        constants::TRANSLATION_STD_DEV, // Z
    ),
    // And all our rotations are good to ±15°
    UnitQuaternion::from_euler_angles(d_theta, d_theta, d_theta),
))
.to_se3();

let extrinsic_covariance = ExtrinsicCovariance::from(Matrix6::from_diagonal(
    &Vector6::from_iterator(std_devs.into_iter().map(|v| v.powi(2))),
));

# extrinsics covariance is currently in se3 space. This will change soon.
# For now, these are fine defaults.
# - translation standard deviation of 15m (basically, we know nothing)
# - rotation standard deviation of 15 degrees
extrinsics_covariance = plex.CovarianceClass(
    [2.888638620581263, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 6701.1800481258915,
     0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 21695.85245421797, 0.0, 0.0, 0.0, 0.0,
     0.0, 0.0, 0.10031263418253282, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
     1.7923190053450093, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.10031263418253282])

// extrinsics covariance is currently in se3 space. This will change soon.
// For now, these are fine defaults.
// - translation standard deviation of 15m (basically, we know nothing)
// - rotation standard deviation of 15 degrees
tangviz::CovarianceClass extrinsics_covariance{
    std::vector<double>{2.888638620581263,   0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                        6701.1800481258915,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                        21695.85245421797,   0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                        0.10031263418253282, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                        1.7923190053450093,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                        0.10031263418253282}};

We can now complete our spatial constraint!

RustPythonC++

let space = SpatialConstraint {
    extrinsics: beta_from_alpha,
    covariance: extrinsic_covariance,
    from: camera_b_uuid,
    to: camera_a_uuid,
};

spatial_constraint = plex.SpatialConstraint(
    extrinsics_covariance, extrinsics, camera_b_uuid, camera_a_uuid)

tangviz::SpatialConstraint spatial_constraint{
  extrinsics_covariance, extrinsics, camera_b_uuid, camera_a_uuid};

Completed Spatial constraint