1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149

use nalgebra::RealField;
use serde::{Deserialize, Serialize};

type Coords3D = (f64, f64, f64);
type Coords2D = (f64, f64);

#[derive(Serialize, Deserialize)]
pub struct CheckerBoardData {
    n_rows: usize,
    n_cols: usize,
    points: Vec<Coords2D>,
}

impl CheckerBoardData {
    pub fn new(n_rows: usize, n_cols: usize, points: &[Coords2D]) -> Self {
        let points = points.to_vec();
        Self {
            n_rows,
            n_cols,
            points,
        }
    }
}

fn to_image_points(board: &CheckerBoardData) -> Vec<Coords2D> {
    board.points.clone()
}

#[derive(Debug, Clone)]
pub struct PixelSize {
    width: usize,
    height: usize,
}

impl PixelSize {
    pub fn new(width: usize, height: usize) -> Self {
        Self { width, height }
    }
}

/// Given some checkerboard corner locations, compute intrinsics using OpenCV.
pub fn compute_intrinsics_with_raw_opencv<R: RealField>(
    size: PixelSize,
    data: &[CheckerBoardData],
) -> Result<opencv_calibrate::CalibrationResult, opencv_calibrate::Error> {
    /*
    cal = camera_calibration.calibrator.MonoCalibrator([])
    cal.size = (width,height)
    r = cal.cal_fromcorners(goodcorners)
    msg = cal.as_message()
    */

    let object_points: Vec<Vec<Coords3D>> = mk_object_points(data);
    let image_points: Vec<Vec<Coords2D>> = data.iter().map(to_image_points).collect();

    debug_assert!(object_points.len() == image_points.len());

    use opencv_calibrate::CorrespondingPoint;
    let pts: Vec<Vec<CorrespondingPoint>> = object_points
        .into_iter()
        .zip(image_points.into_iter())
        .map(|(obj_pts, im_pts)| {
            obj_pts
                .into_iter()
                .zip(im_pts.into_iter())
                .map(|(obj_pt, im_pt)| CorrespondingPoint {
                    object_point: obj_pt,
                    image_point: im_pt,
                })
                .collect()
        })
        .collect();

    Ok(opencv_calibrate::calibrate_camera(
        &pts,
        size.width as i32,
        size.height as i32,
    )?)
}

pub fn convert_to_cam_geom<R: RealField>(
    opencv_results: &opencv_calibrate::CalibrationResult,
) -> opencv_ros_camera::RosOpenCvIntrinsics<R> {
    let fx = nalgebra::convert(opencv_results.camera_matrix[0]);
    let skew = nalgebra::convert(opencv_results.camera_matrix[1]);
    let fy = nalgebra::convert(opencv_results.camera_matrix[4]);
    let cx = nalgebra::convert(opencv_results.camera_matrix[2]);
    let cy = nalgebra::convert(opencv_results.camera_matrix[5]);
    let dist = nalgebra::Vector5::new(
        nalgebra::convert(opencv_results.distortion_coeffs[0]),
        nalgebra::convert(opencv_results.distortion_coeffs[1]),
        nalgebra::convert(opencv_results.distortion_coeffs[2]),
        nalgebra::convert(opencv_results.distortion_coeffs[3]),
        nalgebra::convert(opencv_results.distortion_coeffs[4]),
    );
    let dist = opencv_ros_camera::Distortion::from_opencv_vec(dist);

    opencv_ros_camera::RosOpenCvIntrinsics::from_params_with_distortion(fx, skew, fy, cx, cy, dist)
}

/// Given some checkerboard corner locations, compute intrinsics
///
/// This is based on ROS camera_calibration.calibrator.MonoCalibrator. Note
/// that unlike ROS, which scales the image so that k and p matrices are
/// different, the code here does not. ROS does this so that undistorted images
/// fill the entire image area.
pub fn compute_intrinsics<R: RealField>(
    size: PixelSize,
    data: &[CheckerBoardData],
) -> Result<opencv_ros_camera::RosOpenCvIntrinsics<R>, opencv_calibrate::Error> {
    let opencv_results = compute_intrinsics_with_raw_opencv::<R>(size, data)?;
    Ok(convert_to_cam_geom(&opencv_results))
}

fn mk_object_points(data: &[CheckerBoardData]) -> Vec<Vec<Coords3D>> {
    /*

    def mk_object_points(self, boards, use_board_size = False):
        opts = []
        for i, b in enumerate(boards):
            num_pts = b.n_cols * b.n_rows
            opts_loc = numpy.zeros((num_pts, 1, 3), numpy.float32)
            for j in range(num_pts):
                opts_loc[j, 0, 0] = (j / b.n_cols)
                if self.pattern == Patterns.ACircles:
                    opts_loc[j, 0, 1] = 2*(j % b.n_cols) + (opts_loc[j, 0, 0] % 2)
                else:
                    opts_loc[j, 0, 1] = (j % b.n_cols)
                opts_loc[j, 0, 2] = 0
                if use_board_size:
                    opts_loc[j, 0, :] = opts_loc[j, 0, :] * b.dim
            opts.append(opts_loc)
        return opts

    */
    let mut result = Vec::with_capacity(data.len());
    for b in data.iter() {
        let num_pts = b.n_cols * b.n_rows;
        let mut opts_loc: Vec<Coords3D> = Vec::with_capacity(num_pts);
        for j in 0..num_pts {
            let x = (j as f64 / b.n_cols as f64).trunc();
            let y = j as f64 % b.n_cols as f64;
            let z = 0.0;
            opts_loc.push((x, y, z));
        }
        result.push(opts_loc);
    }
    result
}