Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

remove num dual #6

Merged
merged 1 commit into from
Dec 16, 2024
Merged

remove num dual #6

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 3 additions & 4 deletions Cargo.toml
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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