diff --git a/scripts/ci_install_dependencies.sh b/scripts/ci_install_dependencies.sh
index 2dadfda..2fb8004 100755
--- a/scripts/ci_install_dependencies.sh
+++ b/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."
diff --git a/vortex_utils/cpp_test/test_math.cpp b/vortex_utils/cpp_test/test_math.cpp
index d60f7d8..7799036 100644
--- a/vortex_utils/cpp_test/test_math.cpp
+++ b/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
diff --git a/vortex_utils/cpp_test/test_types.cpp b/vortex_utils/cpp_test/test_types.cpp
index 4bcb555..4226d90 100644
--- a/vortex_utils/cpp_test/test_types.cpp
+++ b/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));
+}
diff --git a/vortex_utils/include/vortex/utils/math.hpp b/vortex_utils/include/vortex/utils/math.hpp
index 8ab189b..aea6468 100644
--- a/vortex_utils/include/vortex/utils/math.hpp
+++ b/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
diff --git a/vortex_utils/include/vortex/utils/types.hpp b/vortex_utils/include/vortex/utils/types.hpp
index bcc5eeb..3a9212d 100644
--- a/vortex_utils/include/vortex/utils/types.hpp
+++ b/vortex_utils/include/vortex/utils/types.hpp
@@ -3,6 +3,7 @@
 
 #include <eigen3/Eigen/Core>
 #include <eigen3/Eigen/Dense>
+#include <format>
 #include "math.hpp"
 
 namespace vortex::utils::types {
@@ -25,6 +26,14 @@ struct Pose;
  */
 struct Twist;
 
+/**
+ * @brief Camera intrinsic parameters for an ideal pinhole (perspective) camera,
+ * following Eq. (2.57) in
+ * Szeliski, R., Computer Vision: Algorithms and Applications,
+ * 2nd ed., Springer, 2022.
+ */
+struct CameraIntrinsics;
+
 struct PoseEuler {
     double x{};
     double y{};
@@ -328,6 +337,128 @@ inline PoseEuler Pose::as_pose_euler() const {
     return eta;
 }
 
+struct CameraIntrinsics {
+    double fx{};
+    double fy{};
+    double cx{};
+    double cy{};
+    double skew{0.0};
+
+    void validate_focals() const {
+        if (fx <= 0.0 || fy <= 0.0) {
+            throw std::runtime_error(std::format(
+                "Invalid Camera Matrix K. Focals fx and fy must be positive:\n"
+                "fx = {}, fy = {}",
+                fx, fy));
+        }
+    }
+
+    /**
+     * @brief Get the camera intrinsic matrix K
+     * according to Eq. (2.57) in Szeliski, 2022.
+     * @return Eigen::Matrix3d K
+     */
+    Eigen::Matrix3d K() const {
+        validate_focals();
+        Eigen::Matrix3d k = Eigen::Matrix3d::Identity();
+        k(0, 0) = fx;
+        k(0, 1) = skew;
+        k(0, 2) = cx;
+        k(1, 1) = fy;
+        k(1, 2) = cy;
+
+        return k;
+    }
+
+    /**
+     * @brief Get the inverse camera intrinsic matrix K
+     * @return Eigen::Matrix3d K_inv
+     */
+    Eigen::Matrix3d K_inv() const {
+        validate_focals();
+        Eigen::Matrix3d k_inv = Eigen::Matrix3d::Identity();
+        k_inv(0, 0) = 1.0 / fx;
+        k_inv(1, 1) = 1.0 / fy;
+        k_inv(1, 2) = -cy / fy;
+
+        if (skew != 0.0) {
+            k_inv(0, 1) = -skew / (fx * fy);
+            k_inv(0, 2) = (skew * cy - cx * fy) / (fx * fy);
+        } else {
+            k_inv(0, 2) = -cx / fx;
+        }
+
+        return k_inv;
+    }
+
+    /**
+     * @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 point 3D point in camera coordinates (Xc, Yc, Zc), with Zc > 0.
+     * @return 2D pixel coordinates (u, v).
+     *
+     * @throws std::runtime_error if Zc <= 0.
+     */
+    Eigen::Vector2d project_point(const Eigen::Vector3d& point) const {
+        validate_focals();
+        const double x_c = point.x();
+        const double y_c = point.y();
+        const double z_c = point.z();
+        if (z_c <= 0.0) {
+            throw std::runtime_error(
+                "Projection of point failed. Can't project with z <= 0.");
+        }
+        const double x_norm = x_c / z_c;
+        const double y_norm = y_c / z_c;
+
+        const double u = fx * x_norm + skew * y_norm + cx;
+        const double v = fy * y_norm + cy;
+
+        return {u, v};
+    }
+
+    /**
+     * @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 pixel Eigen::Vector2d representing camera pixel coordinates.
+     * @return ray in camera space (x, y, 1.0).
+     */
+    Eigen::Vector3d backproject_ray(const Eigen::Vector2d& pixel) const {
+        validate_focals();
+        const double u = pixel(0);
+        const double v = pixel(1);
+
+        const double x =
+            u / fx - v * skew / (fx * fy) + (skew * cy - cx * fy) / (fx * fy);
+        const double y = v / fy - cy / fy;
+
+        return {x, y, 1.0};
+    }
+
+    /**
+     * @brief Backproject a pixel using the inverse intrinsics matrix and a
+     * depth value to compute the corresponding 3D point.
+     * @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::Vector2d& pixel,
+                                      double depth) const {
+        if (depth <= 0.0) {
+            throw std::runtime_error(
+                "Backprojection failed. Depth must be positive.");
+        }
+        return depth * backproject_ray(pixel);
+    }
+};
+
 }  // namespace vortex::utils::types
 
 #endif  // VORTEX_UTILS_TYPES_HPP
diff --git a/vortex_utils/src/math.cpp b/vortex_utils/src/math.cpp
index 590c8cf..9d94c4b 100644
--- a/vortex_utils/src/math.cpp
+++ b/vortex_utils/src/math.cpp
@@ -189,4 +189,22 @@ Eigen::Quaterniond enu_ned_rotation(const Eigen::Quaterniond& quat) {
     return q_out.normalized();
 }
 
+Eigen::Vector2d project_point(const Eigen::Matrix3d& intrinsic_matrix,
+                              const Eigen::Vector3d& point) {
+    Eigen::Vector3d point_norm = point / point.z();
+    return (intrinsic_matrix * point_norm).head(2);
+}
+
+Eigen::Vector3d backproject_ray(const Eigen::Matrix3d& intrinsic_matrix_inv,
+                                const Eigen::Vector2d& pixel) {
+    Eigen::Vector3d pixel_h(pixel.x(), pixel.y(), 1.0);
+    return intrinsic_matrix_inv * pixel_h;
+}
+
+Eigen::Vector3d backproject_point(const Eigen::Matrix3d& intrinsic_matrix_inv,
+                                  const Eigen::Vector2d& pixel,
+                                  double depth) {
+    return backproject_ray(intrinsic_matrix_inv, pixel) * depth;
+}
+
 }  // namespace vortex::utils::math