Feat/camera matrix type

scripts/ci_install_dependencies.sh

@@ -23,4 +23,16 @@ echo "Installing PyGObject dependencies via apt..."
 sudo apt install -y libglib2.0-dev libcairo2-dev libgirepository1.0-dev \
     gir1.2-gtk-3.0 python3-gi-cairo
 
+# Add Ubuntu Toolchain PPA to get gcc-13/g++-13
+sudo apt-get update -qq
+sudo apt-get install -y --no-install-recommends software-properties-common
+sudo add-apt-repository ppa:ubuntu-toolchain-r/test -y
+sudo apt-get update -qq
+
+# Install and switch to GCC 13
+sudo apt-get install -y --no-install-recommends gcc-13 g++-13 lcov
+sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-13 100
+sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-13 100
+sudo update-alternatives --install /usr/bin/gcov gcov /usr/bin/gcov-13 100
+
 echo "All dependencies installed successfully via apt."

vortex_utils/cpp_test/test_math.cpp

@@ -459,4 +459,62 @@ TEST(enu_ned_rotation, test_symmetry) {
                                1e-12));
 }
 
+TEST(project_point, test_project_point_basic) {
+    Eigen::Matrix3d K;
+    K << 300.0, 0.0, 150.0, 0.0, 400.0, 200.0, 0.0, 0.0, 1.0;
+
+    Eigen::Vector3d point(2.0, 4.0, 2.0);
+
+    Eigen::Vector2d pixel = project_point(K, point);
+
+    const double x_norm = 2.0 / 2.0;
+    const double y_norm = 4.0 / 2.0;
+
+    EXPECT_NEAR(pixel.x(), 300.0 * x_norm + 150.0, 1e-12);
+    EXPECT_NEAR(pixel.y(), 400.0 * y_norm + 200.0, 1e-12);
+}
+
+TEST(project_point, test_project_point_with_skew) {
+    Eigen::Matrix3d K;
+    K << 300.0, 50.0, 100.0, 0.0, 300.0, 100.0, 0.0, 0.0, 1.0;
+
+    Eigen::Vector3d point(2.0, 1.0, 1.0);
+
+    Eigen::Vector2d pixel = project_point(K, point);
+
+    EXPECT_NEAR(pixel.x(), 300.0 * 2.0 + 50.0 * 1.0 + 100.0, 1e-12);
+    EXPECT_NEAR(pixel.y(), 300.0 * 1.0 + 100.0, 1e-12);
+}
+
+TEST(backproject_ray, test_backproject_ray_explicit_values) {
+    Eigen::Matrix3d K_inv;
+    K_inv << 1.0 / 200.0, -20.0 / (200.0 * 400.0),
+        (20.0 * 50.0 - 100.0 * 400.0) / (200.0 * 400.0), 0.0, 1.0 / 400.0,
+        -50.0 / 400.0, 0.0, 0.0, 1.0;
+
+    Eigen::Vector2d pixel(300.0, 250.0);
+
+    Eigen::Vector3d ray = backproject_ray(K_inv, pixel);
+
+    EXPECT_NEAR(ray.x(), 0.95, 1e-12);
+    EXPECT_NEAR(ray.y(), 0.5, 1e-12);
+    EXPECT_NEAR(ray.z(), 1.0, 1e-12);
+}
+
+TEST(backproject_point, test_backproject_point_explicit_values) {
+    Eigen::Matrix3d K_inv;
+    K_inv << 1.0 / 200.0, -20.0 / (200.0 * 400.0),
+        (20.0 * 50.0 - 100.0 * 400.0) / (200.0 * 400.0), 0.0, 1.0 / 400.0,
+        -50.0 / 400.0, 0.0, 0.0, 1.0;
+
+    Eigen::Vector2d pixel(300.0, 250.0);
+    const double depth = 4.0;
+
+    Eigen::Vector3d point = backproject_point(K_inv, pixel, depth);
+
+    EXPECT_NEAR(point.x(), 3.8, 1e-12);
+    EXPECT_NEAR(point.y(), 2.0, 1e-12);
+    EXPECT_NEAR(point.z(), 4.0, 1e-12);
+}
+
 }  // namespace vortex::utils::math

vortex_utils/cpp_test/test_types.cpp

@@ -143,3 +143,150 @@ TEST_F(TypesTests, test_twist) {
     EXPECT_NEAR(diff.q, 0.3, 1e-12);
     EXPECT_NEAR(diff.r, 1.4, 1e-12);
 }
