free projection functions

Author: jorgenfj

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/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

vortex_utils/include/vortex/utils/types.hpp

@@ -426,6 +426,7 @@ struct CameraIntrinsics {
      * @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 {
@@ -442,10 +443,16 @@ struct CameraIntrinsics {
     /**
      * @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);
     }
 };

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