Add general dataset, board config, one focal, disable some distortions

Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "camera-intrinsic-calibration"
-version = "0.1.4"
+version = "0.1.5"
 edition = "2021"
 authors = ["Powei Lin <poweilin1994@gmail.com>"]
 readme = "README.md"
@@ -28,6 +28,8 @@ 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_json = "1.0.133"
 sqpnp_simple = "0.1.3"
 tiny-solver = "0.10.0"
 

README.md

@@ -32,6 +32,14 @@ ccrs dataset-calib-cam1_1024_16 --model eucm
             │   └── {time_stamp}.png
             └── data.csv
     ```
+* General `--dataset-format general`
+    ```
+    dataset_root
+    └── cam0
+        ├── any_file_name.png
+        ├── any_file_name.png
+        └── any_file_name.png
+    ```
 ### Camera models
 * Extended Unified (EUCM)
 * Extended Unified with Tangential (EUCMT)

data/board_config.json

@@ -0,0 +1,6 @@
+{
+  "tag_size_meter": 0.088,
+  "tag_spacing": 0.3,
+  "tag_rows": 5,
+  "tag_cols": 9
+}

examples/convert_model.rs

@@ -21,7 +21,7 @@ fn main() {
         source_model.width().round() as u32,
         source_model.height().round() as u32,
     ));
-    convert_model(&source_model, &mut target_model);
+    convert_model(&source_model, &mut target_model, 0);
     model_to_json("ucm.json", &target_model);
     let new_w_h = 1024;
     let p = target_model.estimate_new_camera_matrix_for_undistort(1.0, Some((new_w_h, new_w_h)));

src/bin/camera_calibration.rs

@@ -1,16 +1,25 @@
 use aprilgrid::detector::TagDetector;
 use aprilgrid::TagFamily;
-use camera_intrinsic_calibration::board::create_default_6x6_board;
-use camera_intrinsic_calibration::data_loader::load_euroc;
+use camera_intrinsic_calibration::board::Board;
+use camera_intrinsic_calibration::board::{
+    board_config_from_json, board_config_to_json, BoardConfig,
+};
+use camera_intrinsic_calibration::data_loader::{load_euroc, load_general};
 use camera_intrinsic_calibration::optimization::*;
 use camera_intrinsic_calibration::types::RvecTvec;
 use camera_intrinsic_calibration::util::*;
 use camera_intrinsic_calibration::visualization::*;
 use camera_intrinsic_model::*;
-use clap::Parser;
+use clap::{Parser, ValueEnum};
 use log::trace;
 use std::time::Instant;
 
+#[derive(Debug, Clone, Copy, ValueEnum)]
+enum DatasetFormat {
+    Euroc,
+    General,
+}
+
 #[derive(Parser)]
 #[command(version, about, author)]
 struct CCRSCli {
@@ -34,49 +43,84 @@ struct CCRSCli {
     #[arg(long, default_value_t = 600)]
     max_images: usize,
 
+    #[arg(long, default_value_t = 1)]
+    cam_num: usize,
+
+    #[arg(long)]
+    board_config: Option<String>,
+
     #[arg(short, long, default_value = "output.json")]
     output_json: String,
+
+    #[arg(long, value_enum, default_value = "euroc")]
+    dataset_format: DatasetFormat,
+
+    #[arg(long, action)]
+    one_focal: bool,
+
+    #[arg(long, default_value_t = 0)]
+    disabled_distortion_num: usize,
 }
 
 fn main() {
     env_logger::init();
     let cli = CCRSCli::parse();
     let detector = TagDetector::new(&cli.tag_family, None);
-    let board = create_default_6x6_board();
+    let board = if let Some(board_config_path) = cli.board_config {
+        Board::from_config(&board_config_from_json(&board_config_path))
+    } else {
+        let config = BoardConfig::default();
+        board_config_to_json("default_board_config.json", &config);
+        Board::from_config(&config)
+    };
     let dataset_root = &cli.path;
     let now = Instant::now();
     let recording = rerun::RecordingStreamBuilder::new("calibration")
         .save("output.rrd")
         .unwrap();
     trace!("Start loading data");
-    let mut detected_feature_frames = load_euroc(
-        dataset_root,
-        &detector,
-        &board,
-        cli.start_idx,
-        cli.step,
-        Some(&recording),
-    );
-    detected_feature_frames.truncate(cli.max_images);
+    let mut cams_detected_feature_frames = match cli.dataset_format {
+        DatasetFormat::Euroc => load_euroc(
+            dataset_root,
+            &detector,
+            &board,
+            cli.start_idx,
+            cli.step,
+            cli.cam_num,
+            Some(&recording),
+        ),
+        DatasetFormat::General => load_general(
+            dataset_root,
+            &detector,
+            &board,
+            cli.start_idx,
+            cli.step,
+            cli.cam_num,
+            Some(&recording),
+        ),
+    };
     let duration_sec = now.elapsed().as_secs_f64();
     println!("detecting feature took {:.6} sec", duration_sec);
+    println!("total: {} images", cams_detected_feature_frames[0].len());
+    cams_detected_feature_frames[0].truncate(cli.max_images);
     println!(
         "avg: {} sec",
-        duration_sec / detected_feature_frames.len() as f64
+        duration_sec / cams_detected_feature_frames[0].len() as f64
     );
-    log_frames(&recording, &detected_feature_frames);
-    let (frame0, frame1) = find_best_two_frames(&detected_feature_frames);
-    let key_frames = vec![
-        detected_feature_frames[frame0].clone(),
-        detected_feature_frames[frame1].clone(),
-    ];
-    log_frames(&recording, &key_frames);
+    for (cam_idx, feature_frames) in cams_detected_feature_frames.iter().enumerate() {
+        let topic = format!("/cam{}", cam_idx);
+        log_feature_frames(&recording, &topic, feature_frames);
+    }
+    let (frame0, frame1) = find_best_two_frames(&cams_detected_feature_frames[0]);
+
+    let frame_feature0 = &cams_detected_feature_frames[0][frame0].clone().unwrap();
+    let frame_feature1 = &cams_detected_feature_frames[0][frame1].clone().unwrap();
+
+    let key_frames = vec![Some(frame_feature0.clone()), Some(frame_feature1.clone())];
+    log_feature_frames(&recording, "/cam0/key", &key_frames);
 
     // initialize focal length and undistorted p2d for init poses
-    let (lambda, h_mat) = radial_distortion_homography(
-        &detected_feature_frames[frame0],
-        &detected_feature_frames[frame1],
-    );
+    let (lambda, h_mat) = radial_distortion_homography(frame_feature0, frame_feature1);
     // focal
     let f_option = homography_to_focal(&h_mat);
     if f_option.is_none() {
@@ -86,8 +130,6 @@ fn main() {
     println!("focal {}", focal);
 
     // poses
-    let frame_feature0 = &detected_feature_frames[frame0];
-    let frame_feature1 = &detected_feature_frames[frame1];
     let (rvec0, tvec0) = rtvec_to_na_dvec(init_pose(frame_feature0, lambda));
     let (rvec1, tvec1) = rtvec_to_na_dvec(init_pose(frame_feature1, lambda));
     let rtvec0 = RvecTvec::new(rvec0, tvec0);
@@ -107,16 +149,25 @@ fn main() {
         init_f,
         init_alpha,
     );
+    println!("Initialized {:?}", initial_camera);
     let mut final_model = cli.model;
     final_model.set_w_h(
         initial_camera.width().round() as u32,
         initial_camera.height().round() as u32,
     );
-    println!("{:?}", final_model);
-    convert_model(&initial_camera, &mut final_model);
-    println!("{:?}", final_model);
+    convert_model(
+        &initial_camera,
+        &mut final_model,
+        cli.disabled_distortion_num,
+    );
+    println!("Converted {:?}", final_model);
 
-    let (final_result, _rtvec_list) = calib_camera(&detected_feature_frames, &final_model);
-    println!("{:?}", final_result);
+    let (final_result, _rtvec_list) = calib_camera(
+        &cams_detected_feature_frames[0],
+        &final_model,
+        cli.one_focal,
+        cli.disabled_distortion_num,
+    );
+    println!("Final {:?}", final_result);
     model_to_json(&cli.output_json, &final_result);
 }

src/board.rs

@@ -1,11 +1,51 @@
 use glam;
-use std::collections::HashMap;
+use serde::{Deserialize, Serialize};
+use std::{collections::HashMap, io::Write};
+
+#[derive(Debug, Serialize, Deserialize)]
+pub struct BoardConfig {
+    tag_size_meter: f32,
+    tag_spacing: f32,
+    tag_rows: usize,
+    tag_cols: usize,
+}
+
+pub fn board_config_to_json(output_path: &str, board_config: &BoardConfig) {
+    let j = serde_json::to_string_pretty(board_config).unwrap();
+    let mut file = std::fs::File::create(output_path).unwrap();
+    file.write_all(j.as_bytes()).unwrap();
+}
+
+pub fn board_config_from_json(file_path: &str) -> BoardConfig {
+    let contents =
+        std::fs::read_to_string(file_path).expect("Should have been able to read the file");
+    serde_json::from_str(&contents).unwrap()
+}
+
+impl Default for BoardConfig {
+    fn default() -> Self {
+        Self {
+            tag_size_meter: 0.088,
+            tag_spacing: 0.3,
+            tag_rows: 6,
+            tag_cols: 6,
+        }
+    }
+}
 
 pub struct Board {
     pub id_to_3d: HashMap<u32, glam::Vec3>,
 }
 
 impl Board {
+    pub fn from_config(board_config: &BoardConfig) -> Board {
+        Self::init_aprilgrid(
+            board_config.tag_size_meter,
+            board_config.tag_spacing,
+            board_config.tag_rows,
+            board_config.tag_cols,
+        )
+    }
     pub fn init_aprilgrid(
         tag_size_meter: f32,
         tag_spacing: f32,