opencv_ros_camera/
ros_file_support.rs

1// This module requires std.
2
3#[cfg(feature = "serde-serialize")]
4use std::io::Read;
5
6use na::{
7    allocator::Allocator,
8    core::{dimension::DimMin, Matrix3, OMatrix, RowVector5, SMatrix},
9};
10
11use na::{DefaultAllocator, DimName, RealField};
12use nalgebra as na;
13
14use crate::{Error, Result, RosOpenCvIntrinsics};
15
16#[cfg(feature = "serde-serialize")]
17use serde::{Deserialize, Serialize};
18
19/// Camera calibration info as saved by ROS.
20///
21/// This is a low-level structure only intended for interoperation with ROS. To
22/// convert to a more Rust-friedly type, use
23/// [`NamedIntrinsicParameters::try_from()`](struct.NamedIntrinsicParameters.html#method.try_from).
24/// To create an instance of this structure from a
25/// [`NamedIntrinsicParameters`](struct.NamedIntrinsicParameters.html) struct,
26/// use [`RosCameraInfo::from()`](struct.RosCameraInfo.html#method.from).
27///
28/// This structure implements the format written by `writeCalibrationYml` in ROS
29/// code `camera_calibration_parsers/src/parse_yml.cpp`. It can be serialized or
30/// deserialized with serde. (It is strange that writeCalibrationYml has its own
31/// serializer rather than using the ROS message type
32/// [`sensor_msgs/CameraInfo`](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/CameraInfo.html).)
33#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
34#[derive(Debug, Clone)]
35pub struct RosCameraInfo<R: RealField> {
36    /// The width of the image sensor (in pixels).
37    pub image_width: usize,
38    /// The height of the image sensor (in pixels).
39    pub image_height: usize,
40    /// The name of the camera
41    pub camera_name: String,
42    /// The camera matrix `k`.
43    pub camera_matrix: RosMatrix<R>,
44    /// The name of the distortion model. Only "plumb_bob" is supported.
45    pub distortion_model: String,
46    /// The coefficients of the distortion parameters.
47    pub distortion_coefficients: RosMatrix<R>,
48    /// The stereo rectification matrix.
49    pub rectification_matrix: RosMatrix<R>,
50    /// The projection matrix `p`.
51    pub projection_matrix: RosMatrix<R>,
52}
53
54impl<R: RealField> From<NamedIntrinsicParameters<R>> for RosCameraInfo<R> {
55    fn from(orig: NamedIntrinsicParameters<R>) -> Self {
56        let d = &orig.intrinsics.distortion;
57
58        let distortion = vec![
59            d.radial1(),
60            d.radial2(),
61            d.tangential1(),
62            d.tangential2(),
63            d.radial3(),
64        ];
65        Self {
66            image_width: orig.width,
67            image_height: orig.height,
68            camera_name: orig.name,
69            camera_matrix: to_ros(orig.intrinsics.k),
70            distortion_model: "plumb_bob".to_string(),
71            distortion_coefficients: to_ros_matrix(1, 5, distortion.as_slice()),
72            rectification_matrix: to_ros(orig.intrinsics.rect),
73            projection_matrix: to_ros(orig.intrinsics.p),
74        }
75    }
76}
77
78/// Matrix saved by ROS.
79///
80/// This is a low-level structure only intended for interoperation with ROS,
81/// specifically as the type of fields within the
82/// [`RosCameraInfo`](struct.RosCameraInfo.html) struct.
83#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
84#[derive(Debug, Clone)]
85pub struct RosMatrix<R: RealField> {
86    /// Number of rows in the matrix.
87    pub rows: usize,
88    /// Number of columns in the matrix.
89    pub cols: usize,
90    /// The data in the matrix stored as a column-major `Vec`.
91    pub data: Vec<R>,
92}
93
94fn to_ros<R: RealField, SS: DimName, OS>(arr: na::OMatrix<R, SS, OS>) -> RosMatrix<R>
95where
96    DefaultAllocator: Allocator<SS, SS>,
97    DefaultAllocator: Allocator<SS>,
98    DefaultAllocator: Allocator<OS, SS>,
99    DefaultAllocator: Allocator<SS, OS>,
100    DefaultAllocator: Allocator<OS, OS>,
101    DefaultAllocator: Allocator<OS>,
102    DefaultAllocator: Allocator<OS>,
103    OS: DimName + DimMin<OS, Output = OS>,
104{
105    // need to transpose the data since na is column major and ros is row major.
106    let a2 = arr.transpose();
107    RosMatrix {
108        rows: arr.nrows(),
109        cols: arr.ncols(),
110        data: a2.as_slice().to_vec(),
111    }
112}
113
114#[inline]
115pub(crate) fn to_ros_matrix<R: RealField>(rows: usize, cols: usize, data: &[R]) -> RosMatrix<R> {
116    RosMatrix {
117        rows,
118        cols,
119        data: Vec::from(data),
120    }
121}
122
123pub(crate) fn get_nalgebra_matrix<R, D1, D2>(
124    ros_matrix: &RosMatrix<R>,
125) -> Result<OMatrix<R, D1, D2>>
126where
127    R: RealField,
128    D1: DimName,
129    D2: DimName,
130    DefaultAllocator: Allocator<D1, D1>,
131    DefaultAllocator: Allocator<D1>,
132    DefaultAllocator: Allocator<D2, D1>,
133    DefaultAllocator: Allocator<D1, D2>,
134    DefaultAllocator: Allocator<D2, D2>,
135    DefaultAllocator: Allocator<D2>,
136{
137    if ros_matrix.rows != D1::dim() {
138        return Err(Error::BadMatrixSize);
139    }
140    if ros_matrix.cols != D2::dim() {
141        return Err(Error::BadMatrixSize);
142    }
143    if ros_matrix.data.len() != (ros_matrix.rows * ros_matrix.cols) {
144        return Err(Error::BadMatrixSize);
145    }
146    let data_converted: Vec<R> = ros_matrix
147        .data
148        .clone()
149        .into_iter()
150        .map(na::convert)
151        .collect();
152    Ok(OMatrix::from_row_slice_generic(
153        D1::name(),
154        D2::name(),
155        &data_converted,
156    ))
157}
158
159/// A struct with `RosOpenCvIntrinsics`, camera name and image sensor dimensions.
160///
161/// This is primarily used to read YAML files saved by ROS. Create this struct
162/// with the [`from_ros_yaml`](fn.from_ros_yaml.html) function.
163///
164/// To extract a [`RosOpenCvIntrinsics`](struct.RosOpenCvIntrinsics.html)
165/// structure from this struct, use the
166/// [`intrinsics`](struct.NamedIntrinsicParameters.html#structfield.intrinsics)
167/// field.
168///
169/// See the [module-level documentation for more information](index.html).
170#[derive(Debug, Clone)]
171pub struct NamedIntrinsicParameters<R: RealField> {
172    /// Name of the camera.
173    pub name: String,
174    /// The width of the image sensor (in pixels).
175    pub width: usize,
176    /// The height of the image sensor (in pixels).
177    pub height: usize,
178    /// The intrinsic parameters.
179    pub intrinsics: RosOpenCvIntrinsics<R>,
180}
181
182impl<R: RealField> std::convert::TryFrom<RosCameraInfo<R>> for NamedIntrinsicParameters<R> {
183    type Error = Error;
184    fn try_from(ros_camera: RosCameraInfo<R>) -> Result<NamedIntrinsicParameters<R>> {
185        let intrinsics = {
186            let p = get_nalgebra_matrix(&ros_camera.projection_matrix)?;
187            let k: SMatrix<R, 3, 3> = get_nalgebra_matrix(&ros_camera.camera_matrix)?;
188            if ros_camera.distortion_model != "plumb_bob" {
189                return Err(Error::UnknownDistortionModel);
190            }
191            let d: RowVector5<R> = get_nalgebra_matrix(&ros_camera.distortion_coefficients)?;
192            let rect: Matrix3<R> = get_nalgebra_matrix(&ros_camera.rectification_matrix)?;
193            let distortion = crate::Distortion::from_opencv_vec(d.transpose());
194            RosOpenCvIntrinsics::from_components(p, k, distortion, rect)?
195        };
196        Ok(NamedIntrinsicParameters {
197            name: ros_camera.camera_name,
198            width: ros_camera.image_width,
199            height: ros_camera.image_height,
200            intrinsics,
201        })
202    }
203}
204
205#[cfg(feature = "serde-serialize")]
206/// Construct NamedIntrinsicParameters from ROS format YAML data.
207///
208/// This is a small wrapper around `serde_yaml::from_reader()` and
209/// [`NamedIntrinsicParameters::try_from()`](struct.NamedIntrinsicParameters.html#method.try_from).
210///
211/// See the [module-level documentation for more information](index.html),
212/// including an example of use.
213pub fn from_ros_yaml<R, Rd>(reader: Rd) -> Result<NamedIntrinsicParameters<R>>
214where
215    R: RealField + serde::de::DeserializeOwned,
216    Rd: Read,
217{
218    let ros_camera: RosCameraInfo<R> = serde_yaml::from_reader(reader)?;
219    Ok(std::convert::TryInto::try_into(ros_camera)?)
220}
opencv_ros_camera/ros_file_support.rs

opencv_ros_camera/
ros_file_support.rs
