Export poses (#22)

* update dep

* clean up

* sort poses

Author: powei-lin

Cargo.toml

@@ -1,7 +1,7 @@
 [package]
 name = "camera-intrinsic-calibration"
-version = "0.7.2"
-edition = "2021"
+version = "0.8.0"
+edition = "2024"
 authors = ["Powei Lin <poweilin1994@gmail.com>"]
 readme = "README.md"
 license = "GPL-3.0-or-later"
@@ -21,27 +21,27 @@ exclude = [
 ]
 
 [dependencies]
-aprilgrid = "0.6.0"
+aprilgrid = "0.6.1"
 camera-intrinsic-model = "0.5.0"
 clap = { version = "4.5", features = ["derive"] }
 colorous = "1.0.16"
 env_logger = "0.11.8"
-faer = "0.21.9"
-glam = "0.30"
+faer = "0.22.6"
+glam = "0.30.3"
 glob = "0.3.2"
 image = "0.25.6"
 indicatif = { version = "0.17.11", features = ["rayon"] }
 log = "0.4.27"
 nalgebra = "0.33.2"
-rand = "0.9.0"
+rand = "0.9.1"
 rand_chacha = "0.9.0"
 rayon = "1.10.0"
-rerun = "0.22.1"
+rerun = "0.23.2"
 serde = { version = "1.0.219", features = ["derive"] }
 serde_json = "1.0.140"
 sqpnp_simple = "0.1.6"
-time = "0.3.41"
-tiny-solver = "0.17.0"
+time = { version = "0.3.41", features = ["local-offset"] }
+tiny-solver = "0.17.1"
 
 [[bin]]
 name = "ccrs"

README.md

@@ -33,9 +33,9 @@ ccrs dataset-calib-cam1_1024_16 --model eucm
 ### Visualize details after calibration
 ```sh
 # use cargo to install rerun
-cargo install rerun-cli --version 0.22.1
+cargo install rerun-cli --version 0.23.2
 # or use pip to install rerun
-pip install rerun-sdk==0.22.1
+pip install rerun-sdk==0.23.2
 # visualize result
 rerun results/20YYMMDD_HH_MM_SS/logging.rrd
 ```

src/bin/camera_calibration.rs

@@ -1,18 +1,17 @@
-use aprilgrid::detector::TagDetector;
 use aprilgrid::TagFamily;
+use aprilgrid::detector::TagDetector;
 use camera_intrinsic_calibration::board::Board;
-use camera_intrinsic_calibration::board::{
-    board_config_from_json, board_config_to_json, BoardConfig,
-};
+use camera_intrinsic_calibration::board::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::io::{object_from_json, object_to_json, write_report};
 use camera_intrinsic_calibration::types::{CalibParams, 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::BTreeMap;
 use std::collections::HashMap;
 use std::time::Instant;
 use time::OffsetDateTime;
@@ -74,10 +73,10 @@ fn main() {
     let cli = CCRSCli::parse();
     let detector = TagDetector::new(&cli.tag_family, None);
     let board = if let Some(board_config_path) = cli.board_config {
-        Board::from_config(&board_config_from_json(&board_config_path))
+        Board::from_config(&object_from_json(&board_config_path))
     } else {
         let config = BoardConfig::default();
-        board_config_to_json("default_board_config.json", &config);
+        object_to_json("default_board_config.json", &config);
         Board::from_config(&config)
     };
     let dataset_root = &cli.path;
@@ -199,6 +198,12 @@ fn main() {
                     )
                 })
                 .collect();
+            object_to_json(
+                &format!("{}/cam{}_poses.json", output_folder, cam_idx),
+                &new_rtvec_map
+                    .iter()
+                    .collect::<BTreeMap<&usize, &RvecTvec>>(),
+            );
             let cam_transform =
                 na_isometry3_to_rerun_transform3d(&t_i_0[cam_idx].to_na_isometry3().inverse())
                     .with_axis_length(0.1);
@@ -221,7 +226,7 @@ fn main() {
         }
         write_report(&format!("{}/report.txt", output_folder), true, &rep_rms);
 
-        extrinsics_to_json(
+        object_to_json(
             &format!("{}/extrinsics.json", output_folder),
             &Extrinsics::new(&t_i_0),
         );
@@ -244,6 +249,10 @@ fn main() {
                 serde_json::to_string_pretty(intrinsic).unwrap()
             );
             model_to_json(&format!("{}/cam{}.json", output_folder, cam_idx), intrinsic);
+            object_to_json(
+                &format!("{}/cam{}_poses.json", output_folder, cam_idx),
+                &rtvec_map.iter().collect::<BTreeMap<&usize, &RvecTvec>>(),
+            );
         }
         write_report(&format!("{}/report.txt", output_folder), false, &rep_rms);
     }

