前提

前回は【Rust + OpenCV】視差マッピングで視差マッピングを作成する方法を述べました。私の理解度が低い状態だったため、なかなかうまく説明できない部分も多くありましたが、読んで頂いた方々には感謝しています。

さて、今回はカメラキャリブレーションをRustで実装する方法をご紹介します。
駄文で恐縮ですが、お付き合い頂けますと嬉しいです。

以下の項目で構成していますので、コードだけ見たい方は「実装」項目にお進みください。

カメラキャリブレーションの基礎概念
使用するデータセットについて
出力結果
実装
終わりに

カメラキャリブレーションの基礎概念

カメラキャリブレーションとは

すごく簡単に説明すると、次の手順を行うことでカメラの内部・外部パラメータを知ることができる処理のことです。

チェスボードを用意
色んな角度から10～20枚くらい写真撮影する
カメラキャリブレーションを実施

この内部パラメータとはカメラ行列と歪み係数を表し、外部パラメータは回転ベクトルと並進ベクトルを表します。

Camera calibration estimates the parameters of a lens, the image sensor of an image, or a video camera. You can use these parameters to estimate structures in a scene and to remove lens distortion. The camera parameters include:

Intrinsics — These relate to the internal characteristics of a camera, such as the focal length, the optical center (also known as the principal point), and the skew coefficient.

Extrinsics — These describe the location (position and orientation) of the camera in the 3-D scene.

和訳：
「カメラキャリブレーションは、レンズ、画像のイメージセンサー、またはビデオカメラのパラメータを指します。これらのパラメータを使用して、シーンの構造を推定したり、レンズの歪みを除去したりすることができます。カメラパラメータには以下が含まれます：

内部パラメータ — 焦点距離、光学中心（主点と呼ばれる）、歪み係数など、カメラの内部特性に関するもの。

外部パラメータ — 3Dシーンにおけるカメラの位置（位置と向き）を表します。」

引用元

カメラキャリブレーションの数学的仕組み

カメラキャリブレーションの基盤となるのは、ピンボールカメラモデルです。
このモデルでは、3D空間の点が2D画像平面に投影される過程を数学的に表現します。

ピンホールカメラモデル

カメラ座標系から画像平面への投影は、次のように表されます：

\begin{bmatrix}
x' \\
y' \\
1
\end{bmatrix}
= K ・ \begin{bmatrix} R|T \end{bmatrix} ・ 
\begin{bmatrix}
X \\
Y \\
Z \\
1
\end{bmatrix}

各値について：

\begin{bmatrix}
X \\
Y \\
Z \\
\end{bmatrix}

3D空間内の対象点の座標（ワールド座標系）

\begin{bmatrix}
x' \\
y' \\
\end{bmatrix}

画像平面上の点の座標（ピクセル単位）

カメラ行列（内部パラメータ）

カメラの回転ベクトル

カメラの並進ベクトル

カメラ行列

カメラ行列 K は、カメラの内部パラメータを含む 3x3 の行列で、次の形を表します：

K =
\begin{bmatrix}
f_x & 0 & c_x \\
0 & f_y & c_y \\
0 & 0 & 1
\end{bmatrix}

焦点距離（画素単位）

f_x, f_y

主点（光学中心）の座標

c_x, c_y

歪み係数

カメラレンズによって生じる歪み（特にラジアル歪み）は、次のようにモデル化されます：

\displaylines{
x_{distorted} = x(1 + x_1r^2 + k_xr^4 + k_3r^6) \\
x_{distorted} = x(1 + x_1r^2 + k_xr^4 + k_3r^6)
}

各値について：

r =
\sqrt{x^2+y^2}

画像平面での点の距離

k_1, k_2, k_3

ラジアル歪みの係数

カメラキャリブレーションパターン

簡単に言うと、カメラキャリブレーションを行う上で、カメラで撮影する規則性を持つ模様のことです。

OpenCVで使用できるカメラキャリブレーションパターンは以下の4つがあります：

