use crate::{Bearing, CameraPoint, EssentialMatrix, FeatureWorldMatch, Skew3, WorldPoint};
use derive_more::{AsMut, AsRef, Deref, DerefMut, From, Into};
use nalgebra::{IsometryMatrix3, Matrix3, Matrix6x3, Point3, Rotation3, Vector3, Vector6};
use sample_consensus::Model;
pub trait Pose {
type InputPoint;
type OutputPoint;
fn transform_jacobians(
&self,
input: Self::InputPoint,
) -> (Self::OutputPoint, Matrix3<f64>, Matrix6x3<f64>);
fn transform_jacobian_input(
&self,
input: Self::InputPoint,
) -> (Self::OutputPoint, Matrix3<f64>) {
let (output, input_jacobian, _) = self.transform_jacobians(input);
(output, input_jacobian)
}
fn transform_jacobian_pose(
&self,
input: Self::InputPoint,
) -> (Self::OutputPoint, Matrix6x3<f64>) {
let (output, _, pose_jacobian) = self.transform_jacobians(input);
(output, pose_jacobian)
}
fn transform(&self, input: Self::InputPoint) -> Self::OutputPoint {
let (output, _, _) = self.transform_jacobians(input);
output
}
fn update(&mut self, delta: Vector6<f64>);
}
#[inline]
fn isometry_point_outputs(
isometry: &IsometryMatrix3<f64>,
input: Point3<f64>,
) -> (Point3<f64>, Matrix3<f64>, Matrix6x3<f64>) {
let rotated = isometry.rotation * input;
let output = rotated + isometry.translation.vector;
let dp_dt = Matrix3::<f64>::identity();
let dp_ds = Skew3::jacobian_self(rotated.coords);
let dp_dts = Matrix6x3::<f64>::from_rows(&[
dp_dt.row(0),
dp_dt.row(1),
dp_dt.row(2),
dp_ds.row(0),
dp_ds.row(1),
dp_ds.row(2),
]);
let skew: Skew3 = isometry.rotation.into();
let dp_di = skew.jacobian_input();
(output, dp_di, dp_dts)
}
#[derive(Debug, Clone, Copy, PartialEq, AsMut, AsRef, Deref, DerefMut, From, Into)]
pub struct WorldPose(pub IsometryMatrix3<f64>);
impl WorldPose {
pub fn from_parts(rotation: Rotation3<f64>, translation: Vector3<f64>) -> Self {
Self(IsometryMatrix3::from_parts(translation.into(), rotation))
}
pub fn identity() -> Self {
Self(IsometryMatrix3::identity())
}
}
impl Pose for WorldPose {
type InputPoint = WorldPoint;
type OutputPoint = CameraPoint;
#[inline]
fn transform_jacobians(
&self,
input: Self::InputPoint,
) -> (Self::OutputPoint, Matrix3<f64>, Matrix6x3<f64>) {
let (output, jacobian_input, jacobian_pose) = isometry_point_outputs(&self.0, input.0);
(CameraPoint(output), jacobian_input, jacobian_pose)
}
#[inline]
fn update(&mut self, delta: Vector6<f64>) {
self.translation.vector += Vector3::new(delta[0], delta[1], delta[2]);
let mut skew: Skew3 = self.rotation.into();
skew.0 += Vector3::new(delta[3], delta[4], delta[5]);
self.rotation = skew.into();
}
}
impl<P> Model<FeatureWorldMatch<P>> for WorldPose
where
P: Bearing,
{
fn residual(&self, data: &FeatureWorldMatch<P>) -> f32 {
let WorldPose(iso) = *self;
let FeatureWorldMatch(feature, world) = data;
let new_bearing = (iso * world.coords).normalize();
let bearing_vector = feature.bearing();
(1.0 - bearing_vector.dot(&new_bearing)) as f32
}
}
impl From<CameraPose> for WorldPose {
fn from(camera: CameraPose) -> Self {
Self(camera.inverse())
}
}
#[derive(Debug, Clone, Copy, PartialEq, AsMut, AsRef, Deref, DerefMut, From, Into)]
pub struct CameraPose(pub IsometryMatrix3<f64>);
impl CameraPose {
pub fn from_parts(rotation: Rotation3<f64>, translation: Vector3<f64>) -> Self {
Self(IsometryMatrix3::from_parts(translation.into(), rotation))
}
pub fn identity() -> Self {
Self(IsometryMatrix3::identity())
}
}
impl Pose for CameraPose {
type InputPoint = CameraPoint;
type OutputPoint = WorldPoint;
#[inline]
fn transform_jacobians(
&self,
input: Self::InputPoint,
) -> (Self::OutputPoint, Matrix3<f64>, Matrix6x3<f64>) {
let (output, jacobian_input, jacobian_pose) = isometry_point_outputs(&self.0, input.0);
(WorldPoint(output), jacobian_input, jacobian_pose)
}
#[inline]
fn update(&mut self, delta: Vector6<f64>) {
self.translation.vector += Vector3::new(delta[0], delta[1], delta[2]);
let mut skew: Skew3 = self.rotation.into();
skew.0 += Vector3::new(delta[3], delta[4], delta[5]);
self.rotation = skew.into();
}
}
impl From<WorldPose> for CameraPose {
fn from(world: WorldPose) -> Self {
Self(world.inverse())
}
}
#[derive(Debug, Clone, Copy, PartialEq, AsMut, AsRef, Deref, DerefMut, From, Into)]
pub struct RelativeCameraPose(pub IsometryMatrix3<f64>);
impl RelativeCameraPose {
pub fn from_parts(rotation: Rotation3<f64>, translation: Vector3<f64>) -> Self {
Self(IsometryMatrix3::from_parts(translation.into(), rotation))
}
pub fn essential_matrix(&self) -> EssentialMatrix {
EssentialMatrix(self.0.translation.vector.cross_matrix() * *self.0.rotation.matrix())
}
pub fn inverse(&self) -> Self {
Self(self.0.inverse())
}
}
impl Pose for RelativeCameraPose {
type InputPoint = CameraPoint;
type OutputPoint = CameraPoint;
#[inline]
fn transform_jacobians(
&self,
input: Self::InputPoint,
) -> (Self::OutputPoint, Matrix3<f64>, Matrix6x3<f64>) {
let (output, jacobian_input, jacobian_pose) = isometry_point_outputs(&self.0, input.0);
(CameraPoint(output), jacobian_input, jacobian_pose)
}
#[inline]
fn update(&mut self, delta: Vector6<f64>) {
self.translation.vector += Vector3::new(delta[0], delta[1], delta[2]);
let mut skew: Skew3 = self.rotation.into();
skew.0 += Vector3::new(delta[3], delta[4], delta[5]);
self.rotation = skew.into();
}
}
#[derive(Debug, Clone, Copy, PartialEq, AsMut, AsRef, Deref, DerefMut, From, Into)]
pub struct UnscaledRelativeCameraPose(pub RelativeCameraPose);