Skip to content

Commit

Permalink
remove num dual (#6)
Browse files Browse the repository at this point in the history
  • Loading branch information
@powei-lin
powei-lin authored Dec 16, 2024
1 parent 3b9d086 commit 3af23da
Show file tree
Hide file tree
Showing 3 changed files with 64 additions and 93 deletions.
7 changes: 3 additions & 4 deletions Cargo.toml
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
[package]
name = "camera-intrinsic-calibration"
version = "0.4.3"
version = "0.5.0"
edition = "2021"
authors = ["Powei Lin <poweilin1994@gmail.com>"]
readme = "README.md"
Expand All @@ -25,16 +25,15 @@ image = "0.25.5"
indicatif = { version = "0.17.9", features = ["rayon"] }
log = "0.4.22"
nalgebra = "0.33.2"
num-dual = "0.10.3"
rand = "0.8.5"
rand_chacha = "0.3.1"
rayon = "1.10.0"
rerun = "0.17.0"
serde = { version = "1.0.215", features = ["derive"] }
serde = { version = "1.0.216", features = ["derive"] }
serde_json = "1.0.133"
sqpnp_simple = "0.1.5"
time = "0.3.37"
tiny-solver = "0.11.1"
tiny-solver = "0.12.0"

[[bin]]
name = "ccrs"
Expand Down
108 changes: 46 additions & 62 deletions src/optimization/factors.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,14 +2,13 @@ use crate::types::DVecVec3;

use camera_intrinsic_model::*;
use nalgebra as na;
use num_dual::DualDVec64;
use tiny_solver::factors::Factor;

#[derive(Clone)]
pub struct ModelConvertFactor {
pub source: GenericModel<DualDVec64>,
pub target: GenericModel<DualDVec64>,
pub p3ds: Vec<na::Vector3<DualDVec64>>,
pub source: GenericModel<f64>,
pub target: GenericModel<f64>,
pub p3ds: Vec<na::Vector3<f64>>,
}

impl ModelConvertFactor {
Expand Down Expand Up @@ -46,14 +45,12 @@ impl ModelConvertFactor {
}
}

impl Factor for ModelConvertFactor {
fn residual_func(
&self,
params: &[nalgebra::DVector<num_dual::DualDVec64>],
) -> nalgebra::DVector<num_dual::DualDVec64> {
let model = self.target.new_from_params(&params[0]);
let p2ds0 = self.source.project(&self.p3ds);
let p2ds1 = model.project(&self.p3ds);
impl<T: na::RealField> Factor<T> for ModelConvertFactor {
fn residual_func(&self, params: &[nalgebra::DVector<T>]) -> nalgebra::DVector<T> {
let model = self.target.cast::<T>().new_from_params(&params[0]);
let p3d_template: Vec<_> = self.p3ds.iter().map(|&p| p.cast::<T>()).collect();
let p2ds0 = self.source.cast::<T>().project(&p3d_template);
let p2ds1 = model.project(&p3d_template);
let diff: Vec<_> = p2ds0
.iter()
.zip(p2ds1)
Expand All @@ -64,20 +61,17 @@ impl Factor for ModelConvertFactor {
return vec![pp[0].clone(), pp[1].clone()];
}
}
vec![
num_dual::DualDVec64::from_re(0.0),
num_dual::DualDVec64::from_re(0.0),
]
vec![T::from_f64(10000.0).unwrap(), T::from_f64(10000.0).unwrap()]
})
.collect();
na::DVector::from_vec(diff)
}
}

pub struct UCMInitFocalAlphaFactor {
pub target: GenericModel<DualDVec64>,
pub p3d: na::Point3<DualDVec64>,
pub p2d: na::Vector2<DualDVec64>,
pub target: GenericModel<f64>,
pub p3d: na::Point3<f64>,
pub p2d: na::Vector2<f64>,
}