+
+TEST_F(TypesTests, test_default_focals_throw) {
+    vortex::utils::types::CameraIntrinsics intr;
+
+    EXPECT_THROW(intr.K(), std::runtime_error);
+}
+
+TEST_F(TypesTests, test_negative_focals_throw) {
+    vortex::utils::types::CameraIntrinsics intr{
+        .fx = -1.0, .fy = -1.0, .cx = 100, .cy = 100, .skew = 0.0};
+
+    EXPECT_THROW(intr.K(), std::runtime_error);
+}
+
+TEST_F(TypesTests, test_camera_intrinsics_K_no_skew) {
+    vortex::utils::types::CameraIntrinsics intr{
+        .fx = 300.0, .fy = 400.0, .cx = 150.0, .cy = 200.0, .skew = 0.0};
+
+    Eigen::Matrix3d K = intr.K();
+
+    Eigen::Matrix3d expected;
+    expected << 300.0, 0.0, 150.0, 0.0, 400.0, 200.0, 0.0, 0.0, 1.0;
+
+    EXPECT_TRUE(K.isApprox(expected, 1e-12));
+}
+
+TEST_F(TypesTests, test_camera_intrinsics_K_with_skew) {
+    vortex::utils::types::CameraIntrinsics intr{
+        .fx = 300.0, .fy = 400.0, .cx = 150.0, .cy = 200.0, .skew = 10.0};
+
+    Eigen::Matrix3d K = intr.K();
+
+    Eigen::Matrix3d expected;
+    expected << 300.0, 10.0, 150.0, 0.0, 400.0, 200.0, 0.0, 0.0, 1.0;
+
+    EXPECT_TRUE(K.isApprox(expected, 1e-12));
+}
+
+TEST_F(TypesTests, test_camera_intrinsics_K_inv_no_skew) {
+    vortex::utils::types::CameraIntrinsics intr{
+        .fx = 200.0, .fy = 400.0, .cx = 100.0, .cy = 50.0, .skew = 0.0};
+
+    Eigen::Matrix3d K_inv = intr.K_inv();
+
+    Eigen::Matrix3d expected;
+    expected << 1.0 / 200.0, 0.0, -100.0 / 200.0, 0.0, 1.0 / 400.0,
+        -50.0 / 400.0, 0.0, 0.0, 1.0;
+
+    EXPECT_TRUE(K_inv.isApprox(expected, 1e-12));
+}
+
+TEST_F(TypesTests, test_camera_intrinsics_K_times_K_inv_identity) {
+    vortex::utils::types::CameraIntrinsics intr{
+        .fx = 200.0, .fy = 400.0, .cx = 100.0, .cy = 50.0, .skew = 0.0};
+
+    Eigen::Matrix3d I = intr.K() * intr.K_inv();
+    EXPECT_TRUE(I.isApprox(Eigen::Matrix3d::Identity(), 1e-12));
+}
+
+TEST_F(TypesTests, test_project) {
+    vortex::utils::types::CameraIntrinsics intr{
+        .fx = 300.0, .fy = 300.0, .cx = 150.0, .cy = 150.0, .skew = 0.0};
+
+    Eigen::Vector3d point(10.0, 10.0, 10.0);
+
+    Eigen::Vector2d pixel = intr.project_point(point);
+
+    EXPECT_NEAR(pixel.x(), 450.0, 1e-12);
+    EXPECT_NEAR(pixel.y(), 450.0, 1e-12);
+}
+
+TEST_F(TypesTests, test_project_with_skew) {
+    vortex::utils::types::CameraIntrinsics intr{
+        .fx = 300.0, .fy = 300.0, .cx = 100.0, .cy = 100.0, .skew = 50.0};
+
+    Eigen::Vector3d point(2.0, 1.0, 1.0);
+
+    Eigen::Vector2d pixel = intr.project_point(point);
+
+    const double x_norm = 2.0;
+    const double y_norm = 1.0;
+
+    EXPECT_NEAR(pixel.x(), 300.0 * x_norm + 50.0 * y_norm + 100.0, 1e-12);
+    EXPECT_NEAR(pixel.y(), 300.0 * y_norm + 100.0, 1e-12);
+}
+
+TEST_F(TypesTests, test_project_throws_on_nonpositive_z) {
+    vortex::utils::types::CameraIntrinsics intr{
+        .fx = 300.0, .fy = 300.0, .cx = 150.0, .cy = 150.0, .skew = 0.0};
+
+    Eigen::Vector3d point(1.0, 1.0, 0.0);
+
+    EXPECT_THROW(intr.project_point(point), std::runtime_error);
+}
+
+TEST_F(TypesTests, test_backproject_ray_matches_K_inv) {
+    vortex::utils::types::CameraIntrinsics intr{
+        .fx = 300.0, .fy = 400.0, .cx = 150.0, .cy = 200.0, .skew = 10.0};
+
+    Eigen::Vector2d pixel(350.0, 260.0);
+
+    Eigen::Vector3d ray = intr.backproject_ray(pixel);
+
+    Eigen::Vector3d expected =
+        intr.K_inv() * Eigen::Vector3d(pixel.x(), pixel.y(), 1.0);
+
+    EXPECT_TRUE(ray.isApprox(expected, 1e-12));
+}
+
+TEST_F(TypesTests, test_backproject_point_scales_ray) {
+    vortex::utils::types::CameraIntrinsics intr{
+        .fx = 250.0, .fy = 300.0, .cx = 100.0, .cy = 120.0, .skew = 5.0};
+
+    Eigen::Vector2d pixel(200.0, 180.0);
+    const double depth = 4.0;
+
+    Eigen::Vector3d point = intr.backproject_point(pixel, depth);
+    Eigen::Vector3d ray = intr.backproject_ray(pixel);
+
+    EXPECT_TRUE(point.isApprox(depth * ray, 1e-12));
+}
+
+TEST_F(TypesTests, test_project_backproject_roundtrip) {
+    vortex::utils::types::CameraIntrinsics intr{
+        .fx = 300.0, .fy = 350.0, .cx = 160.0, .cy = 120.0, .skew = 20.0};
+
+    Eigen::Vector3d X_cam(1.5, -0.5, 4.0);
+
+    Eigen::Vector2d pixel = intr.project_point(X_cam);
+    Eigen::Vector3d X_recon = intr.backproject_point(pixel, X_cam.z());
+
+    EXPECT_TRUE(X_cam.isApprox(X_recon, 1e-12));
+}
+
+TEST_F(TypesTests, test_backproject_ray_direction_consistency) {
+    vortex::utils::types::CameraIntrinsics intr{
+        .fx = 300.0, .fy = 300.0, .cx = 150.0, .cy = 150.0, .skew = 0.0};
+
+    Eigen::Vector2d pixel(200.0, 180.0);
+
+    Eigen::Vector3d ray = intr.backproject_ray(pixel);
+    Eigen::Vector3d p1 = intr.backproject_point(pixel, 2.0);
+    Eigen::Vector3d p2 = intr.backproject_point(pixel, 5.0);
+
+    EXPECT_TRUE(ray.normalized().isApprox(p1.normalized(), 1e-12));
+    EXPECT_TRUE(ray.normalized().isApprox(p2.normalized(), 1e-12));
+}