CheckerBoard
- 一般的なパターン
- 外部パラメータの計測には使用できない
CircleGrid
- チェッカーボードパターンよりも精度の良い検出ができる
- 外部パラメータの計測には使用できない
Asynmmetry-CircleGrid
- 内部パラメータ、外部パラメータ、レンズの歪曲収差、全ての計測に使用できる
ChArUco
- ChckerBoardとArUcoマーカーを組み合わせたキャリブレーションパターン
- 内部パラメータ、外部パラメータ、レンズの歪曲収差、全ての計測に使用できる

パターン	内部パラメータ・レンズの歪曲収差	外部パラメータ
CheckerBoard	〇	×
CircleGrid	◎	×
Asynmmetry-CircleGrid	◎	〇
ChArUco	〇	◎

◎：特に適しているため推奨
〇：キャリブレーション可
×：キャリブレーション不可

また、精度に関しては以下のような傾向があります：
CircleGrid = Asynmmetry-CircleGrid > CheckerBoard >= ChArUco

OpenCVで使用できるキャリブレーション用パターンの紹介

OpenCVはAsynmmetry-CircleGridの使用を推奨しています。

参考：
①技術ブログ

②動画

③OpenCV公式

④MathWorks公式

detectCheckerboardPoints

⑤大学の資料
Computer Vision

使用するデータセットについて

印刷したチェス盤：
9x6

行数x列数は下記の傾向があります：

行数x列数	精度	計算効率
7x5（7行×5列）	×	◎
9x6（9行×6列）	〇	〇
10x6（10行×6列）	◎	×
12x8（12行×8列）	◎	×

◎：高い
〇：標準的
×：ひくい

自分で用意したデータセットの一部：

このような写真を様々な角度から20枚用意しました。
枚数が多いほど高精度なパラメータを取得できます。

出力結果

カメラキャリブレーションで得られた内部・外部パラメータ

回転ベクトルと並進ベクトルは要素数が多いため、一部割愛しています。

{
  "camera_matrix": [
    [
      3286.4629841607984,
      0.0,
      2023.5768869100714
    ],
    [
      0.0,
      3271.6754503180246,
      1503.2101514954427
    ],
    [
      0.0,
      0.0,
      1.0
    ]
  ],
  "distortion_parameters": [
    0.2737181796474219,
    -2.349647809603689,
    0.0007015239366941092,
    0.0005142872882248564,
    6.576827620207052
  ],
  "rotation_vectors": [
    [
      1.2968106206837728,
      -0.07194663305638405,
      2.8640500618882805
    ],
    ...,
  ],
  "translation_vectors": [
    [
      0.3500820667813367,
      3.092196734461586,
      13.780983381421258
    ],
    ...,
  ],
  "total_error": 0.9103929636811415
}

total_error とは 再投影誤差 を表しており、この数値が小さいほどカメラキャリブレーションの精度が高いと言えます。理想値は0.5～1.0 となります。

歪み補正した画像

補正前

そもそも元画像が歪んでいないため、補正後に変化は見られませんでした。
もし、カメラキャリブレーションで得た内部パラメータ・外部パラメータを使用して、
視差マッピング を作成する場合、歪み補正に関してはあまり気にしなくていいかと思います。

実装

`main.rs`

use std::time::Instant;

use opencv::core::{
    TermCriteria,
    TermCriteria_Type,
    Size
};

use camera_calibration::{CameraCalibration, CameraCalibrationTrait};
use file::File;
mod camera_calibration;
mod file;

// FILE
const FILE_FORMAT: &str = "jpeg";
const UNDISTORT_IMG_PATH: &str = "./img/calib04.jpeg";
const RESULT_IMG_PATH: &str = "./out/result.jpeg";
const CALIBRATION_JSON_PATH: &str = "./out/calibration.json";
const FAILED_READ_IMAGES_PATH: &str = "./out/failed_read_files.json";

// CAMERA CALIBRATION PARAMETERS
const CHESSBOARD_SIZE: (i32, i32) = (9, 6);
const FRAME_WIDTH: i32 = 1440;
const FRAME_HEIGHT: i32 = 1080;
const CRITERIA_MAX_COUNT: i32 = 30;
const CRITERIA_EPS: f64 = 0.001;
const CORNER_SUB_PIX_WINDOW_WIDTH: i32 = 11;
const CORNER_SUB_PIX_WINDOW_HEIGHT: i32 = 11;
const CORNER_SUB_PIX_ZERO_ZONE: i32 = -1;

