Stereo Calibration Example

This example demonstrates a two-camera calibration workflow in Rust, including:

• ChArUco feature detection for both camera streams
• Joint stereo optimization
• Triangulation uncertainty estimation from covariance
• Multi-camera error map generation and export to disk

use image::{GrayImage, ImageBuffer, Luma};
use libcalibgeom::{
    error_maps_multi, find_charuco_board, residual_statistics,
    subpix_saddle_point_polynomium, validate_license_with_env, ArucoDictionary, Calibration,
    Camera, CameraModelType, DetectionSettings, ImageDetection, InitializationSettings,
    OptimizationSettings, ParameterState, Point2f64, PoseRt, Target,
};
use std::fs;
use std::path::{Path, PathBuf};

fn charuco_object_points(rows: u32, columns: u32, spacing: f64) -> Vec<f64> {
    let mut xyz = Vec::with_capacity((rows * columns * 3) as usize);
    for r in 0..rows {
        for c in 0..columns {
            xyz.push(c as f64 * spacing);
            xyz.push(r as f64 * spacing);
            xyz.push(0.0);
        }
    }
    xyz
}

fn list_pngs(dir: &Path) -> Vec<PathBuf> {
    let mut paths: Vec<PathBuf> = match fs::read_dir(dir) {
        Ok(entries) => entries
            .filter_map(Result::ok)
            .map(|entry| entry.path())
            .filter(|path| {
                path.extension()
                    .and_then(|ext| ext.to_str())
                    .is_some_and(|ext| ext.eq_ignore_ascii_case("png"))
            })
            .collect(),
        Err(_) => Vec::new(),
    };
    paths.sort();
    paths
}

fn dense_points_from_ids(ids: &[u32], points: &[Point2f64], total_points: usize) -> Option<Vec<Point2f64>> {
    if ids.len() != points.len() {
        return None;
    }

    let mut dense = vec![Point2f64 { x: f64::NAN, y: f64::NAN }; total_points];
    for (id, point) in ids.iter().zip(points.iter()) {
        let index = *id as usize;
        if index >= total_points {
            return None;
        }
        dense[index] = *point;
    }
    Some(dense)
}

fn finite_point_count(points: &[Point2f64]) -> usize {
    points
        .iter()
        .filter(|p| p.x.is_finite() && p.y.is_finite())
        .count()
}

fn configure_camera(width: u32, height: u32, name: &str) -> Option<Camera> {
    let mut camera = Camera::new(CameraModelType::OpenCV, width, height, name)?;

    let _ = camera.set_parameter_state("f", ParameterState::Free)
        && camera.set_parameter_state("ar", ParameterState::Free)
        && camera.set_parameter_state("cx", ParameterState::Free)
        && camera.set_parameter_state("cy", ParameterState::Free)
        && camera.set_parameter_state("k1", ParameterState::Free)
        && camera.set_parameter_state("k2", ParameterState::Free)
        && camera.set_parameter_state("k3", ParameterState::Fixed)
        && camera.set_parameter_state("p1", ParameterState::Fixed)
        && camera.set_parameter_state("p2", ParameterState::Fixed);

    Some(camera)
}

fn normalize_luma32f_to_u8(
    image: &ImageBuffer<Luma<f32>, Vec<f32>>,
) -> ImageBuffer<Luma<u8>, Vec<u8>> {
    let mut min_value = f32::INFINITY;
    let mut max_value = f32::NEG_INFINITY;
    for pixel in image.pixels() {
        let value = pixel[0];
        if value.is_finite() {
            min_value = min_value.min(value);
            max_value = max_value.max(value);
        }
    }

    let range = if min_value.is_finite() && max_value.is_finite() {
        max_value - min_value
    } else {
        0.0
    };

    ImageBuffer::<Luma<u8>, Vec<u8>>::from_fn(image.width(), image.height(), |x, y| {
        let value = image.get_pixel(x, y)[0];
        let mapped = if !value.is_finite() || range <= f32::EPSILON {
            0u8
        } else {
            let normalized = ((value - min_value) / range).clamp(0.0, 1.0);
            (normalized * 255.0).round() as u8
        };
        Luma([mapped])
    })
}