vortex_utils/include/vortex/utils/math.hpp

@@ -165,6 +165,45 @@ Eigen::Quaterniond average_quaternions(
  */
 Eigen::Quaterniond enu_ned_rotation(const Eigen::Quaterniond& quat);
 
+/**
+ * @brief Project a 3D point expressed in the camera coordinate frame
+ *        onto the image sensor, producing pixel coordinates.
+ *
+ * This function implements the pinhole camera model by:
+ *  1) applying perspective division (Eq. 2.50 in Szeliski),
+ *  2) mapping normalized image coordinates to pixel coordinates
+ *     using the camera intrinsics (Eq. 2.54 with K defined in Eq. 2.57).
+ *
+ * @param intrinsic_matrix Eigen::Matrix3d representing the camera matrix K.
+ * @param point 3D point in camera coordinates (Xc, Yc, Zc), with Zc > 0.
+ * @return 2D pixel coordinates (u, v).
+ */
+Eigen::Vector2d project_point(const Eigen::Matrix3d& intrinsic_matrix,
+                              const Eigen::Vector3d& point);
+/**
+ * @brief Backproject a pixel using the inverse intrinsics matrix to produce
+ * a ray through the pixel. The ray is in normalized image coordinates with
+ * z = 1.0 and is not necessarily normalized.
+ * @param intrinsic_matrix_inv Eigen::Matrix3d representing the inverse camera
+ * matrix K_inv.
+ * @param pixel Eigen::Vector2d representing the pixel to backproject.
+ * @return ray in camera space (x, y, 1.0).
+ */
+Eigen::Vector3d backproject_ray(const Eigen::Matrix3d& intrinsic_matrix_inv,
+                                const Eigen::Vector2d& pixel);
+
+/**
+ * @brief Backproject a pixel using the inverse intrinsics matrix and a
+ * depth value to compute the corresponding 3D point.
+ * @param intrinsic_matrix_inv Eigen::Matrix3d representing the inverse camera
+ * matrix K_inv.
+ * @param pixel Eigen::Vector2d representing camera pixel coordinates.
+ * @param depth Depth for the corresponding pixel.
+ * @return 3D point in camera space
+ */
+Eigen::Vector3d backproject_point(const Eigen::Matrix3d& intrinsic_matrix_inv,
+                                  const Eigen::Vector2d& pixel,
+                                  double depth);
 }  // namespace vortex::utils::math
 
 #endif  // VORTEX_UTILS_MATH_HPP