// DISPLAY IMAGE WINDOW SIZE
const  WINDOW_TITLE: &str = "Chessboard Corners Detection";
const GUI_WINDOW_WIDTH: i32 = 900;
const GUI_WINDOW_HEIGHT: i32 = 700;
const WAIT_KEY_DELAY: i32 = 1000;

// TEXT
const TEXT_POINT: (i32, i32) = (10, 100);
const TEXT_FONT_SCALE: f64 = 3.0;
const TEXT_COLOR: (f64, f64, f64, f64) = (0.0, 255.0, 0.0, 0.0); // GREEN

fn main() -> opencv::Result<()> {
    File::create_out_dir();

    let start_time = Instant::now();

    let chessboard_size = Size::new(CHESSBOARD_SIZE.0, CHESSBOARD_SIZE.1);
    let frame_size = Size::new(FRAME_WIDTH, FRAME_HEIGHT);
    
    let image_paths = File::get_image_paths("./img");
    let criteria = TermCriteria::new(
        (TermCriteria_Type::COUNT as i32) + (TermCriteria_Type::EPS as i32),
        CRITERIA_MAX_COUNT,
        CRITERIA_EPS,
    )?;

    let Ok((obj_points, img_points)) =
        CameraCalibration::detect_chessboard_corners(&image_paths, chessboard_size, criteria)
        else {
            eprintln!("コーナー検出に失敗しました。");
            return Ok(());
        };

    let (camera_matrix, dist_coeffs, rvecs, tvecs) =
        CameraCalibration::calibrate_camera(&obj_points, &img_points, frame_size, criteria)?;

    CameraCalibration::undistort_image(&camera_matrix, &dist_coeffs)?;

    let error = CameraCalibration::compute_reprojection_error(&obj_points, &img_points, &rvecs, &tvecs, &camera_matrix, &dist_coeffs)?;
    println!("再投影誤差: {}", error);

    if let Err(e) = CameraCalibration::save_to_json(&camera_matrix, &dist_coeffs, &rvecs, &tvecs, error, CALIBRATION_JSON_PATH) {
        eprintln!("JSONへの書き込みに失敗しました： {}", e);
    }

    let duration = start_time.elapsed();
    println!("処理時間： {:?}", duration);

    Ok(())
}

`file.rs`

use std::fs;
use std::path::Path;

use crate::FILE_FORMAT;

pub struct File {}

impl File {
    /// 出力ディレクトリを作成する
    pub fn create_out_dir() {
        let dir_name = "out";
        if !Path::new(dir_name).exists() {
            fs::create_dir(dir_name).expect("ディレクトリの作成に失敗しました。");
            println!("ディレクトリを作成しました： '{}'", dir_name);
        }
    }

    /// 指定したディレクトリ内の画像パスを取得する
    pub fn get_image_paths(dir: &str) -> Vec<std::path::PathBuf> {
        std::fs::read_dir(dir)
            .unwrap()
            .filter_map(Result::ok)
            .filter(|entry| entry.path().extension().map(|ext| ext == FILE_FORMAT).unwrap_or(false))
            .map(|entry| entry.path())
            .collect()
    }
}

`camera_calibration.rs`

use std::fs::File;
use std::io::Write;

use serde::{Serialize, Deserialize};
use serde_json::{self, json};
use opencv::{
    calib3d,
    core::{self, Mat, Point2f, Point3f, Size, Vector},
    highgui,
    imgcodecs,
    imgproc,
    prelude::*,
    Error as OpenCvError,
};
use rayon::prelude::*;

use crate::{
    CORNER_SUB_PIX_WINDOW_HEIGHT, CORNER_SUB_PIX_WINDOW_WIDTH, CORNER_SUB_PIX_ZERO_ZONE, FAILED_READ_IMAGES_PATH, GUI_WINDOW_HEIGHT, GUI_WINDOW_WIDTH, RESULT_IMG_PATH, TEXT_COLOR, TEXT_FONT_SCALE, TEXT_POINT, UNDISTORT_IMG_PATH, WAIT_KEY_DELAY, WINDOW_TITLE
};

