Multicam support (#4)

* multi cam but individual

* ignore

* init extrinsic

* trait dvec vec3

* clippy

* params name

* initial extrinsic works

* still failed but fix one bug

* work but need to save the output

* need validation

* write extrinsic

* seperate result

* add io

* clean up

* clippy

* type

* type

* update image

.gitignore

@@ -28,4 +28,5 @@ Cargo.lock
 remaped.png
 .DS_Store
 output.json
-results/
+results/
+default_board_config.json

Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "camera-intrinsic-calibration"
-version = "0.3.0"
+version = "0.4.0"
 edition = "2021"
 authors = ["Powei Lin <poweilin1994@gmail.com>"]
 readme = "README.md"

data/board_config5x9.json

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

src/bin/camera_calibration.rs

@@ -5,11 +5,15 @@ use camera_intrinsic_calibration::board::{
     board_config_from_json, board_config_to_json, BoardConfig,
 };
 use camera_intrinsic_calibration::data_loader::{load_euroc, load_others};
+use camera_intrinsic_calibration::detected_points::FrameFeature;
+use camera_intrinsic_calibration::io::{extrinsics_to_json, write_report};
+use camera_intrinsic_calibration::types::{Extrinsics, RvecTvec, ToRvecTvec};
 use camera_intrinsic_calibration::util::*;
 use camera_intrinsic_calibration::visualization::*;
 use camera_intrinsic_model::*;
 use clap::{Parser, ValueEnum};
 use log::trace;
+use std::collections::HashMap;
 use std::time::Instant;
 use time::OffsetDateTime;
 
@@ -102,7 +106,8 @@ fn main() {
         .unwrap();
     trace!("Start loading data");
     println!("Start loading images and detecting charts.");
-    let mut cams_detected_feature_frames = match cli.dataset_format {
+    let mut cams_detected_feature_frames: Vec<Vec<Option<FrameFeature>>> = match cli.dataset_format
+    {
         DatasetFormat::Euroc => load_euroc(
             dataset_root,
             &detector,
@@ -125,81 +130,117 @@ fn main() {
     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);
+    cams_detected_feature_frames
+        .iter_mut()
+        .for_each(|f| f.truncate(cli.max_images));
     println!(
         "avg: {} sec",
         duration_sec / cams_detected_feature_frames[0].len() as f64
     );
-    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 (calibrated_intrinsics, cam_rtvecs): (Vec<_>, Vec<_>) = cams_detected_feature_frames
+        .iter()
+        .enumerate()
+        .map(|(cam_idx, feature_frames)| {
+            let topic = format!("/cam{}", cam_idx);
+            log_feature_frames(&recording, &topic, feature_frames);
+            let mut calibrated_result: Option<(GenericModel<f64>, HashMap<usize, RvecTvec>)> = None;
+            let max_trials = 3;
+            let cam0_fixed_focal = if cam_idx == 0 { cli.fixed_focal } else { None };
+            for _ in 0..max_trials {
+                calibrated_result = init_and_calibrate_one_camera(
+                    cam_idx,
+                    &cams_detected_feature_frames,
+                    &cli.model,
+                    &recording,
+                    cam0_fixed_focal,
+                    cli.disabled_distortion_num,
+                    cli.one_focal,
+                );
+                if calibrated_result.is_some() {
+                    break;
+                }
+            }
+            if calibrated_result.is_none() {
+                panic!(
+                    "Failed to calibrate cam{} after {} times",
+                    cam_idx, max_trials
+                );
+            }
+            let (final_result, rtvec_map) = calibrated_result.unwrap();
+            (final_result, rtvec_map)
+        })
+        .unzip();
+    let t_cam_i_0_init = init_camera_extrinsic(&cam_rtvecs);
+    for t in &t_cam_i_0_init {
+        println!("r {} t {}", t.na_rvec(), t.na_tvec());
     }
-    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 = [Some(frame_feature0.clone()), Some(frame_feature1.clone())];
-    key_frames.iter().enumerate().for_each(|(i, k)| {
-        let topic = format!("/cam0/keyframe{}", i);
-        recording.set_time_nanos("stable", k.clone().unwrap().time_ns);
-        recording
-            .log(topic, &rerun::TextLog::new("keyframe"))
-            .unwrap();
-    });
-
-    let mut initial_camera = GenericModel::UCM(UCM::zeros());
-    for i in 0..10 {
-        trace!("Initialize ucm {}", i);
-        if let Some(initialized_ucm) =
-            try_init_camera(frame_feature0, frame_feature1, cli.fixed_focal)
+    if let Some((camera_intrinsics, t_i_0, board_rtvecs)) = calib_all_camera_with_extrinsics(
+        &calibrated_intrinsics,
+        &t_cam_i_0_init,
+        &cam_rtvecs,
+        &cams_detected_feature_frames,
+        cli.one_focal || cli.fixed_focal.is_some(),
+        cli.disabled_distortion_num,
+        cli.fixed_focal.is_some(),
+    ) {
+        let mut rep_rms = Vec::new();
+        for (cam_idx, intrinsic) in camera_intrinsics.iter().enumerate() {
+            model_to_json(&format!("{}/cam{}.json", output_folder, cam_idx), intrinsic);
+            let new_rtvec_map: HashMap<usize, RvecTvec> = board_rtvecs
+                .iter()
+                .map(|(k, t_0_b)| {
+                    (
+                        *k,
+                        (t_i_0[cam_idx].to_na_isometry3() * t_0_b.to_na_isometry3()).to_rvec_tvec(),
+                    )
+                })
+                .collect();
+            recording
+                .log_static(
+                    format!("/cam{}", cam_idx),
+                    &na_isometry3_to_rerun_transform3d(&t_i_0[cam_idx].to_na_isometry3().inverse()),
+                )
+                .unwrap();
+            let rep = validation(
+                cam_idx,
+                intrinsic,
+                &new_rtvec_map,
+                &cams_detected_feature_frames[cam_idx],
+                Some(&recording),
+            );
+            rep_rms.push(rep);
+            println!(
+                "Cam {} final params with extrinsic{}",
+                cam_idx,
+                serde_json::to_string_pretty(intrinsic).unwrap()
+            );
+        }
+        write_report(&format!("{}/report.txt", output_folder), true, &rep_rms);
+
+        extrinsics_to_json(
+            &format!("{}/extrinsics.json", output_folder),
+            &Extrinsics::new(&t_i_0),
+        );
+    } else {
+        let mut rep_rms = Vec::new();
+        for (cam_idx, (intrinsic, rtvec_map)) in
+            calibrated_intrinsics.iter().zip(cam_rtvecs).enumerate()
         {
-            initial_camera = initialized_ucm;
-            break;
+            let rep = validation(
+                cam_idx,
+                intrinsic,
+                &rtvec_map,
+                &cams_detected_feature_frames[cam_idx],
+                Some(&recording),
+            );
+            rep_rms.push(rep);
+            println!(
+                "Cam {} final params{}",
+                cam_idx,
+                serde_json::to_string_pretty(intrinsic).unwrap()
+            );
+            model_to_json(&format!("{}/cam{}.json", output_folder, cam_idx), intrinsic);
         }
+        write_report(&format!("{}/report.txt", output_folder), false, &rep_rms);
     }
-    if initial_camera.params()[0] == 0.0 {
-        println!("calibration failed.");
-        return;
-    }
-    let mut final_model = cli.model;
-    final_model.set_w_h(
-        initial_camera.width().round() as u32,
-        initial_camera.height().round() as u32,
-    );
-    convert_model(
-        &initial_camera,
-        &mut final_model,
-        cli.disabled_distortion_num,
-    );
-    println!("Converted {:?}", final_model);
-    let (one_focal, fixed_focal) = if let Some(focal) = cli.fixed_focal {
-        // if fixed focal then set one focal true
-        let mut p = final_model.params();
-        p[0] = focal;
-        p[1] = focal;
-        final_model.set_params(&p);
-        (true, true)
-    } else {
-        (cli.one_focal, false)
-    };
-
-    let (final_result, rtvec_list) = calib_camera(
-        &cams_detected_feature_frames[0],
-        &final_model,
-        one_focal,
-        cli.disabled_distortion_num,
-        fixed_focal,
-    );
-    validation(
-        &final_result,
-        &rtvec_list,
-        &cams_detected_feature_frames[0],
-        Some(&recording),
-    );
-    println!(
-        "Final params{}",
-        serde_json::to_string_pretty(&final_result).unwrap()
-    );
-    model_to_json(&format!("{}/result.json", output_folder), &final_result);
 }

src/data_loader.rs

@@ -10,6 +10,8 @@ use glob::glob;
 use image::{DynamicImage, ImageReader};
 use rayon::prelude::*;
 
+const MIN_CORNERS: usize = 24;
+
 fn path_to_timestamp(path: &Path) -> i64 {
     let time_ns: i64 = path
         .file_stem()
@@ -66,13 +68,13 @@ pub fn load_euroc(
     cam_num: usize,
     recording_option: Option<&rerun::RecordingStream>,
 ) -> Vec<Vec<Option<FrameFeature>>> {
-    const MIN_CORNERS: usize = 24;
     (0..cam_num)
         .map(|cam_idx| {
+            log::trace!("loading cam{}", cam_idx);
             let img_paths =
                 glob(format!("{}/mav0/cam{}/data/*.png", root_folder, cam_idx).as_str())
                     .expect("failed");
-            img_paths
+            let mut time_frame: Vec<_> = img_paths
                 .skip(start_idx)
                 .step_by(step)
                 .par_bridge()
@@ -85,9 +87,20 @@ pub fn load_euroc(
                         let topic = format!("/cam{}", cam_idx);
                         log_image_as_compressed(recording, &topic, &img, image::ImageFormat::Jpeg);
                     };
-                    image_to_option_feature_frame(tag_detector, &img, board, MIN_CORNERS, time_ns)
+                    (
+                        time_ns,
+                        image_to_option_feature_frame(
+                            tag_detector,
+                            &img,
+                            board,
+                            MIN_CORNERS,
+                            time_ns,
+                        ),
+                    )
                 })
-                .collect()
+                .collect();
+            time_frame.sort_by(|a, b| a.0.cmp(&b.0));
+            time_frame.iter().map(|f| f.1.clone()).collect()
         })
         .collect()
 }
@@ -101,12 +114,12 @@ pub fn load_others(
     cam_num: usize,
     recording_option: Option<&rerun::RecordingStream>,
 ) -> Vec<Vec<Option<FrameFeature>>> {
-    const MIN_CORNERS: usize = 24;
     (0..cam_num)
         .map(|cam_idx| {
             let img_paths = glob(format!("{}/**/cam{}/**/*.png", root_folder, cam_idx).as_str())
                 .expect("failed");
-            img_paths
+            log::trace!("loading cam{}", cam_idx);
+            let mut time_frame: Vec<_> = img_paths
                 .skip(start_idx)
                 .step_by(step)
                 .enumerate()
@@ -120,9 +133,20 @@ pub fn load_others(
                         let topic = format!("/cam{}", cam_idx);
                         log_image_as_compressed(recording, &topic, &img, image::ImageFormat::Jpeg);
                     };
-                    image_to_option_feature_frame(tag_detector, &img, board, MIN_CORNERS, time_ns)
+                    (
+                        time_ns,
+                        image_to_option_feature_frame(
+                            tag_detector,
+                            &img,
+                            board,
+                            MIN_CORNERS,
+                            time_ns,
+                        ),
+                    )
                 })
-                .collect()
+                .collect();
+            time_frame.sort_by(|a, b| a.0.cmp(&b.0));
+            time_frame.iter().map(|f| f.1.clone()).collect()
         })
         .collect()
 }

src/io.rs

@@ -0,0 +1,21 @@
+use std::io::Write;
+
+use crate::types::Extrinsics;
+
+pub fn extrinsics_to_json(output_path: &str, extrinsic: &Extrinsics) {
+    let j = serde_json::to_string_pretty(extrinsic).unwrap();
+    let mut file = std::fs::File::create(output_path).unwrap();
+    file.write_all(j.as_bytes()).unwrap();
+}
+
+pub fn write_report(output_path: &str, with_extrinsic: bool, rep_rms: &[(f64, f64)]) {
+    let mut s = String::new();
+    s += format!("Calibrate with extrinsics: {}\n\n", with_extrinsic).as_str();
+    for (cam_idx, &(avg_rep, med_rep)) in rep_rms.iter().enumerate() {
+        s += format!("cam{}:\n", cam_idx).as_str();
+        s += format!("    average reprojection error: {:.5} px\n", avg_rep).as_str();
+        s += format!("    median  reprojection error: {:.5} px\n\n", med_rep).as_str();
+    }
+    let mut file = std::fs::File::create(output_path).unwrap();
+    file.write_all(s.as_bytes()).unwrap();
+}

src/lib.rs

@@ -1,6 +1,7 @@
 pub mod board;
 pub mod data_loader;
 pub mod detected_points;
+pub mod io;
 pub mod optimization;
 pub mod types;
 pub mod util;