From 240ea25c3fff78c447b5926d9150337186234b61 Mon Sep 17 00:00:00 2001 From: Powei Lin Date: Mon, 25 Nov 2024 18:57:27 -0500 Subject: [PATCH 1/2] save --- src/bin/camera_calibration.rs | 42 +++++++++++++++-- src/util.rs | 85 ++++++++++++++++++++++++++--------- 2 files changed, 102 insertions(+), 25 deletions(-) diff --git a/src/bin/camera_calibration.rs b/src/bin/camera_calibration.rs index 1b41364..a33d757 100644 --- a/src/bin/camera_calibration.rs +++ b/src/bin/camera_calibration.rs @@ -3,20 +3,20 @@ use aprilgrid::TagFamily; use camera_intrinsic::board::create_default_6x6_board; use camera_intrinsic::data_loader::load_euroc; use camera_intrinsic::detected_points::{FeaturePoint, FrameFeature}; -use camera_intrinsic::util::init_ucm; +use camera_intrinsic::util::{init_ucm, RandomPnpFactor}; use camera_intrinsic::visualization::*; use clap::Parser; use core::f32; -use faer::col::ColBatch; use faer::solvers::SpSolverLstsq; use glam::Vec2; use log::trace; use nalgebra as na; use rand::seq::SliceRandom; -use rand::Rng; use rerun::RecordingStream; use std::collections::HashMap; use std::time::Instant; +use tiny_solver::loss_functions::HuberLoss; +use tiny_solver::Optimizer; #[derive(Parser)] #[command(version, about, author)] @@ -345,6 +345,31 @@ fn radial_distortion_homography( (best_lambda, best_H) } +fn random_pnp(normalized_undistort_pt_pair: &[(glam::Vec2, glam::Vec3)]) { + let mut problem = tiny_solver::Problem::new(); + for (p2d, p3d) in normalized_undistort_pt_pair { + let cost = RandomPnpFactor::new(p2d, p3d); + problem.add_residual_block( + 2, + vec![("rvec".to_string(), 3), ("tvec".to_string(), 3)], + Box::new(cost), + Some(Box::new(HuberLoss::new(1.0))), + ); + } + + let initial_values = HashMap::>::from([ + ("rvec".to_string(), na::dvector![0.0, 0.0, 0.0]), + ("tvec".to_string(), na::dvector![0.0, 0.0, 0.0]), + ]); + + // initialize optimizer + let optimizer = tiny_solver::GaussNewtonOptimizer {}; + + // optimize + let result = optimizer.optimize(&problem, &initial_values, None); + println!("{:?}", result); +} + fn main() { env_logger::init(); let cli = CCRSCli::parse(); @@ -380,6 +405,17 @@ fn main() { let half_h = frame_feature0.img_w_h.1 as f32 / 2.0; let half_img_size = half_h.max(half_w); let cxcy = glam::Vec2::new(half_w, half_h); + let normalized_undistort_frame_feauture0: Vec<_> = frame_feature0 + .features + .iter() + .map(|f| { + let xy = (f.1.p2d - cxcy) / half_img_size; + let sc = 1.0 + lambda * (xy.x * xy.x + xy.y * xy.y); + (xy / sc, f.1.p3d) + }) + .collect(); + random_pnp(&normalized_undistort_frame_feauture0); + return; let normalized_p2d_pairs: Vec<_> = frame_feature0 .features .iter() diff --git a/src/util.rs b/src/util.rs index 69a6622..d13223f 100644 --- a/src/util.rs +++ b/src/util.rs @@ -151,28 +151,28 @@ impl CustomFactor { } } -impl Factor for CustomFactor { - fn residual_func( - &self, - params: &[nalgebra::DVector], - ) -> nalgebra::DVector { - // let f = params[0][0].clone(); - // let alpha = params[0][1].clone(); - let f = DualDVec64::from_re(189.0); - let alpha = DualDVec64::from_re(0.6); - let cx = DualDVec64::from_re(self.img_w as f64 / 2.0); - let cy = DualDVec64::from_re(self.img_h as f64 / 2.0); - let new_params = na::dvector![f.clone(), f, cx, cy, alpha]; - let ucm = UCM::new(&new_params, self.img_w, self.img_h); - let h_flat = params[0].push(DualDVec64::from_re(1.0)); - let h = h_flat.reshape_generic(Const::<3>, Dyn(3)); - let p3d0 = ucm.unproject_one(&self.p2d0); - let p3d1 = h * p3d0; - let p2d1p = ucm.project_one(&p3d1); - let diff = p2d1p - self.p2d1.clone(); - na::dvector![diff[0].clone(), diff[1].clone()] - } -} +// impl Factor for CustomFactor { +// fn residual_func( +// &self, +// params: &[nalgebra::DVector], +// ) -> nalgebra::DVector { +// // let f = params[0][0].clone(); +// // let alpha = params[0][1].clone(); +// let f = DualDVec64::from_re(189.0); +// let alpha = DualDVec64::from_re(0.6); +// let cx = DualDVec64::from_re(self.img_w as f64 / 2.0); +// let cy = DualDVec64::from_re(self.img_h as f64 / 2.0); +// let new_params = na::dvector![f.clone(), f, cx, cy, alpha]; +// let ucm = UCM::new(&new_params, self.img_w, self.img_h); +// let h_flat = params[0].push(DualDVec64::from_re(1.0)); +// let h = h_flat.reshape_generic(Const::<3>, Dyn(3)); +// let p3d0 = ucm.unproject_one(&self.p2d0); +// let p3d1 = h * p3d0; +// let p2d1p = ucm.project_one(&p3d1); +// let diff = p2d1p - self.p2d1.clone(); +// na::dvector![diff[0].clone(), diff[1].clone()] +// } +// } pub fn init_ucm(p2d_pairs: &[(glam::Vec2, glam::Vec2)], img_w: u32, img_h: u32) { let mut problem = tiny_solver::Problem::new(); @@ -204,3 +204,44 @@ pub fn init_ucm(p2d_pairs: &[(glam::Vec2, glam::Vec2)], img_w: u32, img_h: u32) // save result // let result_params = result.get("x").unwrap(); } + +pub struct RandomPnpFactor { + p2d: na::Vector2, + p3d: na::Point3, +} +impl RandomPnpFactor { + pub fn new(p2d: &glam::Vec2, p3d: &glam::Vec3) -> RandomPnpFactor { + let p2d = na::Vector2::new( + DualDVec64::from_re(p2d.x as f64), + DualDVec64::from_re(p2d.y as f64), + ); + let p3d = na::Point3::new( + DualDVec64::from_re(p3d.x as f64), + DualDVec64::from_re(p3d.y as f64), + DualDVec64::from_re(p3d.z as f64), + ); + RandomPnpFactor { p2d, p3d } + } +} +impl Factor for RandomPnpFactor { + fn residual_func( + &self, + params: &[nalgebra::DVector], + ) -> nalgebra::DVector { + let rvec = na::Vector3::new( + params[0][0].clone(), + params[0][1].clone(), + params[0][2].clone(), + ); + let tvec = na::Vector3::new( + params[1][0].clone(), + params[1][1].clone(), + params[1][2].clone(), + ); + let transform = na::Isometry3::new(tvec, rvec); + let p3dp = transform * self.p3d.clone(); + let x = p3dp.x.clone() / p3dp.z.clone(); + let y = p3dp.y.clone() / p3dp.z.clone(); + na::dvector![x - self.p2d.x.clone(), y - self.p2d.y.clone()] + } +} From 144780b324196ca625b791c2a3f4f71fb74274e1 Mon Sep 17 00:00:00 2001 From: Powei Lin Date: Mon, 25 Nov 2024 21:04:50 -0500 Subject: [PATCH 2/2] focal --- src/bin/camera_calibration.rs | 71 ++++++++++++++++++++++++++++++++--- src/util.rs | 44 +++++++++++----------- 2 files changed, 87 insertions(+), 28 deletions(-) diff --git a/src/bin/camera_calibration.rs b/src/bin/camera_calibration.rs index a33d757..a2e5e1b 100644 --- a/src/bin/camera_calibration.rs +++ b/src/bin/camera_calibration.rs @@ -93,7 +93,7 @@ fn find_best_two_frames(detected_feature_frames: &[FrameFeature]) -> (usize, usi .collect(); let avg_all = v.iter().map(|(_, p)| *p).reduce(|acc, e| acc + e).unwrap() / v.len() as f32; - let avg_all = Vec2::ZERO; + // let avg_all = Vec2::ZERO; v.sort_by(|a, b| { vec2_distance2(&a.1, &avg_all) .partial_cmp(&vec2_distance2(&b.1, &avg_all)) @@ -358,8 +358,8 @@ fn random_pnp(normalized_undistort_pt_pair: &[(glam::Vec2, glam::Vec3)]) { } let initial_values = HashMap::>::from([ - ("rvec".to_string(), na::dvector![0.0, 0.0, 0.0]), - ("tvec".to_string(), na::dvector![0.0, 0.0, 0.0]), + ("rvec".to_string(), na::dvector![0.0, 0.0, 0.01]), + ("tvec".to_string(), na::dvector![-0.6, 0.0, 0.22,]), ]); // initialize optimizer @@ -370,6 +370,62 @@ fn random_pnp(normalized_undistort_pt_pair: &[(glam::Vec2, glam::Vec3)]) { println!("{:?}", result); } +fn homography_to_focal(h_mat: &na::Matrix3) -> Option { + let h0 = h_mat[(0, 0)]; + let h1 = h_mat[(0, 1)]; + let h2 = h_mat[(0, 2)]; + let h3 = h_mat[(1, 0)]; + let h4 = h_mat[(1, 1)]; + let h5 = h_mat[(1, 2)]; + let h6 = h_mat[(2, 0)]; + let h7 = h_mat[(2, 1)]; + let h8 = h_mat[(2, 2)]; + + let d1 = h6 * h7; + let d2 = (h7 - h6) * (h7 + h6); + let v1 = -(h0 * h1 + h3 * h4) / d1; + let v2 = (h0 * h0 + h3 * h3 - h1 * h1 - h4 * h4) / d2; + let (v1, v2) = if (v1 < v2) { (v2, v1) } else { (v1, v2) }; + + let f1 = if (v1 > 0.0 && v2 > 0.0) { + if d1.abs() > d2.abs() { + Some(v1.sqrt()) + } else { + Some(v2.sqrt()) + } + } else if v1 > 0.0 { + Some(v1.sqrt()) + } else { + None + }; + + let d1 = h0 * h3 + h1 * h4; + let d2 = h0 * h0 + h1 * h1 - h3 * h3 - h4 * h4; + let v1 = -h2 * h5 / d1; + let v2 = (h5 * h5 - h2 * h2) / d2; + let (v1, v2) = if (v1 < v2) { (v2, v1) } else { (v1, v2) }; + let f0 = if (v1 > 0.0 && v2 > 0.0) { + if d1.abs() > d2.abs() { + Some(v1.sqrt()) + } else { + Some(v2.sqrt()) + } + } else if (v1 > 0.0) { + Some(v1.sqrt()) + } else { + None + }; + if f0.is_some() && f1.is_some() { + Some((f0.unwrap() * f1.unwrap()).sqrt()) + } else if f0.is_some() { + f0 + } else if f1.is_some() { + f1 + } else { + None + } +} + fn main() { env_logger::init(); let cli = CCRSCli::parse(); @@ -381,7 +437,7 @@ fn main() { .save("output.rrd") .unwrap(); trace!("Start loading data"); - let detected_feature_frames = load_euroc(dataset_root, &detector, &board, Some(&recording)); + let detected_feature_frames = load_euroc(dataset_root, &detector, &board, None); let duration_sec = now.elapsed().as_secs_f64(); println!("detecting feature took {:.6} sec", duration_sec); println!( @@ -404,6 +460,10 @@ fn main() { let half_w = frame_feature0.img_w_h.0 as f32 / 2.0; let half_h = frame_feature0.img_w_h.1 as f32 / 2.0; let half_img_size = half_h.max(half_w); + let f_option = homography_to_focal(&h_mat); + if let Some(f) = f_option { + println!("f = {}", f * half_img_size) + } let cxcy = glam::Vec2::new(half_w, half_h); let normalized_undistort_frame_feauture0: Vec<_> = frame_feature0 .features @@ -411,10 +471,9 @@ fn main() { .map(|f| { let xy = (f.1.p2d - cxcy) / half_img_size; let sc = 1.0 + lambda * (xy.x * xy.x + xy.y * xy.y); - (xy / sc, f.1.p3d) + (xy / sc * half_img_size, f.1.p3d) }) .collect(); - random_pnp(&normalized_undistort_frame_feauture0); return; let normalized_p2d_pairs: Vec<_> = frame_feature0 .features diff --git a/src/util.rs b/src/util.rs index d13223f..ff8e3b8 100644 --- a/src/util.rs +++ b/src/util.rs @@ -151,28 +151,28 @@ impl CustomFactor { } } -// impl Factor for CustomFactor { -// fn residual_func( -// &self, -// params: &[nalgebra::DVector], -// ) -> nalgebra::DVector { -// // let f = params[0][0].clone(); -// // let alpha = params[0][1].clone(); -// let f = DualDVec64::from_re(189.0); -// let alpha = DualDVec64::from_re(0.6); -// let cx = DualDVec64::from_re(self.img_w as f64 / 2.0); -// let cy = DualDVec64::from_re(self.img_h as f64 / 2.0); -// let new_params = na::dvector![f.clone(), f, cx, cy, alpha]; -// let ucm = UCM::new(&new_params, self.img_w, self.img_h); -// let h_flat = params[0].push(DualDVec64::from_re(1.0)); -// let h = h_flat.reshape_generic(Const::<3>, Dyn(3)); -// let p3d0 = ucm.unproject_one(&self.p2d0); -// let p3d1 = h * p3d0; -// let p2d1p = ucm.project_one(&p3d1); -// let diff = p2d1p - self.p2d1.clone(); -// na::dvector![diff[0].clone(), diff[1].clone()] -// } -// } +impl Factor for CustomFactor { + fn residual_func( + &self, + params: &[nalgebra::DVector], + ) -> nalgebra::DVector { + // let f = params[0][0].clone(); + // let alpha = params[0][1].clone(); + let f = DualDVec64::from_re(189.0); + let alpha = DualDVec64::from_re(0.6); + let cx = DualDVec64::from_re(self.img_w as f64 / 2.0); + let cy = DualDVec64::from_re(self.img_h as f64 / 2.0); + let new_params = na::dvector![f.clone(), f, cx, cy, alpha]; + let ucm = UCM::new(&new_params, self.img_w, self.img_h); + let h_flat = params[0].push(DualDVec64::from_re(1.0)); + let h = h_flat.reshape_generic(Const::<3>, Dyn(3)); + let p3d0 = ucm.unproject_one(&self.p2d0); + let p3d1 = h * p3d0; + let p2d1p = ucm.project_one(&p3d1); + let diff = p2d1p - self.p2d1.clone(); + na::dvector![diff[0].clone(), diff[1].clone()] + } +} pub fn init_ucm(p2d_pairs: &[(glam::Vec2, glam::Vec2)], img_w: u32, img_h: u32) { let mut problem = tiny_solver::Problem::new();