camcal/
lib.rs

1// Copyright (C) The Strand-Braid Authors
2// SPDX-License-Identifier: MIT OR Apache-2.0
3
4use nalgebra::RealField;
5use serde::{Deserialize, Serialize};
6
7mod backend;
8
9pub use backend::{
10    CalibrationResult, CorrespondingPoint, Error, calibrate_camera, find_chessboard_corners,
11};
12
13type Coords3D = (f64, f64, f64);
14type Coords2D = (f64, f64);
15
16#[derive(Serialize, Deserialize)]
17pub struct CheckerBoardData {
18    n_rows: usize,
19    n_cols: usize,
20    points: Vec<Coords2D>,
21}
22
23impl CheckerBoardData {
24    pub fn new(n_rows: usize, n_cols: usize, points: &[Coords2D]) -> Self {
25        let points = points.to_vec();
26        Self {
27            n_rows,
28            n_cols,
29            points,
30        }
31    }
32}
33
34fn to_image_points(board: &CheckerBoardData) -> Vec<Coords2D> {
35    board.points.clone()
36}
37
38#[derive(Debug, Clone)]
39pub struct PixelSize {
40    width: usize,
41    height: usize,
42}
43
44impl PixelSize {
45    pub fn new(width: usize, height: usize) -> Self {
46        Self { width, height }
47    }
48}
49
50pub fn save_yaml<P: AsRef<std::path::Path>>(
51    cam_info_file_stamped: P,
52    pkg_name: &str,
53    local: chrono::DateTime<chrono::Local>,
54    raw_opencv_cal: &CalibrationResult,
55    raw_cam_name: &str,
56) -> eyre::Result<()> {
57    let intrinsics = convert_to_cam_geom::<f64>(raw_opencv_cal);
58
59    // Convert from braid_mvg to ROS format.
60    let ci: opencv_ros_camera::RosCameraInfo<_> = opencv_ros_camera::NamedIntrinsicParameters {
61        intrinsics,
62        width: raw_opencv_cal.image_width as usize,
63        height: raw_opencv_cal.image_height as usize,
64        name: raw_cam_name.into(),
65    }
66    .into();
67
68    let mut f = std::fs::File::create(&cam_info_file_stamped)?;
69    std::io::Write::write_all(
70        &mut f,
71        format!(
72            "# Saved by {} at {}\n\
73                        # Mean reprojection distance: {:.2}\n",
74            pkg_name, local, raw_opencv_cal.mean_reprojection_distance_pixels
75        )
76        .as_bytes(),
77    )?;
78    serde_yaml::to_writer(f, &ci)?;
79    Ok(())
80}
81
82/// Given some checkerboard corner locations, compute intrinsics using OpenCV.
83pub fn compute_intrinsics_with_raw_opencv<R: RealField>(
84    size: PixelSize,
85    data: &[CheckerBoardData],
86) -> Result<CalibrationResult, Error> {
87    /*
88    cal = camera_calibration.calibrator.MonoCalibrator([])
89    cal.size = (width,height)
90    r = cal.cal_fromcorners(goodcorners)
91    msg = cal.as_message()
92    */
93
94    let object_points: Vec<Vec<Coords3D>> = mk_object_points(data);
95    let image_points: Vec<Vec<Coords2D>> = data.iter().map(to_image_points).collect();
96
97    debug_assert!(object_points.len() == image_points.len());
98
99    let pts: Vec<Vec<CorrespondingPoint>> = object_points
100        .into_iter()
101        .zip(image_points)
102        .map(|(obj_pts, im_pts)| {
103            obj_pts
104                .into_iter()
105                .zip(im_pts)
106                .map(|(obj_pt, im_pt)| CorrespondingPoint {
107                    object_point: obj_pt,
108                    image_point: im_pt,
109                })
110                .collect()
111        })
112        .collect();
113
114    calibrate_camera(&pts, size.width as i32, size.height as i32)
115}
116
117pub fn convert_to_cam_geom<R: RealField>(
118    opencv_results: &CalibrationResult,
119) -> opencv_ros_camera::RosOpenCvIntrinsics<R> {
120    let fx = nalgebra::convert(opencv_results.camera_matrix[0]);
121    let skew = nalgebra::convert(opencv_results.camera_matrix[1]);
122    let fy = nalgebra::convert(opencv_results.camera_matrix[4]);
123    let cx = nalgebra::convert(opencv_results.camera_matrix[2]);
124    let cy = nalgebra::convert(opencv_results.camera_matrix[5]);
125    let dist = nalgebra::Vector5::new(
126        nalgebra::convert(opencv_results.distortion_coeffs[0]),
127        nalgebra::convert(opencv_results.distortion_coeffs[1]),
128        nalgebra::convert(opencv_results.distortion_coeffs[2]),
129        nalgebra::convert(opencv_results.distortion_coeffs[3]),
130        nalgebra::convert(opencv_results.distortion_coeffs[4]),
131    );
132    let dist = opencv_ros_camera::Distortion::from_opencv_vec(dist);
133
134    opencv_ros_camera::RosOpenCvIntrinsics::from_params_with_distortion(fx, skew, fy, cx, cy, dist)
135}
136
137/// Given some checkerboard corner locations, compute intrinsics
138///
139/// This is based on ROS camera_calibration.calibrator.MonoCalibrator. Note
140/// that unlike ROS, which scales the image so that k and p matrices are
141/// different, the code here does not. ROS does this so that undistorted images
142/// fill the entire image area.
143pub fn compute_intrinsics<R: RealField>(
144    size: PixelSize,
145    data: &[CheckerBoardData],
146) -> Result<opencv_ros_camera::RosOpenCvIntrinsics<R>, Error> {
147    let opencv_results = compute_intrinsics_with_raw_opencv::<R>(size, data)?;
148    Ok(convert_to_cam_geom(&opencv_results))
149}
150
151fn mk_object_points(data: &[CheckerBoardData]) -> Vec<Vec<Coords3D>> {
152    /*
153
154    def mk_object_points(self, boards, use_board_size = False):
155        opts = []
156        for i, b in enumerate(boards):
157            num_pts = b.n_cols * b.n_rows
158            opts_loc = numpy.zeros((num_pts, 1, 3), numpy.float32)
159            for j in range(num_pts):
160                opts_loc[j, 0, 0] = (j / b.n_cols)
161                if self.pattern == Patterns.ACircles:
162                    opts_loc[j, 0, 1] = 2*(j % b.n_cols) + (opts_loc[j, 0, 0] % 2)
163                else:
164                    opts_loc[j, 0, 1] = (j % b.n_cols)
165                opts_loc[j, 0, 2] = 0
166                if use_board_size:
167                    opts_loc[j, 0, :] = opts_loc[j, 0, :] * b.dim
168            opts.append(opts_loc)
169        return opts
170
171    */
172    let mut result = Vec::with_capacity(data.len());
173    for b in data.iter() {
174        let num_pts = b.n_cols * b.n_rows;
175        let mut opts_loc: Vec<Coords3D> = Vec::with_capacity(num_pts);
176        for j in 0..num_pts {
177            let x = (j as f64 / b.n_cols as f64).trunc();
178            let y = j as f64 % b.n_cols as f64;
179            let z = 0.0;
180            opts_loc.push((x, y, z));
181        }
182        result.push(opts_loc);
183    }
184    result
185}
camcal/lib.rs

camcal/
lib.rs