impl UCMInitFocalAlphaFactor {
Expand All @@ -92,35 +86,32 @@ impl UCMInitFocalAlphaFactor {
UCMInitFocalAlphaFactor { target, p3d, p2d }
}
}
impl Factor for UCMInitFocalAlphaFactor {
fn residual_func(
&self,
params: &[nalgebra::DVector<num_dual::DualDVec64>],
) -> nalgebra::DVector<num_dual::DualDVec64> {
impl<T: na::RealField> Factor<T> for UCMInitFocalAlphaFactor {
fn residual_func(&self, params: &[nalgebra::DVector<T>]) -> nalgebra::DVector<T> {
// params[[f, alpha], rvec, tvec]
let mut cam_params = self.target.params();
let mut cam_params = self.target.cast::<T>().params();
cam_params[0] = params[0][0].clone();
cam_params[1] = params[0][0].clone();
cam_params[4] = params[0][1].clone();
let model = self.target.new_from_params(&cam_params);
let model = self.target.cast().new_from_params(&cam_params);
let rvec = params[1].to_vec3();
let tvec = params[2].to_vec3();
let transform = na::Isometry3::new(tvec, rvec);
let p3d_t = transform * self.p3d.clone();
let p3d_t = transform * self.p3d.cast();
let p3d_t = na::Vector3::new(p3d_t.x.clone(), p3d_t.y.clone(), p3d_t.z.clone());
let p2d_p = model.project_one(&p3d_t);

let p2d_tp = self.p2d.cast::<T>();
na::dvector![
p2d_p[0].clone() - self.p2d[0].clone(),
p2d_p[1].clone() - self.p2d[1].clone()
p2d_p[0].clone() - p2d_tp[0].clone(),
p2d_p[1].clone() - p2d_tp[1].clone()
]
}
}

pub struct ReprojectionFactor {
pub target: GenericModel<DualDVec64>,
pub p3d: na::Point3<DualDVec64>,
pub p2d: na::Vector2<DualDVec64>,
pub target: GenericModel<f64>,
pub p3d: na::Point3<f64>,
pub p2d: na::Vector2<f64>,
pub xy_same_focal: bool,
}

Expand All @@ -142,35 +133,33 @@ impl ReprojectionFactor {
}
}
}
impl Factor for ReprojectionFactor {
fn residual_func(
&self,
params: &[nalgebra::DVector<num_dual::DualDVec64>],
) -> nalgebra::DVector<num_dual::DualDVec64> {
impl<T: na::RealField> Factor<T> for ReprojectionFactor {
fn residual_func(&self, params: &[nalgebra::DVector<T>]) -> nalgebra::DVector<T> {
// params[params, rvec, tvec]
let mut params0 = params[0].clone();
if self.xy_same_focal {
params0 = params0.clone().insert_row(1, params0[0].clone());
}
let model = self.target.new_from_params(&params0);
let model = self.target.cast().new_from_params(&params0);
let rvec = params[1].to_vec3();
let tvec = params[2].to_vec3();
let transform = na::Isometry3::new(tvec, rvec);
let p3d_t = transform * self.p3d.clone();
let p3d_t = transform * self.p3d.cast();
let p3d_t = na::Vector3::new(p3d_t.x.clone(), p3d_t.y.clone(), p3d_t.z.clone());
let p2d_p = model.project_one(&p3d_t);

let p2d_tp = self.p2d.cast::<T>();
na::dvector![
p2d_p[0].clone() - self.p2d[0].clone(),
p2d_p[1].clone() - self.p2d[1].clone()
p2d_p[0].clone() - p2d_tp[0].clone(),
p2d_p[1].clone() - p2d_tp[1].clone()
]
}
}

pub struct OtherCamReprojectionFactor {
pub target: GenericModel<DualDVec64>,
pub p3d: na::Point3<DualDVec64>,
pub p2d: na::Vector2<DualDVec64>,
pub target: GenericModel<f64>,
pub p3d: na::Point3<f64>,
pub p2d: na::Vector2<f64>,
pub xy_same_focal: bool,
}

Expand All @@ -192,37 +181,35 @@ impl OtherCamReprojectionFactor {
}
}
}
impl Factor for OtherCamReprojectionFactor {
fn residual_func(
&self,
params: &[nalgebra::DVector<num_dual::DualDVec64>],
) -> nalgebra::DVector<num_dual::DualDVec64> {
impl<T: na::RealField> Factor<T> for OtherCamReprojectionFactor {
fn residual_func(&self, params: &[nalgebra::DVector<T>]) -> nalgebra::DVector<T> {
// params[params, rvec, tvec]
let mut params0 = params[0].clone();
if self.xy_same_focal {
params0 = params0.clone().insert_row(1, params0[0].clone());
}
let model = self.target.new_from_params(&params0);
let model = self.target.cast().new_from_params(&params0);
let rvec0 = params[1].to_vec3();
let tvec0 = params[2].to_vec3();
let t_0_b = na::Isometry3::new(tvec0, rvec0);
let rvec1 = params[3].to_vec3();
let tvec1 = params[4].to_vec3();
let t_i_0 = na::Isometry3::new(tvec1, rvec1);
let p3d_t = t_i_0 * t_0_b * self.p3d.clone();
let p3d_t = t_i_0 * t_0_b * self.p3d.cast();
let p3d_t = na::Vector3::new(p3d_t.x.clone(), p3d_t.y.clone(), p3d_t.z.clone());
let p2d_p = model.project_one(&p3d_t);

let p2d_tp = self.p2d.cast::<T>();
na::dvector![
p2d_p[0].clone() - self.p2d[0].clone(),
p2d_p[1].clone() - self.p2d[1].clone()
p2d_p[0].clone() - p2d_tp[0].clone(),
p2d_p[1].clone() - p2d_tp[1].clone()
]
}
}

pub struct SE3Factor {
pub t_0_b: na::Isometry3<DualDVec64>,
pub t_i_b: na::Isometry3<DualDVec64>,
pub t_0_b: na::Isometry3<f64>,
pub t_i_b: na::Isometry3<f64>,
}

impl SE3Factor {
Expand All @@ -234,11 +221,8 @@ impl SE3Factor {
}
}

impl Factor for SE3Factor {
fn residual_func(
&self,
params: &[nalgebra::DVector<num_dual::DualDVec64>],
) -> nalgebra::DVector<num_dual::DualDVec64> {
impl<T: na::RealField> Factor<T> for SE3Factor {
fn residual_func(&self, params: &[nalgebra::DVector<T>]) -> nalgebra::DVector<T> {
let rvec = na::Vector3::new(
params[0][0].clone(),
params[0][1].clone(),
Expand All @@ -250,7 +234,7 @@ impl Factor for SE3Factor {
params[1][2].clone(),
);
let t_i_0 = na::Isometry3::new(tvec, rvec);
let t_diff = self.t_i_b.inverse() * t_i_0 * self.t_0_b.clone();
let t_diff = self.t_i_b.cast().inverse() * t_i_0 * self.t_0_b.cast();
let r_diff = t_diff.rotation.scaled_axis();
na::dvector![
r_diff[0].clone(),
Expand Down
42 changes: 15 additions & 27 deletions src/util.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -228,7 +228,7 @@ pub fn convert_model(
let cost = ModelConvertFactor::new(source_model, target_model, edge_pixels, steps as usize);
problem.add_residual_block(
cost.residaul_num(),
vec![("params".to_string(), target_model.params().len())],
&[("params", target_model.params().len())],
Box::new(cost),
Some(Box::new(HuberLoss::new(1.0))),
);
Expand Down Expand Up @@ -286,11 +286,7 @@ pub fn init_ucm(
let cost = UCMInitFocalAlphaFactor::new(&ucm_init_model, &fp.p3d, &fp.p2d);
init_focal_alpha_problem.add_residual_block(
2,
vec![
("params".to_string(), 2),
("rvec0".to_string(), 3),
("tvec0".to_string(), 3),
],
&[("params", 2), ("rvec0", 3), ("tvec0", 3)],
Box::new(cost),
Some(Box::new(HuberLoss::new(1.0))),
);
Expand All @@ -300,11 +296,7 @@ pub fn init_ucm(
let cost = UCMInitFocalAlphaFactor::new(&ucm_init_model, &fp.p3d, &fp.p2d);
init_focal_alpha_problem.add_residual_block(
2,
vec![
("params".to_string(), 2),
("rvec1".to_string(), 3),
("tvec1".to_string(), 3),
],
&[("params", 2), ("rvec1", 3), ("tvec1", 3)],
Box::new(cost),
Some(Box::new(HuberLoss::new(1.0))),
);
Expand Down Expand Up @@ -390,11 +382,7 @@ pub fn calib_camera(
let cost = ReprojectionFactor::new(generic_camera, &fp.p3d, &fp.p2d, xy_same_focal);
problem.add_residual_block(
2,
vec![
("params".to_string(), params_len),
(rvec_name.clone(), 3),
(tvec_name.clone(), 3),
],
&[("params", params_len), (&rvec_name, 3), (&tvec_name, 3)],
Box::new(cost),
Some(Box::new(HuberLoss::new(1.0))),
);
Expand Down Expand Up @@ -509,7 +497,7 @@ pub fn init_camera_extrinsic(cam_rtvecs: &[HashMap<usize, RvecTvec>]) -> Vec<Rve
let cost = SE3Factor::new(t_0_b, t_i_b);
problem.add_residual_block(
6,
vec![("rvec".to_string(), 3), ("tvec".to_string(), 3)],
&[("rvec", 3), ("tvec", 3)],
Box::new(cost),
Some(Box::new(HuberLoss::new(0.5))),
);
Expand Down Expand Up @@ -577,10 +565,10 @@ pub fn calib_all_camera_with_extrinsics(
ReprojectionFactor::new(generic_camera, &fp.p3d, &fp.p2d, xy_same_focal);
problem.add_residual_block(
2,
vec![
(params_name.clone(), params_len),
(rvec_0_b_name.clone(), 3),
(tvec_0_b_name.clone(), 3),
&[
(&params_name, params_len),
(&rvec_0_b_name, 3),
(&tvec_0_b_name, 3),
],
Box::new(cost),
Some(Box::new(HuberLoss::new(1.0))),
Expand All @@ -594,12 +582,12 @@ pub fn calib_all_camera_with_extrinsics(
);
problem.add_residual_block(
2,
vec![
(params_name.clone(), params_len),
(rvec_0_b_name.clone(), 3),
(tvec_0_b_name.clone(), 3),
(rvec_i_0_name.clone(), 3),
(tvec_i_0_name.clone(), 3),
&[
(&params_name, params_len),
(&rvec_0_b_name, 3),
(&tvec_0_b_name, 3),
(&rvec_i_0_name, 3),
(&tvec_i_0_name, 3),
],
Box::new(cost),
Some(Box::new(HuberLoss::new(1.0))),
Expand Down

0 comments on commit 3af23da

Please sign in to comment.