src/board.rs

@@ -1,6 +1,6 @@
 use glam;
 use serde::{Deserialize, Serialize};
-use std::{collections::HashMap, io::Write};
+use std::collections::HashMap;
 
 #[derive(Debug, Serialize, Deserialize)]
 pub struct BoardConfig {
@@ -11,18 +11,6 @@ pub struct BoardConfig {
     first_id: u32,
 }
 
-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 {

src/data_loader.rs

@@ -10,6 +10,7 @@ use glob::glob;
 use image::{DynamicImage, ImageReader};
 use indicatif::ParallelProgressIterator;
 use rayon::prelude::*;
+use rerun::TimeCell;
 
 const MIN_CORNERS: usize = 24;
 
@@ -98,7 +99,10 @@ pub fn load_euroc(
                     let time_ns = path_to_timestamp(path);
                     let img = ImageReader::open(path).unwrap().decode().unwrap();
                     if let Some(recording) = recording_option {
-                        recording.set_time_nanos("stable", time_ns);
+                        recording.set_time(
+                            "stable",
+                            TimeCell::from_timestamp_nanos_since_epoch(time_ns),
+                        );
                         let topic = format!("cam{}", cam_idx);
                         log_image(recording, &topic, &img);
                     };
@@ -152,7 +156,10 @@ pub fn load_others(
                     let time_ns = *idx as i64 * 100000000;
                     let img = ImageReader::open(path).unwrap().decode().unwrap();
                     if let Some(recording) = recording_option {
-                        recording.set_time_nanos("stable", time_ns);
+                        recording.set_time(
+                            "stable",
+                            TimeCell::from_timestamp_nanos_since_epoch(time_ns),
+                        );
                         let topic = format!("cam{}", cam_idx);
                         log_image(recording, &topic, &img);
                     };

src/io.rs

@@ -1,12 +1,17 @@
 use std::io::Write;
 
-use crate::types::Extrinsics;
+use serde::{Serialize, de::DeserializeOwned};
 
-pub fn extrinsics_to_json(output_path: &str, extrinsic: &Extrinsics) {
-    let j = serde_json::to_string_pretty(extrinsic).unwrap();
+pub fn object_to_json<T: Serialize>(output_path: &str, object: &T) {
+    let j = serde_json::to_string_pretty(object).unwrap();
     let mut file = std::fs::File::create(output_path).unwrap();
     file.write_all(j.as_bytes()).unwrap();
 }
+pub fn object_from_json<T: DeserializeOwned>(file_path: &str) -> T {
+    let contents =
+        std::fs::read_to_string(file_path).expect("Should have been able to read the file");
+    serde_json::from_str(&contents).unwrap()
+}
 
 pub fn write_report(output_path: &str, with_extrinsic: bool, rep_rms: &[(f64, f64)]) {
     let mut s = String::new();

src/util.rs

@@ -13,15 +13,15 @@ use log::debug;
 use nalgebra as na;
 use rand::seq::SliceRandom;
 use rerun::RecordingStream;
-use tiny_solver::loss_functions::HuberLoss;
 use tiny_solver::Optimizer;
+use tiny_solver::loss_functions::HuberLoss;
 
 pub fn rtvec_to_na_dvec(
     rtvec: ((f64, f64, f64), (f64, f64, f64)),
 ) -> (na::DVector<f64>, na::DVector<f64>) {
     (
-        na::dvector![rtvec.0 .0, rtvec.0 .1, rtvec.0 .2],
-        na::dvector![rtvec.1 .0, rtvec.1 .1, rtvec.1 .2],
+        na::dvector![rtvec.0.0, rtvec.0.1, rtvec.0.2],
+        na::dvector![rtvec.1.0, rtvec.1.1, rtvec.1.2],
     )
 }
 
@@ -708,7 +708,10 @@ pub fn validation(
                         (p3p.x, p3p.y, p3p.z)
                     })
                     .collect();
-                recording.set_time_nanos("stable", f.time_ns);
+                recording.set_time(
+                    "stable",
+                    rerun::TimeCell::from_timestamp_nanos_since_epoch(f.time_ns),
+                );
                 recording
                     .log(
                         format!("/cam{}/board", cam_idx),
@@ -758,7 +761,10 @@ pub fn validation(
                     ((c.r, c.g, c.b, 255), format!("{}", r))
                 })
                 .unzip();
-            recording.set_time_nanos("stable", *time_ns);
+            recording.set_time(
+                "stable",
+                rerun::TimeCell::from_timestamp_nanos_since_epoch(*time_ns),
+            );
             recording
                 .log(
                     topic.to_string(),
@@ -843,7 +849,10 @@ pub fn init_and_calibrate_one_camera(
         let key_frames = [Some(frame_feature0.clone()), Some(frame_feature1.clone())];
         key_frames.iter().enumerate().for_each(|(i, k)| {
             let topic = format!("/cam{}/keyframe{}", cam_idx, i);
-            recording.set_time_nanos("stable", k.clone().unwrap().time_ns);
+            recording.set_time(
+                "stable",
+                rerun::TimeCell::from_timestamp_nanos_since_epoch(k.clone().unwrap().time_ns),
+            );
             recording
                 .log(topic, &rerun::TextLog::new("keyframe"))
                 .unwrap();

src/visualization.rs

@@ -1,3 +1,5 @@
+use std::io::Cursor;
+
 use image::DynamicImage;
 use rand::prelude::*;
 use rand_chacha::ChaCha8Rng;
@@ -6,11 +8,11 @@ use rerun::RecordingStream;
 use crate::detected_points::FrameFeature;
 
 pub fn log_image(recording: &RecordingStream, topic: &str, img: &DynamicImage) {
-    let gray_img = img.to_luma8();
-    let rr_image = rerun::Image::from_l8(gray_img.to_vec(), [gray_img.width(), gray_img.height()]);
-    recording
-        .log(format!("{}/image", topic), &rr_image)
-        .unwrap();
+    let mut bytes: Vec<u8> = Vec::new();
+    img.write_to(&mut Cursor::new(&mut bytes), image::ImageFormat::Png)
+        .expect("not able to write png");
+    let rr_image = rerun::EncodedImage::from_file_contents(bytes);
+    recording.log(topic.to_string(), &rr_image).unwrap();
 }
 
 pub fn id_to_color(id: usize) -> (u8, u8, u8, u8) {
@@ -55,7 +57,10 @@ pub fn log_feature_frames(
         let (colors, labels): (Vec<_>, Vec<_>) = colors_labels.iter().cloned().unzip();
         let pts = rerun_shift(&pts);
 
-        recording.set_time_nanos("stable", time_ns);
+        recording.set_time(
+            "stable",
+            rerun::TimeCell::from_timestamp_nanos_since_epoch(time_ns),
+        );
         recording
             .log(
                 format!("{}/pts", topic),