#[derive(Serialize, Deserialize)]
pub struct CameraCalibration {
    camera_matrix: Vec<Vec<f64>>,
    distortion_parameters: Vec<f64>,
    rotation_vectors: Vec<Vec<f64>>,
    translation_vectors: Vec<Vec<f64>>,
    total_error: f64,
}

pub trait CameraCalibrationTrait {
    /// チェスボードのコーナー検出 & 精緻化
    fn detect_chessboard_corners(
        image_paths: &[std::path::PathBuf],
        chessboard_size: Size,
        criteria: core::TermCriteria,
    ) -> opencv::Result<(Vector<Vector<Point3f>>, Vector<Vector<Point2f>>)>;

    /// カメラキャリブレーション
    fn calibrate_camera(
        obj_points: &Vector<Vector<Point3f>>,
        img_points: &Vector<Vector<Point2f>>,
        frame_size: Size,
        criteria: core::TermCriteria,
    ) -> opencv::Result<(Mat, Mat, Vector<Mat>, Vector<Mat>)>;

    /// 画像の歪み補正
    fn undistort_image(camera_matrix: &Mat, dist_coeffs: &Mat) -> opencv::Result<()>;

    /// 再投影誤差を計算
    fn compute_reprojection_error(
        obj_points: &Vector<Vector<Point3f>>,
        img_points: &Vector<Vector<Point2f>>,
        rvecs: &Vector<Mat>,
        tvecs: &Vector<Mat>,
        camera_matrix: &Mat,
        dist_coeffs: &Mat,
    ) -> opencv::Result<f64>;

    /// カメラキャリブレーション結果をJSON形式で保存
    fn save_to_json(
        camera_matrix: &Mat,
        dist_coeffs: &Mat,
        rvecs: &Vector<Mat>,
        tvecs: &Vector<Mat>,
        error: f64,
        filename: &str
    ) -> std::io::Result<()>;
}

impl CameraCalibrationTrait for CameraCalibration {
    fn detect_chessboard_corners(
        image_paths: &[std::path::PathBuf],
        chessboard_size: Size,
        criteria: core::TermCriteria,
    ) -> opencv::Result<(Vector<Vector<Point3f>>, Vector<Vector<Point2f>>)> {
        let mut objp = Vector::<Point3f>::new();
        for i in 0..chessboard_size.height {
            for j in 0..chessboard_size.width {
                objp.push(Point3f::new(j as f32, i as f32, 0.0));
            }
        }
    
        let mut obj_points = Vector::<Vector<Point3f>>::new();
        let mut img_points = Vector::<Vector<Point2f>>::new();

        let mut failed_images = Vec::new();
    
        highgui::named_window(WINDOW_TITLE, highgui::WINDOW_NORMAL)?;
        highgui::resize_window(WINDOW_TITLE, GUI_WINDOW_WIDTH, GUI_WINDOW_HEIGHT)?;
    
        for image_path in image_paths {
            let img = imgcodecs::imread(image_path.to_str().unwrap(), imgcodecs::IMREAD_COLOR)?;
            let mut gray = Mat::default();
            imgproc::cvt_color(&img, &mut gray, imgproc::COLOR_BGR2GRAY, 0)?;
    
            let mut corners = Vector::<Point2f>::new();
            let found = calib3d::find_chessboard_corners(
                &gray,
                chessboard_size,
                &mut corners,
                calib3d::CALIB_CB_ADAPTIVE_THRESH
                    | calib3d::CALIB_CB_FAST_CHECK
                    | calib3d::CALIB_CB_NORMALIZE_IMAGE,
            )?;
    
            if found {
                obj_points.push(objp.clone());
    
                let mut refined_corners = corners.clone();
                imgproc::corner_sub_pix(
                    &gray,
                    &mut refined_corners,
                    Size::new(CORNER_SUB_PIX_WINDOW_WIDTH, CORNER_SUB_PIX_WINDOW_HEIGHT),
                    Size::new(CORNER_SUB_PIX_ZERO_ZONE, CORNER_SUB_PIX_ZERO_ZONE),
                    criteria,
                )?;
                img_points.push(refined_corners);
    
                let mut img_clone = img.clone();
                calib3d::draw_chessboard_corners(&mut img_clone, chessboard_size, &corners, found)?;
    
                // 読み込んだファイル名をウィンドウの左上に表示
                if let Some(filename) = image_path.file_name().and_then(|f| f.to_str()) {
                    let text = format!("{}", filename);
                    let org = core::Point::new(TEXT_POINT.0, TEXT_POINT.1);
                    let font_face = imgproc::FONT_HERSHEY_SIMPLEX;
                    let font_scale = TEXT_FONT_SCALE;
                    let color = core::Scalar::new(TEXT_COLOR.0, TEXT_COLOR.1, TEXT_COLOR.2, TEXT_COLOR.3);
                    let thickness = 2;
                    imgproc::put_text(&mut img_clone, &text, org, font_face, font_scale, color, thickness, imgproc::LINE_AA, false)?;
                }
    
                highgui::imshow(WINDOW_TITLE, &img_clone)?;
                highgui::wait_key(WAIT_KEY_DELAY)?;
            } else {
                if let Some(filename) = image_path.file_name().and_then(|f| f.to_str()) {
                    failed_images.push(filename.to_string());
                }
            }
        }
    
        highgui::destroy_all_windows()?;
        
        let failed_json = json!({ "failed_read_json": failed_images });
        let mut file = File::create(FAILED_READ_IMAGES_PATH)
            .map_err(|e| OpenCvError::new(core::StsError, format!("File create error: {}", e)))?;
        file.write_all(failed_json.to_string().as_bytes())
            .map_err(|e| OpenCvError::new(core::StsError, format!("File write error: {}", e)))?;

        Ok((obj_points, img_points))
    }