fn main() {
    let licensed = validate_license_with_env();
    println!("license_valid: {licensed}");
    if !licensed {
        return;
    }

    let mut calibration = Calibration::new().expect("failed to create calibration object");

    let rows = 26u32;
    let columns = 38u32;
    let total_points = (rows * columns) as usize;
    let marker_size = 0.5f64;
    let feature_spacing = 0.025f64;
    let min_detected_features_per_view = 80usize;

    let data_root = PathBuf::from(env!("CARGO_MANIFEST_DIR")).join("../data/charuco_stereo");
    let cam0_dir = data_root.join("cam0");
    let cam1_dir = data_root.join("cam1");

    let cam0_paths = list_pngs(&cam0_dir);
    let cam1_paths = list_pngs(&cam1_dir);
    let pair_count = cam0_paths.len().min(cam1_paths.len());

    println!("dataset_root: {}", data_root.display());
    println!("cam0_images: {}", cam0_paths.len());
    println!("cam1_images: {}", cam1_paths.len());
    println!("paired_images: {pair_count}");

    if pair_count == 0 {
        println!("no image pairs found");
        return;
    }

    let first_image = match image::open(&cam0_paths[0]) {
        Ok(image) => image.to_luma8(),
        Err(error) => {
            println!("failed_to_open_first_cam0_image: {error}");
            return;
        }
    };
    let image_width = first_image.width();
    let image_height = first_image.height();

    let mut target = Target::new().expect("failed to create target");
    let target_ok = target.set_object_points_xyz(&charuco_object_points(rows, columns, feature_spacing));
    if !target_ok {
        println!("failed_to_set_target_object_points");
        return;
    }

    let Some(camera0) = configure_camera(image_width, image_height, "cam0") else {
        println!("failed_to_create_camera0");
        return;
    };
    let Some(camera1) = configure_camera(image_width, image_height, "cam1") else {
        println!("failed_to_create_camera1");
        return;
    };

    calibration.reset();
    if calibration.add_target(&target).is_none()
        || calibration.add_camera(&camera0).is_none()
        || calibration.add_camera(&camera1).is_none()
    {
        println!("failed_to_configure_calibration_graph");
        return;
    }

    println!(
        "configured cameras={} targets={} poses={}",
        calibration.cameras().len(),
        calibration.targets().len(),
        calibration.poses().len()
    );

    let detector_settings = DetectionSettings::new();
    let mut image_detections = Vec::new();

    for index in 0..pair_count {
        let cam0_path = &cam0_paths[index];
        let cam1_path = &cam1_paths[index];

        let image0: GrayImage = match image::open(cam0_path) {
            Ok(image) => image.to_luma8(),
            Err(error) => {
                println!("failed_to_open {}: {error}", cam0_path.display());
                continue;
            }
        };
        let image1: GrayImage = match image::open(cam1_path) {
            Ok(image) => image.to_luma8(),
            Err(error) => {
                println!("failed_to_open {}: {error}", cam1_path.display());
                continue;
            }
        };

        if image0.width() != image_width
            || image0.height() != image_height
            || image1.width() != image_width
            || image1.height() != image_height
        {
            println!("skipping_pair_size_mismatch index={index}");
            continue;
        }

        let detection0 = find_charuco_board(
            &image0,
            rows,
            columns,
            ArucoDictionary::Dict4x4,
            0,
            marker_size,
            &detector_settings,
        )
        .and_then(|(points, ids)| subpix_saddle_point_polynomium(&image0, 5, &points, 50).map(|refined| (refined, ids)))
        .and_then(|(points, ids)| dense_points_from_ids(&ids, &points, total_points));

        let detection1 = find_charuco_board(
            &image1,
            rows,
            columns,
            ArucoDictionary::Dict4x4,
            0,
            marker_size,
            &detector_settings,
        )
        .and_then(|(points, ids)| subpix_saddle_point_polynomium(&image1, 5, &points, 50).map(|refined| (refined, ids)))
        .and_then(|(points, ids)| dense_points_from_ids(&ids, &points, total_points));

        let valid0 = detection0
            .as_ref()
            .map(|points| finite_point_count(points))
            .unwrap_or(0);
        let valid1 = detection1
            .as_ref()
            .map(|points| finite_point_count(points))
            .unwrap_or(0);

        if valid0 < min_detected_features_per_view && valid1 < min_detected_features_per_view {
            println!("pair[{index}] skipped: insufficient detections (cam0={valid0}, cam1={valid1})");
            continue;
        }

        let Some(pose_id) = calibration.add_pose(PoseRt {
            rvec: [0.0, 0.0, 0.0],
            tvec: [0.0, 0.0, 0.0],
        }) else {
            println!("pair[{index}] failed_to_add_pose");
            continue;
        };

        if let Some(points) = detection0 {
            if valid0 >= min_detected_features_per_view {
                image_detections.push(ImageDetection {
                    camera_id: 0,
                    pose_id,
                    target_id: 0,
                    points,
                });
            }
        }

        if let Some(points) = detection1 {
            if valid1 >= min_detected_features_per_view {
                image_detections.push(ImageDetection {
                    camera_id: 1,
                    pose_id,
                    target_id: 0,
                    points,
                });
            }
        }

        println!("pair[{index}] accepted: cam0={valid0}, cam1={valid1}");
    }

    println!("detections_total: {}", image_detections.len());
    println!("poses_total: {}", calibration.poses().len());

    if image_detections.len() < 6 {
        println!("not_enough_detections_for_stereo_calibration");
        return;
    }

    let init_report = calibration.initialize(
        &image_detections,
        None,
        None,
        InitializationSettings::default(),
    );
    println!(
        "initialize_succeeded: {}, status: {:?}",
        init_report.succeeded, init_report.status
    );

    if !init_report.succeeded {
        return;
    }

    let optimize_report = calibration.optimize(
        &image_detections,
        None,
        None,
        OptimizationSettings {
            max_iterations: 80,
            ..OptimizationSettings::default()
        },
    );
    println!(
        "optimize_succeeded: {}, converged: {}, rpe_rms: {}, status: {:?}",
        optimize_report.succeeded,
        optimize_report.converged,
        optimize_report.rpe_rms,
        optimize_report.status
    );

    if let Some(stats) = residual_statistics(&optimize_report.reprojection_errors) {
        println!(
            "residual_stats rms={} mean={} median={}",
            stats.rms, stats.mean, stats.median
        );
    }

    let output_dir = PathBuf::from(env!("CARGO_MANIFEST_DIR"))
        .join("../output/stereo_calibration");
    if let Err(error) = fs::create_dir_all(&output_dir) {
        println!("failed_to_create_output_dir {}: {error}", output_dir.display());
        return;
    }

    let covariance = calibration.estimate_covariance(&image_detections);
    println!(
        "covariance_succeeded: {}, status: {:?}, shape={}x{}",
        covariance.succeeded, covariance.status, covariance.rows, covariance.cols
    );

    if covariance.succeeded {
        if let Some(triangulation) = calibration.estimate_triangulation_error(&covariance, 1.0, 0.2)
        {
            println!(
                "triangulation_uncertainty rms={} max={}",
                triangulation.rms,
                triangulation.max
            );

            let summary_path = output_dir.join("triangulation_uncertainty.txt");
            let mut summary = String::new();
            summary.push_str(&format!("rms={}\n", triangulation.rms));
            summary.push_str(&format!("max={}\n", triangulation.max));
            let _ = fs::write(&summary_path, summary);
            println!("wrote: {}", summary_path.display());
        }
    }

    let image_sizes = vec![(image_width, image_height), (image_width, image_height)];
    if let Some(maps) = error_maps_multi(&optimize_report.reprojection_errors, &image_sizes)
    {
        for (camera_index, maps) in maps.into_iter().enumerate() {
            let magnitude_path = output_dir.join(format!("error_map_cam{}_magnitude.png", camera_index));
            let direction_path = output_dir.join(format!("error_map_cam{}_direction.png", camera_index));

            let magnitude_u8 = normalize_luma32f_to_u8(&maps.magnitude);
            let magnitude_ok = magnitude_u8.save(&magnitude_path).is_ok();
            let direction_ok = maps.directions_rgb.save(&direction_path).is_ok();

            println!(
                "error_maps_cam{} magnitude_saved={} direction_saved={}",
                camera_index, magnitude_ok, direction_ok
            );
        }
    } else {
        println!("failed_to_compute_error_maps_multi");
    }

    println!("output_dir: {}", output_dir.display());
    println!("{}", calibration.to_string().unwrap_or_default());
}

The example writes outputs to output/stereo_calibration/, including triangulation uncertainty summaries and per-camera error maps.