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

Summary:
Add the ability to generate the jacobian of the projection with respect to the 2D pixel it projects to

When calling project, we now have the option to pass a non-owning pointer to an eigne Matrix<2,3> that represents how changes in the 3D point relate to changes in the corresponding output pixel. 

This isn't supported by the Spherical model (it's explicitly disabled in the original perception implmentation)

Differential Revision: D68807914

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>* dProjection_dPointInCam) const {
   return std::visit(
       [&](auto&& projection) {
         using T = std::decay_t<decltype(projection)>;
-        return T::project(pointInCamera, projectionParams_);
+        return T::project(pointInCamera, projectionParams_, dProjection_dPointInCam);
       },
       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 dProjection_dPointInCam 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>* dProjection_dPointInCam = 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) {
+      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);
+  }
+}