    fn calibrate_camera(
        obj_points: &Vector<Vector<Point3f>>,
        img_points: &Vector<Vector<Point2f>>,
        frame_size: Size,
        criteria: core::TermCriteria,
    ) -> opencv::Result<(Mat, Mat, Vector<Mat>, Vector<Mat>)> {
        let mut camera_matrix = Mat::default();
        let mut dist_coeffs = Mat::default();
        let mut rvecs = Vector::<Mat>::new();
        let mut tvecs = Vector::<Mat>::new();

        let ret = calib3d::calibrate_camera(
            obj_points,
            img_points,
            frame_size,
            &mut camera_matrix,
            &mut dist_coeffs,
            &mut rvecs,
            &mut tvecs,
            0,
            criteria,
        )?;

        // より高い精度で数値を扱う必要がある場合（例：顕微鏡）に役立つかもしれないので、
        // この出力は残しておきます。
        println!("Camera Calibrated: {}", ret);
        println!("カメラ行列：\n{:?}", camera_matrix);
        println!("歪み係数：\n{:?}", dist_coeffs);

        Ok((camera_matrix, dist_coeffs, rvecs, tvecs))
    }

    fn undistort_image(camera_matrix: &Mat, dist_coeffs: &Mat) -> opencv::Result<()> {
        let img = imgcodecs::imread(UNDISTORT_IMG_PATH, imgcodecs::IMREAD_COLOR)?;
        let size = img.size()?;
        let new_camera_matrix = Mat::default();
        let mut roi = core::Rect::default();

        calib3d::get_optimal_new_camera_matrix(
            camera_matrix,
            dist_coeffs,
            size,
            1.0,
            size,
            Some(&mut roi),
            false,
        )?;

        let mut dst = Mat::default();
        calib3d::undistort(&img, &mut dst, camera_matrix, dist_coeffs, &new_camera_matrix)?;

        imgcodecs::imwrite(RESULT_IMG_PATH, &dst, &Vector::new())?;
        Ok(())
    }

