From 3af23dafc2e7378cf853d11392d82c2a01c310f4 Mon Sep 17 00:00:00 2001 From: Powei Lin Date: Sun, 15 Dec 2024 23:28:47 -0500 Subject: [PATCH] remove num dual (#6) --- Cargo.toml | 7 +-- src/optimization/factors.rs | 108 +++++++++++++++--------------------- src/util.rs | 42 +++++--------- 3 files changed, 64 insertions(+), 93 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 0b2edd2..e15e030 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "camera-intrinsic-calibration" -version = "0.4.3" +version = "0.5.0" edition = "2021" authors = ["Powei Lin "] readme = "README.md" @@ -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" diff --git a/src/optimization/factors.rs b/src/optimization/factors.rs index 6760d3c..595961d 100644 --- a/src/optimization/factors.rs +++ b/src/optimization/factors.rs @@ -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, - pub target: GenericModel, - pub p3ds: Vec>, + pub source: GenericModel, + pub target: GenericModel, + pub p3ds: Vec>, } impl ModelConvertFactor { @@ -46,14 +45,12 @@ impl ModelConvertFactor { } } -impl Factor for ModelConvertFactor { - fn residual_func( - &self, - params: &[nalgebra::DVector], - ) -> nalgebra::DVector { - let model = self.target.new_from_params(¶ms[0]); - let p2ds0 = self.source.project(&self.p3ds); - let p2ds1 = model.project(&self.p3ds); +impl Factor for ModelConvertFactor { + fn residual_func(&self, params: &[nalgebra::DVector]) -> nalgebra::DVector { + let model = self.target.cast::().new_from_params(¶ms[0]); + let p3d_template: Vec<_> = self.p3ds.iter().map(|&p| p.cast::()).collect(); + let p2ds0 = self.source.cast::().project(&p3d_template); + let p2ds1 = model.project(&p3d_template); let diff: Vec<_> = p2ds0 .iter() .zip(p2ds1) @@ -64,10 +61,7 @@ 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) @@ -75,9 +69,9 @@ impl Factor for ModelConvertFactor { } pub struct UCMInitFocalAlphaFactor { - pub target: GenericModel, - pub p3d: na::Point3, - pub p2d: na::Vector2, + pub target: GenericModel, + pub p3d: na::Point3, + pub p2d: na::Vector2, } impl UCMInitFocalAlphaFactor { @@ -92,35 +86,32 @@ impl UCMInitFocalAlphaFactor { UCMInitFocalAlphaFactor { target, p3d, p2d } } } -impl Factor for UCMInitFocalAlphaFactor { - fn residual_func( - &self, - params: &[nalgebra::DVector], - ) -> nalgebra::DVector { +impl Factor for UCMInitFocalAlphaFactor { + fn residual_func(&self, params: &[nalgebra::DVector]) -> nalgebra::DVector { // params[[f, alpha], rvec, tvec] - let mut cam_params = self.target.params(); + let mut cam_params = self.target.cast::().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::(); 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, - pub p3d: na::Point3, - pub p2d: na::Vector2, + pub target: GenericModel, + pub p3d: na::Point3, + pub p2d: na::Vector2, pub xy_same_focal: bool, } @@ -142,35 +133,33 @@ impl ReprojectionFactor { } } } -impl Factor for ReprojectionFactor { - fn residual_func( - &self, - params: &[nalgebra::DVector], - ) -> nalgebra::DVector { +impl Factor for ReprojectionFactor { + fn residual_func(&self, params: &[nalgebra::DVector]) -> nalgebra::DVector { // 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(¶ms0); + let model = self.target.cast().new_from_params(¶ms0); 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::(); 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, - pub p3d: na::Point3, - pub p2d: na::Vector2, + pub target: GenericModel, + pub p3d: na::Point3, + pub p2d: na::Vector2, pub xy_same_focal: bool, } @@ -192,37 +181,35 @@ impl OtherCamReprojectionFactor { } } } -impl Factor for OtherCamReprojectionFactor { - fn residual_func( - &self, - params: &[nalgebra::DVector], - ) -> nalgebra::DVector { +impl Factor for OtherCamReprojectionFactor { + fn residual_func(&self, params: &[nalgebra::DVector]) -> nalgebra::DVector { // 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(¶ms0); + let model = self.target.cast().new_from_params(¶ms0); 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::(); 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, - pub t_i_b: na::Isometry3, + pub t_0_b: na::Isometry3, + pub t_i_b: na::Isometry3, } impl SE3Factor { @@ -234,11 +221,8 @@ impl SE3Factor { } } -impl Factor for SE3Factor { - fn residual_func( - &self, - params: &[nalgebra::DVector], - ) -> nalgebra::DVector { +impl Factor for SE3Factor { + fn residual_func(&self, params: &[nalgebra::DVector]) -> nalgebra::DVector { let rvec = na::Vector3::new( params[0][0].clone(), params[0][1].clone(), @@ -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(), diff --git a/src/util.rs b/src/util.rs index f0b7a25..0482329 100644 --- a/src/util.rs +++ b/src/util.rs @@ -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))), ); @@ -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))), ); @@ -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))), ); @@ -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))), ); @@ -509,7 +497,7 @@ pub fn init_camera_extrinsic(cam_rtvecs: &[HashMap]) -> Vec