Cygnet ceres support unproject ray (#240)

Co-authored-by: Ethan Rublee <ethan@space-ng.com>

cpp/sophus2/ceres/jet_helpers.h

@@ -26,7 +26,9 @@ struct GetValue {
 
 template <class TScalar, int kN>
 struct GetValue<::ceres::Jet<TScalar, kN>> {
-  static auto impl(TScalar const& t) -> TScalar { return t.a; }
+  static auto impl(::ceres::Jet<TScalar, kN> const& t) -> TScalar {
+    return t.a;
+  }
 };
 
 }  // namespace jet_helpers

cpp/sophus2/common/common.h

@@ -72,7 +72,7 @@ template <>
 inline float const kEpsilon<float> = float(1e-5);
 
 static float const kEpsilonF32 = kEpsilon<float>;
-static float const kEpsilonF64 = kEpsilon<double>;
+static double const kEpsilonF64 = kEpsilon<double>;
 
 /// Slightly larger than kEpsilon.
 template <class TScalar>

cpp/sophus2/geometry/ray.h

@@ -64,7 +64,7 @@ class Ray {
   }
 
   struct IntersectionResult {
-    double lambda;
+    TScalar lambda;
     Eigen::Matrix<TScalar, kN, 1> point;
   };
 

cpp/sophus2/manifold/unit_vector.h

@@ -8,6 +8,7 @@
 
 #pragma once
 
+#include "sophus2/ceres/jet_helpers.h"
 #include "sophus2/common/common.h"
 #include "sophus2/concepts/manifold.h"
 
@@ -77,9 +78,11 @@ class UnitVectorImpl {
   /// Returns false if the norm of the vector if not close to 1.
   static auto areParamsValid(Params const& unit_vector)
       -> sophus2::Expected<Success> {
-    static Scalar const kThr = kEpsilon<Scalar>;
-    Scalar const squared_norm = unit_vector.squaredNorm();
     using std::abs;
+    static auto const kThr =
+        jet_helpers::GetValue<Scalar>::impl(kEpsilon<Scalar>);
+    auto const squared_norm =
+        jet_helpers::GetValue<Scalar>::impl(unit_vector.squaredNorm());
     if (!(abs(squared_norm - 1.0) <= kThr)) {
       return SOPHUS_UNEXPECTED(
           "unit vector ({}) is not of unit length.\n"

cpp/sophus2/sensor/CMakeLists.txt

@@ -36,11 +36,10 @@ SOURCES
 target_link_libraries(sophus2_sensor sophus2_lie sophus2_image)
 
 
-if(${BUILD_SOPHUS_TESTS})
-  foreach(test_basename ${sophus2_sensor_src_prefixes})
-    farm_ng_add_test(${test_basename}
-                          PARENT_LIBRARY sophus2_sensor
-                          LINK_LIBRARIES sophus2_sensor
-                          LABELS small)
-  endforeach()
-endif()
+
+foreach(test_basename ${sophus2_sensor_src_prefixes})
+  farm_ng_add_test(${test_basename}
+                        PARENT_LIBRARY sophus2_sensor
+                        LINK_LIBRARIES sophus2_sensor
+                        LABELS small)
+endforeach()

cpp/sophus2/sensor/camera_distortion/brown_conrady.h

@@ -67,6 +67,8 @@ class BrownConradyTransform {
   static auto unprojImpl(
       DistorationParams<TScalar> const& distortion,
       PixelImage<TScalar> const& uv_normalized) -> PixelImage<TScalar> {
+    using std::abs;
+
     // We had no luck with OpenCV's undistort. It seems not to be accurate if
     // "icdist" is close to 0.
     // https://github.com/opencv/opencv/blob/63bb2abadab875fc648a572faccafee134f06fc8/modules/calib3d/src/undistort.dispatch.cpp#L365
@@ -90,7 +92,6 @@ class BrownConradyTransform {
     PixelImage<TScalar> xy = uv_normalized;
 
     for (int i = 0; i < 50; ++i) {
-      using std::abs;
       TScalar x = xy[0];
       TScalar y = xy[1];
 
@@ -116,27 +117,29 @@ class BrownConradyTransform {
         TScalar const c4 = c3 * c2;  // pow(c2, 3);
         TScalar const c5 = c2 * d[5] + c3 * d[6] + c4 * d[7] + 1.0;
         TScalar const c6 = c5 * c5;  // pow(c5, 2);
-        TScalar const c7 = 1.0 / c6;
+        TScalar const c7 = TScalar(1.0) / c6;
         TScalar const c8 = a * d[3];
-        TScalar const c9 = 2.0 * d[2];
-        TScalar const c10 = 2 * c2;
-        TScalar const c11 = 3 * c3;
+        TScalar const c9 = TScalar(2.0) * d[2];
+        TScalar const c10 = TScalar(2) * c2;
+        TScalar const c11 = TScalar(3) * c3;
         TScalar const c12 = c2 * d[0];
         TScalar const c13 = c3 * d[1];
         TScalar const c14 = c4 * d[4];
-        TScalar const c15 =
-            2.0 * (c10 * d[6] + c11 * d[7] + d[5]) * (c12 + c13 + c14 + 1.0);
-        TScalar const c16 = 2.0 * c10 * d[1] + 2.0 * c11 * d[4] + 2.0 * d[0];
-        TScalar const c17 = 1.0 * c12 + 1.0 * c13 + 1.0 * c14 + 1.0;
+        TScalar const c15 = TScalar(2.0) * (c10 * d[6] + c11 * d[7] + d[5]) *
+                            (c12 + c13 + c14 + TScalar(1.0));
+        TScalar const c16 = TScalar(2.0) * c10 * d[1] +
+                            TScalar(2.0) * c11 * d[4] + TScalar(2.0) * d[0];
+        TScalar const c17 = TScalar(1.0) * c12 + TScalar(1.0) * c13 +
+                            TScalar(1.0) * c14 + TScalar(1.0);
         TScalar const c18 = b * d[3];
         TScalar const c19 = a * b;
         TScalar const c20 = -c15 * c19 + c16 * c19 * c5;
-        du_dx =
-            c7 * (-c0 * c15 + c5 * (c0 * c16 + c17) + c6 * (b * c9 + 6.0 * c8));
-        du_dy = c7 * (c20 + c6 * (a * c9 + 2 * c18));
-        dv_dx = c7 * (c20 + c6 * (2 * a * d[2] + 2.0 * c18));
+        du_dx = c7 * (-c0 * c15 + c5 * (c0 * c16 + c17) +
+                      c6 * (b * c9 + TScalar(6.0) * c8));
+        du_dy = c7 * (c20 + c6 * (a * c9 + TScalar(2) * c18));
+        dv_dx = c7 * (c20 + c6 * (TScalar(2) * a * d[2] + TScalar(2.0) * c18));
         dv_dy = c7 * (-c1 * c15 + c5 * (c1 * c16 + c17) +
-                      c6 * (6.0 * b * d[2] + 2.0 * c8));
+                      c6 * (TScalar(6.0) * b * d[2] + TScalar(2.0) * c8));
       }
 
       //     | du_dx  du_dy |      | a  b |
@@ -161,7 +164,7 @@ class BrownConradyTransform {
       Eigen::Matrix<TScalar, 2, 2> j_inv = TScalar(1) / (a * d - b * c) * m;
       PixelImage<TScalar> step = j_inv * f_xy;
 
-      if (abs(jet_helpers::GetValue<TScalar>::impl(step.squaredNorm())) <
+      if (abs(step.squaredNorm()) <
           sophus2::kEpsilon<TScalar> * sophus2::kEpsilon<TScalar>) {
         break;
       }

cpp/sophus2/sensor/camera_distortion/kannala_brandt.h

@@ -133,8 +133,7 @@ class KannalaBrandtK3Transform {
       const TScalar step = (thd - rth) / d_thd_wtr_th;
       th -= step;
       // has converged?
-      if (abs(jet_helpers::GetValue<TScalar>::impl(step)) <
-          sophus2::kEpsilon<TScalar>) {
+      if (abs(step) < sophus2::kEpsilon<TScalar>) {
         break;
       }
     }