    fn compute_reprojection_error(
        obj_points: &Vector<Vector<Point3f>>,
        img_points: &Vector<Vector<Point2f>>,
        rvecs: &Vector<Mat>,
        tvecs: &Vector<Mat>,
        camera_matrix: &Mat,
        dist_coeffs: &Mat,
    ) -> opencv::Result<f64> {
        let errors: Vec<f64> = (0..obj_points.len())
            .into_par_iter()
            .map(|i| {
                let mut img_points2 = Vector::<Point2f>::new();
                let mut jacobian = Mat::default();
    
                if let Ok(_) = calib3d::project_points(
                    &obj_points.get(i).unwrap(),
                    &rvecs.get(i).unwrap(),
                    &tvecs.get(i).unwrap(),
                    camera_matrix,
                    dist_coeffs,
                    &mut img_points2,
                    &mut jacobian,
                    0.0,
                ) {
                    // 実測点と投影点の差分を計算
                    let diff: Vec<Point2f> = img_points.get(i).unwrap()
                        .iter()
                        .zip(img_points2.iter())
                        .map(|(p1, p2)| Point2f::new(p1.x - p2.x, p1.y - p2.y))
                        .collect();
    
                    // 二乗平均平方根誤差を計算
                    let squared_errors: f64 = diff.iter()
                        .map(|p| (p.x * p.x + p.y * p.y) as f64)
                        .sum();
                    
                    (squared_errors / diff.len() as f64).sqrt()
                } else {
                    0.0
                }
            })
            .collect();
    
        let mean_error = errors.iter().sum::<f64>() / obj_points.len() as f64;
        Ok(mean_error)
    }

    fn save_to_json(
        camera_matrix: &Mat,
        dist_coeffs: &Mat,
        rvecs: &Vector<Mat>,
        tvecs: &Vector<Mat>,
        error: f64,
        filename: &str,
    ) -> std::io::Result<()> {
        //  カメラ行列をVec<Vec<f64>>に変換
        let rows = camera_matrix.rows() as usize;
        let cols = camera_matrix.cols() as usize;
        let mut camera_matrix_vec = vec![vec![0.0; cols]; rows];
        for i in 0..rows {
            for j in 0..cols {
                camera_matrix_vec[i][j] = *camera_matrix.at_2d::<f64>(i as i32, j as i32).unwrap();
            }
        }

        let dist_coeffs_vec: Vec<f64> = (0..dist_coeffs.total())
            .map(|i| *dist_coeffs.at::<f64>(i as i32).unwrap())
            .collect();

        // 回転ベクトルと並進ベクトルをVec<Vec<f64>>に変換
        let mut rotation_vectors = Vec::new();
        let mut translation_vectors = Vec::new();
        for rvec in rvecs {
            let rvec_vec = (0..rvec.total() as usize)
                .map(|i| *rvec.at::<f64>(i as i32).unwrap())
                .collect::<Vec<f64>>();
            rotation_vectors.push(rvec_vec);
        }
        for tvec in tvecs {
            let tvec_vec = (0..tvec.total() as usize)
                .map(|i| *tvec.at::<f64>(i as i32).unwrap())
                .collect::<Vec<f64>>();
            translation_vectors.push(tvec_vec);
        }

        let calibration = CameraCalibration {
            camera_matrix: camera_matrix_vec,
            distortion_parameters: dist_coeffs_vec,
            rotation_vectors,
            translation_vectors,
            total_error: error,
        };

        let json_string = serde_json::to_string_pretty(&calibration)?;
        let mut file = File::create(filename)?;
        file.write_all(json_string.as_bytes())?;

        Ok(())
    }
}

今回記載したコードの出所
【GitHub】Camera Calibration - ChessBoard

ChessBoard以外のパターンも試せるように下記リポジトリも用意しています。
コマンドラインでパターンを使い分けることができます。
【GitHub】Camera Calibrator

終わりに

今回は、カメラキャリブレーションをRustで実装する例をご紹介しました。
コーディング中に、幾度も
（これ、Pythonで書いた方がもっと楽だし、情報も多いから時短できたよな。。。）
と思いつつ完成させたので、ぜひ研究や次世代のカメラ技術の開発などで活用してもらいたいです。

無論、私自身がコンピュータヴィジョンの分野に触れたのが1週間前のため、皆様より知識は乏しいと思います。
そのため、コードについて修正した方が良い点や追加した方が良い機能など教えていただけると嬉しく思います。