Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix camera matrix correction method #1650

Closed
wants to merge 8 commits into from
+134 −42
Closed

Fix camera matrix correction method #1650

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
3808476
Camera matrix corrections now works
tuxbotix Feb 23, 2025
6d5e908
Fix camera matrix correction method
tuxbotix Feb 23, 2025
e0a31b3
Fix other usecases
tuxbotix Feb 23, 2025
b9b40e7
Robot body-head rotations as parameters
tuxbotix Feb 25, 2025
62fc1e1
Fix error of test
tuxbotix Feb 25, 2025
c411b2b
Remove extranous corrections
tuxbotix Feb 25, 2025
8ae0d11
Remove extranous asserts
tuxbotix Feb 25, 2025
7a7ad52
Minor cleanups
tuxbotix Feb 25, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
61 changes: 40 additions & 21 deletions crates/control/src/camera_matrix_calculator.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,15 +1,15 @@
use std::f32::consts::FRAC_PI_2;

use color_eyre::Result;
use nalgebra::UnitQuaternion;
use nalgebra::{UnitQuaternion, Vector3 as NalgebraVector3};
use projection::{camera_matrices::CameraMatrices, camera_matrix::CameraMatrix, Projection};
use serde::{Deserialize, Serialize};

use context_attribute::context;
use coordinate_systems::{Camera, Ground, Head, Pixel, Robot};
use framework::{AdditionalOutput, MainOutput};
use geometry::line_segment::LineSegment;
use linear_algebra::{point, vector, IntoTransform, Isometry3, Vector3};
use linear_algebra::{point, vector, IntoTransform, Isometry3, Rotation3, Vector3};
use types::{
field_dimensions::FieldDimensions, field_lines::ProjectedFieldLines,
parameters::CameraMatrixParameters, robot_dimensions::RobotDimensions,
Expand All @@ -29,11 +29,13 @@ pub struct CycleContext {
robot_kinematics: Input<RobotKinematics, "robot_kinematics">,
robot_to_ground: RequiredInput<Option<Isometry3<Robot, Ground>>, "robot_to_ground?">,

field_dimensions: Parameter<FieldDimensions, "field_dimensions">,
bottom_camera_matrix_parameters:
Parameter<CameraMatrixParameters, "camera_matrix_parameters.vision_bottom">,
field_dimensions: Parameter<FieldDimensions, "field_dimensions">,
top_camera_matrix_parameters:
Parameter<CameraMatrixParameters, "camera_matrix_parameters.vision_top">,
robot_rotation_parameters:
Parameter<NalgebraVector3<f32>, "camera_matrix_parameters.robot_rotation_parameters">,
}

#[context]
Expand All @@ -49,24 +51,35 @@ impl CameraMatrixCalculator {

pub fn cycle(&mut self, mut context: CycleContext) -> Result<MainOutputs> {
let image_size = vector![640.0, 480.0];
let head_to_top_camera = head_to_camera(

let robot_rotation_radians = context
.robot_rotation_parameters
.map(|degree: f32| degree.to_radians());

let correction_robot = Rotation3::from_euler_angles(
robot_rotation_radians.x,
robot_rotation_radians.y,
robot_rotation_radians.z,
);

let (head_to_top_camera, corrections_top_camera) = head_to_camera(
context.top_camera_matrix_parameters.extrinsic_rotations,
context
.top_camera_matrix_parameters
.camera_pitch
.to_radians(),
context.top_camera_matrix_parameters.camera_pitch,
RobotDimensions::HEAD_TO_TOP_CAMERA,
);

let top_camera_matrix = CameraMatrix::from_normalized_focal_and_center(
context.top_camera_matrix_parameters.focal_lengths,
context.top_camera_matrix_parameters.cc_optical_center,
image_size,
context.robot_to_ground.inverse(),
context.robot_kinematics.head.head_to_robot.inverse(),
head_to_top_camera,
corrections_top_camera,
correction_robot,
);

let head_to_bottom_camera = head_to_camera(
let (head_to_bottom_camera, corrections_bottom_camera) = head_to_camera(
context.bottom_camera_matrix_parameters.extrinsic_rotations,
context
.bottom_camera_matrix_parameters
Expand All @@ -81,6 +94,8 @@ impl CameraMatrixCalculator {
context.robot_to_ground.inverse(),
context.robot_kinematics.head.head_to_robot.inverse(),
head_to_bottom_camera,
corrections_bottom_camera,
correction_robot,
);

let field_dimensions = context.field_dimensions;
Expand Down Expand Up @@ -146,21 +161,25 @@ fn project_penalty_area_on_images(
}

fn head_to_camera(
extrinsic_rotation: nalgebra::Vector3<f32>,
camera_pitch: f32,
extrinsic_rotation_degrees: nalgebra::Vector3<f32>,
camera_pitch_degrees: f32,
head_to_camera: Vector3<Head>,
) -> Isometry3<Head, Camera> {
let extrinsic_angles_in_radians = extrinsic_rotation.map(|degree: f32| degree.to_radians());
) -> (Isometry3<Head, Camera>, Rotation3<Camera, Camera>) {
let extrinsic_angles_in_radians =
extrinsic_rotation_degrees.map(|degree: f32| degree.to_radians());
let extrinsic_rotation = UnitQuaternion::from_euler_angles(
extrinsic_angles_in_radians.x,
extrinsic_angles_in_radians.y,
extrinsic_angles_in_radians.z,
);

(extrinsic_rotation
* nalgebra::Isometry3::rotation(nalgebra::Vector3::x() * -camera_pitch)
* nalgebra::Isometry3::rotation(nalgebra::Vector3::y() * -FRAC_PI_2)
* nalgebra::Isometry3::rotation(nalgebra::Vector3::x() * FRAC_PI_2)
* nalgebra::Isometry3::from(-head_to_camera.inner))
.framed_transform()
)
.framed_transform();

let uncalibrated_head_to_camera =
(nalgebra::Isometry3::rotation(
nalgebra::Vector3::x() * -camera_pitch_degrees.to_radians(),
) * nalgebra::Isometry3::rotation(nalgebra::Vector3::y() * -FRAC_PI_2)
* nalgebra::Isometry3::rotation(nalgebra::Vector3::x() * FRAC_PI_2)
* nalgebra::Isometry3::from(-head_to_camera.inner))
.framed_transform();
(uncalibrated_head_to_camera, extrinsic_rotation)
}
49 changes: 33 additions & 16 deletions crates/projection/src/camera_matrix.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,17 +21,21 @@ use crate::{
PathIntrospect,
)]
pub struct CameraMatrix {
pub ground_to_robot: Isometry3<Ground, Robot>,
pub robot_to_head: Isometry3<Robot, Head>,
pub head_to_camera: Isometry3<Head, Camera>,
pub intrinsics: Intrinsic,
pub focal_length: nalgebra::Vector2<f32>,
pub optical_center: Point2<Pixel>,
pub field_of_view: nalgebra::Vector2<f32>,
pub horizon: Option<Horizon>,
pub image_size: Vector2<Pixel>,

pub uncalibrated_head_to_camera: Isometry3<Head, Camera>,
pub uncalibrated_ground_to_robot: Isometry3<Ground, Robot>,
pub uncalibrated_robot_to_head: Isometry3<Robot, Head>,

// Precomputed values for faster calculations
pub ground_to_robot: Isometry3<Ground, Robot>,
pub robot_to_head: Isometry3<Robot, Head>,
pub head_to_camera: Isometry3<Head, Camera>,
pub ground_to_camera: Isometry3<Ground, Camera>,

pub ground_to_pixel: CameraProjection<Ground>,
Expand All @@ -44,9 +48,11 @@ impl CameraMatrix {
focal_length: nalgebra::Vector2<f32>,
optical_center: nalgebra::Point2<f32>,
image_size: Vector2<Pixel>,
ground_to_robot: Isometry3<Ground, Robot>,
robot_to_head: Isometry3<Robot, Head>,
head_to_camera: Isometry3<Head, Camera>,
uncalibrated_ground_to_robot: Isometry3<Ground, Robot>,
uncalibrated_robot_to_head: Isometry3<Robot, Head>,
uncalibrated_head_to_camera: Isometry3<Head, Camera>,
correction_in_camera: Rotation3<Camera, Camera>,
correction_in_robot: Rotation3<Robot, Robot>,
) -> Self {
let focal_length_scaled = focal_length.component_mul(&image_size.inner);
let optical_center_scaled = optical_center
Expand All @@ -57,6 +63,9 @@ impl CameraMatrix {

let field_of_view = Self::calculate_field_of_view(focal_length_scaled, image_size);

let ground_to_robot = correction_in_robot * uncalibrated_ground_to_robot;
let robot_to_head = uncalibrated_robot_to_head * correction_in_robot;
let head_to_camera = correction_in_camera * uncalibrated_head_to_camera;
let ground_to_camera = head_to_camera * robot_to_head * ground_to_robot;

let intrinsics = Intrinsic::new(focal_length_scaled, optical_center_scaled);
Expand All @@ -69,11 +78,15 @@ impl CameraMatrix {
optical_center: optical_center_scaled,
field_of_view,
horizon,
image_size,

uncalibrated_head_to_camera,
uncalibrated_ground_to_robot,
uncalibrated_robot_to_head,
// Precomputed values
ground_to_robot,
robot_to_head,
head_to_camera,
image_size,
// Precomputed values
ground_to_camera,
ground_to_pixel: CameraProjection::new(ground_to_camera, intrinsics.clone()),
pixel_to_ground: CameraProjection::new(ground_to_camera, intrinsics).inverse(0.0),
Expand Down Expand Up @@ -105,29 +118,33 @@ impl CameraMatrix {
correction_in_robot: Rotation3<Robot, Robot>,
correction_in_camera: Rotation3<Camera, Camera>,
) -> Self {
let corrected_ground_to_robot = correction_in_robot * self.ground_to_robot;
let corrected_robot_to_head = self.robot_to_head * correction_in_robot;
let corrected_head_to_camera = correction_in_camera * self.head_to_camera;

let corrected_ground_to_robot = correction_in_robot * self.uncalibrated_ground_to_robot;
let corrected_robot_to_head = self.uncalibrated_robot_to_head * correction_in_robot;
let corrected_head_to_camera = correction_in_camera * self.uncalibrated_head_to_camera;
let corrected_ground_to_camera =
corrected_head_to_camera * corrected_robot_to_head * corrected_ground_to_robot;

let ground_to_pixel =
CameraProjection::new(corrected_ground_to_camera, self.intrinsics.clone());

Self {
// precomputed values with corrections
ground_to_robot: corrected_ground_to_robot,
robot_to_head: corrected_robot_to_head,
head_to_camera: corrected_head_to_camera,
ground_to_camera: corrected_ground_to_camera,
ground_to_pixel: ground_to_pixel.clone(),
pixel_to_ground: ground_to_pixel.inverse(0.0),
horizon: Horizon::from_parameters(corrected_ground_to_camera, &self.intrinsics),
// original values
intrinsics: self.intrinsics.clone(),
focal_length: self.focal_length,
optical_center: self.optical_center,
field_of_view: self.field_of_view,
horizon: Horizon::from_parameters(corrected_ground_to_camera, &self.intrinsics),
image_size: self.image_size,
ground_to_camera: corrected_ground_to_camera,
ground_to_pixel: ground_to_pixel.clone(),
pixel_to_ground: ground_to_pixel.inverse(0.0),
uncalibrated_head_to_camera: self.uncalibrated_head_to_camera,
uncalibrated_ground_to_robot: self.uncalibrated_ground_to_robot,
uncalibrated_robot_to_head: self.uncalibrated_robot_to_head,
}
}
}
Expand Down
49 changes: 48 additions & 1 deletion crates/projection/tests/projection.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,8 @@ use std::f32::consts::{FRAC_PI_2, FRAC_PI_4};
use approx::assert_relative_eq;

use coordinate_systems::{Camera, Head, Pixel};
use linear_algebra::{point, vector, IntoTransform, Isometry3, Vector2, Vector3};
use linear_algebra::{point, vector, IntoTransform, Isometry3, Rotation3, Vector2, Vector3};
use nalgebra::UnitQuaternion;
use projection::{camera_matrix::CameraMatrix, Projection};

fn from_normalized_focal_and_center_short(
Expand All @@ -18,6 +19,8 @@ fn from_normalized_focal_and_center_short(
Isometry3::identity(),
Isometry3::identity(),
Isometry3::from_translation(0.0, 0.0, 1.0),
Rotation3::from_euler_angles(0.0, 0.0, 0.0),
Rotation3::from_euler_angles(0.0, 0.0, 0.0),
)
}

Expand Down Expand Up @@ -294,3 +297,47 @@ fn get_pixel_radius_pitch_45_degree_down() {
epsilon = 0.01,
);
}

#[test]
fn check_corrections_vs_from_construction() {
let correction_camera = UnitQuaternion::from_euler_angles(
4.0f32.to_radians(),
-2.0f32.to_radians(),
1.0f32.to_radians(),
)
.framed_transform();
let correction_robot = UnitQuaternion::from_euler_angles(
4.0f32.to_radians(),
-2.0f32.to_radians(),
1.0f32.to_radians(),
)
.framed_transform();

let camera_matrix = CameraMatrix::from_normalized_focal_and_center(
nalgebra::vector![0.95, 1.27],
nalgebra::point![0.5, 0.5],
vector![640.0, 480.0],
Isometry3::identity(),
Isometry3::identity(),
Isometry3::from_translation(0.0, 0.0, 1.0),
correction_camera,
correction_robot,
);

let corrected_varient = camera_matrix.to_corrected(correction_robot, correction_camera);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

corrected_variant


// Doing equal-assert is fair as the calculations should be done in the same way and order.
assert_eq!(
camera_matrix.ground_to_robot,
corrected_varient.ground_to_robot
);
assert_eq!(
camera_matrix.head_to_camera,
corrected_varient.head_to_camera
);
assert_eq!(camera_matrix.robot_to_head, corrected_varient.robot_to_head);
assert_eq!(
camera_matrix.ground_to_pixel,
corrected_varient.ground_to_pixel
);
}
4 changes: 3 additions & 1 deletion crates/vision/src/ball_detection.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -372,7 +372,7 @@ mod tests {

use approx::assert_relative_eq;
use coordinate_systems::{Camera, Ground, Head};
use linear_algebra::{IntoTransform, Isometry3, Vector3};
use linear_algebra::{IntoTransform, Isometry3, Rotation3, Vector3};
use nalgebra::{Translation, UnitQuaternion};

use super::*;
Expand Down Expand Up @@ -527,6 +527,8 @@ mod tests {
.framed_transform(),
nalgebra::Isometry3::identity().framed_transform(),
head_to_camera(0.0, Vector3::zeros()),
Rotation3::from_euler_angles(0.0, 0.0, 0.0),
Rotation3::from_euler_angles(0.0, 0.0, 0.0),
);

let mut additional_output_buffer = None;
Expand Down
4 changes: 3 additions & 1 deletion crates/vision/src/line_detection/checks.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ fn get_gradient(

#[cfg(test)]
mod tests {
use linear_algebra::{point, IntoTransform};
use linear_algebra::{point, IntoTransform, Rotation3};
use nalgebra::{Isometry3, Translation, UnitQuaternion};

use super::*;
Expand All @@ -128,6 +128,8 @@ mod tests {
.framed_transform(),
Isometry3::identity().framed_transform(),
Isometry3::identity().framed_transform(),
Rotation3::from_euler_angles(0.0, 0.0, 0.0),
Rotation3::from_euler_angles(0.0, 0.0, 0.0),
);

let segment = GenericSegment {
Expand Down
6 changes: 5 additions & 1 deletion crates/vision/src/perspective_grid_candidates_provider.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -166,7 +166,7 @@ mod tests {
use std::iter::FromIterator;

use approx::assert_relative_eq;
use linear_algebra::{vector, IntoTransform, Isometry3};
use linear_algebra::{vector, IntoTransform, Isometry3, Rotation3};
use nalgebra::{Translation, UnitQuaternion};
use types::{
color::{Intensity, YCbCr444},
Expand All @@ -191,6 +191,8 @@ mod tests {
.framed_transform(),
Isometry3::identity(),
Isometry3::identity(),
Rotation3::from_euler_angles(0.0, 0.0, 0.0),
Rotation3::from_euler_angles(0.0, 0.0, 0.0),
);
let minimum_radius = 5.0;

Expand All @@ -213,6 +215,8 @@ mod tests {
.framed_transform(),
Isometry3::identity(),
Isometry3::identity(),
Rotation3::from_euler_angles(0.0, 0.0, 0.0),
Rotation3::from_euler_angles(0.0, 0.0, 0.0),
);
let minimum_radius = 5.0;

Expand Down
3 changes: 2 additions & 1 deletion etc/parameters/default.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,8 @@
"extrinsic_rotations": [0, 0, 0],
"focal_lengths": [0.95, 1.27],
"cc_optical_center": [0.5, 0.5]
}
},
"robot_rotation_parameters": [0, 0, 0]
},
"foot_bumper_filter": {
"activations_needed": 2,
Expand Down