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.

// 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.

// 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!

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