batch: camera models

cpp/farm_ng/core/logging/assert_within.h

@@ -19,8 +19,6 @@
 
 #include <Eigen/Core>
 
-
-
 namespace farm_ng {
 
 /// Checks whether two floating point numbers x, y are close to each other.

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/concepts/params.h

@@ -115,7 +115,4 @@ struct ScalarBinaryOpTraits<
   using ReturnType = sophus2::concepts::CompatScalarEx<TT>;
 };
 
-
-
-
 }  // namespace Eigen

cpp/sophus2/lie/impl/spiral_similarity2.h

@@ -10,6 +10,7 @@
 #include "sophus2/concepts/lie_group.h"
 #include "sophus2/lie/impl/rotation2.h"
 #include "sophus2/lie/impl/sim_mat_w.h"
+#include "sophus2/linalg/batch.h"
 #include "sophus2/manifold/complex.h"
 #include "sophus2/manifold/unit_vector.h"
 
@@ -100,6 +101,7 @@ class SpiralSimilarity2Impl {
   }
 
   static auto log(Params const& complex) -> Tangent {
+    using std::atan2;
     using std::log;
     Tangent theta_sigma;
     theta_sigma[0] =

cpp/sophus2/linalg/CMakeLists.txt

@@ -28,7 +28,7 @@ farm_ng_add_library(sophus2_linalg
   ${sophus2_linalg_h}
 )
 
-target_link_libraries(sophus2_linalg INTERFACE sophus2_common sophus_concept)
+target_link_libraries(sophus2_linalg INTERFACE sophus2_common sophus_concept farm_ng_core_pipeline)
 
   foreach(test_basename ${sophus2_linalg_src_prefixes})
     farm_ng_add_test(${test_basename}

cpp/sophus2/linalg/batch.h

@@ -11,6 +11,8 @@
 #include "sophus2/common/common.h"
 #include "sophus2/concepts/batch.h"
 
+#include <fmt/ostream.h>
+
 namespace sophus2 {
 
 // Following similar pattern as ceres::Jet
@@ -31,17 +33,58 @@ struct Batch {
 
   Batch<TT, kNum> operator-() const { return Batch<TT, kNum>(-this->lanes); }
 
+  // This is required and we cannot change the defintion, for Batch to act as a
+  // scalar.
   bool operator==(Batch<TT, kNum> const& y) {
     return (this->lanes == y.lanes).all();
   }
 
+  Batch<bool, kNum> cwLessEqual(Batch<TT, kNum> const& rhs) const {
+    return Batch<bool, kNum>((this->lanes <= rhs.lanes).eval());
+  }
+
+  Batch<bool, kNum> cwLess(Batch<TT, kNum> const& rhs) const {
+    return Batch<bool, kNum>((this->lanes < rhs.lanes).eval());
+  }
+
+  Batch<bool, kNum> cwGreaterEqual(Batch<TT, kNum> const& rhs) const {
+    return Batch<bool, kNum>((this->lanes >= rhs.lanes).eval());
+  }
+
+  Batch<bool, kNum> cwGreater(Batch<TT, kNum> const& rhs) const {
+    return Batch<bool, kNum>((this->lanes > rhs.lanes).eval());
+  }
+
+  template <class TOtherScalar>
+  Batch<TOtherScalar, kNum> select(
+      Batch<TOtherScalar, kNum> const& lhs,
+      Batch<TOtherScalar, kNum> const& rhs) const {
+    return Batch<TOtherScalar, kNum>(this->lanes.select(lhs.lanes, rhs.lanes));
+  }
+
+  template <class TOtherScalar, int kRows, int kCols>
+  Eigen::Matrix<Batch<TOtherScalar, kNum>, kRows, kCols> select(
+      Eigen::Matrix<Batch<TOtherScalar, kNum>, kRows, kCols> const& lhs,
+      Eigen::Matrix<Batch<TOtherScalar, kNum>, kRows, kCols> const& rhs) const {
+    static_assert(kRows >= 0);
+    static_assert(kCols >= 0);
+
+    Eigen::Matrix<Batch<TOtherScalar, kNum>, kRows, kCols> result;
+    for (int r = 0; r < kRows; ++r) {
+      for (int c = 0; c < kCols; ++c) {
+        result(r, c) = this->select(lhs(r, c), rhs(r, c));
+      }
+    }
+    return result;
+  }
+
   Eigen::Array<TT, kNum, 1> lanes;
 };
 
 template <class TT, int kNum>
 auto operator<<(std::ostream& os, Batch<TT, kNum> const& batch)
     -> std::ostream& {
-  os << "[" << batch.lanes << "]";
+  os << "[" << batch.lanes.transpose() << "]";
   return os;
 }
 
@@ -55,6 +98,37 @@ inline Batch<T, N> abs(Batch<T, N> const& f) {
   return Batch{f.lanes.abs().eval()};
 }
 
+template <class T, int N>
+inline Batch<T, N> tan(Batch<T, N> const& f) {
+  return Batch{f.lanes.tan().eval()};
+}
+
+template <class T, int N>
+inline Batch<T, N> atan(Batch<T, N> const& f) {
+  return Batch{f.lanes.atan().eval()};
+}
+
+template <class T, int N>
+inline Batch<T, N> pow(Batch<T, N> const& f, T v) {
+  return Batch{f.lanes.pow(v).eval()};
+}
+
+template <class T, int N>
+inline Batch<T, N> atan2(Batch<T, N> const& lhs, Batch<T, N> const& rhs) {
+  Eigen::Array<T, N, 1> result;
+  using std::atan2;
+
+  // TODO: See whether there is a more efficient way.
+  //       First we would want to confirm that the other functions (tan, sqrt)
+  //       have efficient versions - by inspecting the Eigen implementation
+  //       for relevant intrinsics (AMD64 and ARM64).
+  for (int i = 0; i < N; ++i) {
+    result[i] = atan2(lhs.lanes[i], rhs.lanes[i]);
+  }
+
+  return Batch{result};
+}
+
 template <class T, int N>
 inline Batch<T, N> sqrt(Batch<T, N> const& f) {
   return Batch{f.lanes.sqrt().eval()};
@@ -80,13 +154,13 @@ inline Batch<T, N> operator/(Batch<T, N> const& f, Batch<T, N> const& g) {
 }
 
 template <class TT, int N>
-inline Batch<TT, N> operator*(Batch<TT, N> const& f, TT s) {
-  return Batch{(f.lanes * s).eval()};
+inline Batch<TT, N> operator/(Batch<TT, N> const& f, TT s) {
+  return Batch{(f.lanes / s).eval()};
 }
 
 template <class TT, int N>
 inline Batch<TT, N> operator*(TT s, Batch<TT, N> const& f) {
-  return Batch{(s * f.lanes).eval()};
+  return Batch{(s / f.lanes).eval()};
 }
 
 template <class TScalar, int kNum>
@@ -113,10 +187,15 @@ struct BatchTrait<Batch<TScalar, kNum>> {
   }
 };
 
+template <class TScalar, int kRows, int kBatchSize>
+using BatchVector = Eigen::Vector<Batch<TScalar, kBatchSize>, kRows>;
+
+template <class TScalar, int kRows, int kCols, int kBatchSize>
+using BatchMatrix = Eigen::Matrix<Batch<TScalar, kBatchSize>, kRows, kCols>;
+
 }  // namespace sophus2
 
 namespace Eigen {
-
 // This is mirrored from ceres::Jet.
 template <class TT, int kNum>
 struct NumTraits<sophus2::Batch<TT, kNum>> {
@@ -163,3 +242,11 @@ struct ScalarBinaryOpTraits<TT, sophus2::Batch<TT, kNum>, BinaryOp> {
 };
 
 }  // namespace Eigen
+
+// Defined in fmt namespace due to gcc bug
+// https://stackoverflow.com/a/69144223
+namespace fmt {
+
+template <class TT, int kNum>
+struct formatter<sophus2::Batch<TT, kNum>> : ostream_formatter {};
+}  // namespace fmt