{Feature} C++ - Core - Add Jacobian computation in CameraProjection

core/calibration/camera_projections/CameraProjection.cpp

@@ -79,11 +79,12 @@ Eigen::Matrix<Scalar, 2, 1> CameraProjectionTemplated<Scalar>::getPrincipalPoint
 
 template <typename Scalar>
 Eigen::Matrix<Scalar, 2, 1> CameraProjectionTemplated<Scalar>::project(
-    const Eigen::Matrix<Scalar, 3, 1>& pointInCamera) const {
+    const Eigen::Matrix<Scalar, 3, 1>& pointInCamera,
+    Eigen::Matrix<Scalar, 2, 3>* jacobianWrtPoint) const {
   return std::visit(
       [&](auto&& projection) {
         using T = std::decay_t<decltype(projection)>;
-        return T::project(pointInCamera, projectionParams_);
+        return T::project(pointInCamera, projectionParams_, jacobianWrtPoint);
       },
       projectionVariant_);
 }

core/calibration/camera_projections/CameraProjection.h

@@ -67,8 +67,16 @@ struct CameraProjectionTemplated {
   /**
    * @brief projects a 3d world point in the camera space to a 2d pixel in the image space. No
    * checks performed in this process.
+   *
+   * @param pointInCamera The 3D point in camera space to be projected.
+   * @param jacobianWrtPoint Optional, if not null, will store the Jacobian of the projection
+   * with respect to the point in camera space.
+   *
+   * @return The 2D pixel coordinates of the projected point in the image space.
    */
-  Eigen::Matrix<Scalar, 2, 1> project(const Eigen::Matrix<Scalar, 3, 1>& pointInCamera) const;
+  Eigen::Matrix<Scalar, 2, 1> project(
+      const Eigen::Matrix<Scalar, 3, 1>& pointInCamera,
+      Eigen::Matrix<Scalar, 2, 3>* jacobianWrtPoint = nullptr) const;
 
   /**
    * @brief unprojects a 2d pixel in the image space to a 3d world point in homogenous coordinate.

core/calibration/camera_projections/FisheyeRadTanThinPrism.h

@@ -71,10 +71,11 @@ class FisheyeRadTanThinPrism {
   static constexpr bool kIsFisheye = true;
   static constexpr bool kHasAnalyticalProjection = true;
 
-  template <class D, class DP>
+  template <class D, class DP, class DJ = Eigen::Matrix<typename D::Scalar, 2, 3>>
   static Eigen::Matrix<typename D::Scalar, 2, 1> project(
       const Eigen::MatrixBase<D>& pointOptical,
-      const Eigen::MatrixBase<DP>& params) {
+      const Eigen::MatrixBase<DP>& params,
+      Eigen::MatrixBase<DJ>* d_point = nullptr) {
     using T = typename D::Scalar;
 
     validateProjectInput<D, DP, kNumParams>();
@@ -136,6 +137,44 @@ class FisheyeRadTanThinPrism {
       uvDistorted(1) += params.template segment<2>(startS + 2).dot(radialPowers2And4);
     }
 
+    // Maybe compute point jacobian
+    if (d_point) {
+      Eigen::Matrix<T, 2, 2> duvDistorted_dxryr;
+      compute_duvDistorted_dxryr(xr_yr, xr_yr_squaredNorm, params, duvDistorted_dxryr);
+
+      // compute jacobian wrt point
+      Eigen::Matrix<T, 2, 2> duvDistorted_dab;
+      if (r == static_cast<T>(0.0)) {
+        duvDistorted_dab.setIdentity();
+      } else {
+        T dthD_dth = static_cast<T>(1.0);
+        T theta2i = thetaSq;
+        for (size_t i = 0; i < numK; ++i) {
+          dthD_dth += T(2 * i + 3) * params[startK + i] * theta2i;
+          theta2i *= thetaSq;
+        }
+
+        const T w1 = dthD_dth / (r_sq + r_sq * r_sq);
+        const T w2 = th_radial * th_divr / r_sq;
+        const T ab10 = ab[0] * ab[1];
+        Eigen::Matrix<T, 2, 2> temp1;
+        temp1(0, 0) = w1 * ab_squared[0] + w2 * ab_squared[1];
+        temp1(0, 1) = (w1 - w2) * ab10;
+        temp1(1, 0) = temp1(0, 1);
+        temp1(1, 1) = w1 * ab_squared[1] + w2 * ab_squared[0];
+        duvDistorted_dab.noalias() = duvDistorted_dxryr * temp1;
+      }
+
+      // compute the derivative of the projection wrt the point:
+      if (useSingleFocalLength) {
+        d_point->template leftCols<2>() = params[0] * inv_z * duvDistorted_dab;
+      } else {
+        d_point->template leftCols<2>() =
+            params.template head<2>().asDiagonal() * duvDistorted_dab * inv_z;
+      }
+      d_point->template rightCols<1>().noalias() = -d_point->template leftCols<2>() * ab;
+    }
+
     // compute the return value
     if (useSingleFocalLength) {
       return params[0] * uvDistorted + params.template segment<2>(kPrincipalPointColIdx);

core/calibration/camera_projections/KannalaBrandtK3.h

@@ -50,10 +50,11 @@ class KannalaBrandtK3Projection {
   static constexpr bool kIsFisheye = true;
   static constexpr bool kHasAnalyticalProjection = true;
 
-  template <class D, class DP>
+  template <class D, class DP, class DJ = Eigen::Matrix<typename D::Scalar, 2, 3>>
   static Eigen::Matrix<typename D::Scalar, 2, 1> project(
       const Eigen::MatrixBase<D>& pointOptical,
-      const Eigen::MatrixBase<DP>& params) {
+      const Eigen::MatrixBase<DP>& params,
+      Eigen::MatrixBase<DJ>* d_point = nullptr) {
     validateProjectInput<D, DP, kNumParams>();
 
     using T = typename D::Scalar;
@@ -84,6 +85,28 @@ class KannalaBrandtK3Projection {
       const T rDistorted = theta * (T(1.0) + k0 * theta2 + k1 * theta4 + k2 * theta6 + k3 * theta8);
       const T scaling = rDistorted * radiusInverse;
 
+      if (d_point) {
+        const T xSquared = pointOptical(0) * pointOptical(0);
+        const T ySquared = pointOptical(1) * pointOptical(1);
+        const T normSquared = pointOptical(2) * pointOptical(2) + radiusSquared;
+        const T rDistortedDerivative = T(1.0) + T(3.0) * k0 * theta2 + T(5.0) * k1 * theta4 +
+            T(7.0) * k2 * theta6 + T(9.0) * k3 * theta8;
+        const T x13 = pointOptical(2) * rDistortedDerivative / normSquared - scaling;
+        const T rDistortedDerivativeNormalized = rDistortedDerivative / normSquared;
+        const T x20 =
+            pointOptical(2) * rDistortedDerivative / (normSquared)-radiusInverse * rDistorted;
+
+        (*d_point)(0, 0) = xSquared / radiusSquared * x20 + scaling;
+        (*d_point)(0, 1) = pointOptical(1) * x13 * pointOptical(0) / radiusSquared;
+        (*d_point)(0, 2) = -pointOptical(0) * rDistortedDerivativeNormalized;
+        (*d_point)(1, 0) = (*d_point)(0, 1);
+        (*d_point)(1, 1) = ySquared / radiusSquared * x20 + scaling;
+        (*d_point)(1, 2) = -pointOptical(1) * rDistortedDerivativeNormalized;
+
+        // toDenseMatrix() is needed for CUDA to explicitly know the matrix dimensions
+        (*d_point) = ff.asDiagonal().toDenseMatrix() * (*d_point);
+      }
+
       const Eigen::Matrix<T, 2, 1> px =
           scaling * ff.cwiseProduct(pointOptical.template head<2>()) + pp;
 

core/calibration/camera_projections/Linear.h

@@ -42,10 +42,11 @@ class LinearProjection {
   static constexpr bool kIsFisheye = false;
   static constexpr bool kHasAnalyticalProjection = true;
 
-  template <class D, class DP>
+  template <class D, class DP, class DJ = Eigen::Matrix<typename D::Scalar, 2, 3>>
   static Eigen::Matrix<typename D::Scalar, 2, 1> project(
       const Eigen::MatrixBase<D>& pointOptical,
-      const Eigen::MatrixBase<DP>& params) {
+      const Eigen::MatrixBase<DP>& params,
+      Eigen::MatrixBase<DJ>* d_point = nullptr) {
     validateProjectInput<D, DP, kNumParams>();
     using T = typename D::Scalar;
 
@@ -57,6 +58,17 @@ class LinearProjection {
     const Eigen::Matrix<T, 2, 1> px =
         ff.cwiseProduct(pointOptical.template head<2>()) / pointOptical(2) + pp;
 
+    if (d_point) {
+      const T oneOverZ = T(1) / pointOptical(2);
+
+      (*d_point)(0, 0) = ff(0) * oneOverZ;
+      (*d_point)(0, 1) = static_cast<T>(0.0);
+      (*d_point)(0, 2) = -(*d_point)(0, 0) * pointOptical(0) * oneOverZ;
+      (*d_point)(1, 0) = static_cast<T>(0.0);
+      (*d_point)(1, 1) = ff(1) * oneOverZ;
+      (*d_point)(1, 2) = -(*d_point)(1, 1) * pointOptical(1) * oneOverZ;
+    }
+
     return px;
   }
 

core/calibration/camera_projections/Spherical.h

@@ -49,10 +49,15 @@ class SphericalProjection {
   //
   // Return 2-point in the image plane.
   //
-  template <class D, class DP>
+  template <class D, class DP, class DJ = Eigen::Matrix<typename D::Scalar, 2, 3>>
   static Eigen::Matrix<typename D::Scalar, 2, 1> project(
       const Eigen::MatrixBase<D>& pointOptical,
-      const Eigen::MatrixBase<DP>& params) {
+      const Eigen::MatrixBase<DP>& params,
+      Eigen::MatrixBase<DJ>* d_point = nullptr) {
+    if (d_point != nullptr) {
+      throw std::runtime_error("Jacobians not implemented in Spherical projection model");
+    }
+
     validateProjectInput<D, DP, kNumParams>();
     using T = typename D::Scalar;
     SOPHUS_ENSURE(pointOptical.z() != T(0), "z(%) must not be zero.", pointOptical.z());

core/calibration/camera_projections/test/CameraProjectionTest.cpp

@@ -116,3 +116,43 @@ TEST(CameraProjectionTest, FloatDoubleParamsComparison) {
     testCastForSinglePair(cameraProjectionDouble2, cameraProjectionFloat2, "FtoD, ");
   }
 }
+
+TEST(CameraProjectionTest, Jacobian) {
+  // Set up problem
+  Eigen::Matrix<double, 2, 3> dProj_dPointCam;
+  const double max_norm = 1e-4;
+
+  for (const auto& [modelType, projectionParams] : kTestProjectionParams) {
+    // Skip spherical model, since we can't compute Jacobian
+    if (modelType == CameraProjection::ModelType::Spherical) {
+      continue;
+    }
+
+    CameraProjectionTemplated<double> cameraProjectionDouble(modelType, projectionParams);
+
+    // Project at X and compute Jacobian
+    Eigen::Matrix<double, 2, 1> fX0 =
+        cameraProjectionDouble.project(kPtInCameraDouble, &dProj_dPointCam);
+
+    // Check Jacobian with finite difference approximation
+    double eps = 1e-8;
+    Eigen::Matrix<double, 2, 3> J(fX0.rows(), kPtInCameraDouble.rows());
+
+    for (int i = 0; i < J.cols(); ++i) {
+      Eigen::Matrix<double, 3, 1> xp = kPtInCameraDouble;
+      Eigen::Matrix<double, 3, 1> xn = kPtInCameraDouble;
+
+      xp[i] += eps;
+      xn[i] -= eps;
+
+      const Eigen::Matrix<double, 2, 1> fxp = cameraProjectionDouble.project(xp);
+      const Eigen::Matrix<double, 2, 1> fxn = cameraProjectionDouble.project(xn);
+
+      J.col(i) = (fxp - fxn) / (2 * eps);
+    }
+
+    // We expect the approximate derivative to be close to the actual derivative
+    double norm = (J - dProj_dPointCam).norm();
+    EXPECT_NEAR(norm, 0.0, max_norm);
+  }
+}

core/python/DeviceCalibrationPyBind.h

@@ -62,6 +62,7 @@ inline void declareCameraCalibration(py::module& m) {
           "project",
           &CameraProjection::project,
           py::arg("point_in_camera"),
+          py::arg("jacobian_wrt_point") = nullptr,
           "projects a 3d world point in the camera space to a 2d pixel in the image space."
           " No checks performed in this process.")
       .def(