batch feat - fix tests

cpp/farm_ng/core/logging/assert_within.h

@@ -15,7 +15,7 @@
 #pragma once
 
 #include "farm_ng/core/logging/expected.h"
-#include "sophus2/linalg/batch.h"
+#include "sophus2/concepts/batch.h"
 
 #include <Eigen/Core>
 
@@ -147,7 +147,7 @@ template <class TBatch, int kRows, int kCols>
 struct CheckNear<
     Eigen::Matrix<TBatch, kRows, kCols>,
     Eigen::Matrix<TBatch, kRows, kCols>,
-    std::enable_if_t<sophus2::BatchTrait<TBatch>::kIsBatch>> {
+    std::enable_if_t<::sophus2::BatchTrait<TBatch>::kIsBatch>> {
   static Expected<Success> check(
       bool check_relative,
       Eigen::Matrix<TBatch, kRows, kCols> const& lhs,

cpp/sophus2/calculus/CMakeLists.txt

@@ -14,8 +14,6 @@ farm_ng_add_library(sophus2_calculus
 )
 target_link_libraries(sophus2_calculus INTERFACE sophus2_common)
 
-if(${BUILD_SOPHUS_TESTS})
   farm_ng_add_test(region
     PARENT_LIBRARY sophus2_calculus
     LABELS small)
-endif()

cpp/sophus2/ceres/CMakeLists.txt

@@ -14,7 +14,6 @@ farm_ng_add_library(sophus_ceres
 )
 target_link_libraries(sophus_ceres INTERFACE sophus2_lie)
 
-if(${BUILD_SOPHUS_TESTS})
     find_package(Ceres 2.1.0 REQUIRED)
     foreach(test_basename
       ceres
@@ -28,4 +27,3 @@ if(${BUILD_SOPHUS_TESTS})
                           sophus2_sensor
                           LABELS large)
     endforeach()
-endif()

cpp/sophus2/color/CMakeLists.txt

@@ -15,12 +15,10 @@ farm_ng_add_library(sophus2_color
 )
 target_link_libraries(sophus2_color PUBLIC sophus2_calculus)
 
-if(${BUILD_SOPHUS_TESTS})
   foreach(test_basename
     color)
     farm_ng_add_test(${test_basename}
                           PARENT_LIBRARY sophus2_color
                           LINK_LIBRARIES sophus2_color
                           LABELS small)
   endforeach()
-endif()

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/batch.h

@@ -0,0 +1,29 @@
+// Copyright (c) 2011, Hauke Strasdat
+// Copyright (c) 2012, Steven Lovegrove
+// Copyright (c) 2021, farm-ng, inc.
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file or at
+// https://opensource.org/licenses/MIT.
+
+#pragma once
+
+#include "sophus2/common/common.h"
+
+namespace sophus2 {
+template <class TScalar>
+struct BatchTrait {
+  using Scalar = TScalar;
+  using ScalarBatch = TScalar;
+  static int constexpr kBatchSize = 1;
+  static bool constexpr kIsBatch = false;
+
+  static bool anyLessEqual(TScalar const& lhs, TScalar const& rhs) {
+    return lhs < rhs;
+  }
+
+  static bool allLessEqual(TScalar const& lhs, TScalar const& rhs) {
+    return lhs < rhs;
+  }
+};
+}  // namespace sophus2

cpp/sophus2/geometry/CMakeLists.txt

@@ -17,7 +17,6 @@ farm_ng_add_library(sophus2_geometry
 )
 target_link_libraries(sophus2_geometry INTERFACE sophus2_lie)
 
-if(${BUILD_SOPHUS_TESTS})
   foreach(test_basename
     fit_plane
     inverse_depth
@@ -29,4 +28,3 @@ if(${BUILD_SOPHUS_TESTS})
                           LINK_LIBRARIES sophus2_geometry
                           LABELS small)
   endforeach()
-endif()

cpp/sophus2/image/CMakeLists.txt

@@ -41,11 +41,9 @@ target_link_libraries(sophus2_image sophus2_geometry)
 
 
 
-if(${BUILD_SOPHUS_TESTS})
   foreach(test_basename ${sophus2_image_src_prefixes})
     farm_ng_add_test(${test_basename}
                           PARENT_LIBRARY sophus2_image
                           LINK_LIBRARIES sophus2_image
                           LABELS small)
   endforeach()
-endif()

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,13 +28,12 @@ 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)
 
-if(${BUILD_SOPHUS_TESTS})
   foreach(test_basename ${sophus2_linalg_src_prefixes})
     farm_ng_add_test(${test_basename}
                           PARENT_LIBRARY sophus2_linalg
                           LINK_LIBRARIES sophus2_linalg
                           LABELS small)
   endforeach()
-endif()
+

cpp/sophus2/linalg/batch.h

@@ -9,6 +9,9 @@
 #pragma once
 
 #include "sophus2/common/common.h"
+#include "sophus2/concepts/batch.h"
+
+#include <fmt/ostream.h>
 
 namespace sophus2 {
 
@@ -30,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;
 }
 
@@ -54,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()};
@@ -79,31 +154,15 @@ 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>
-struct BatchTrait {
-  using Scalar = TScalar;
-  using ScalarBatch = TScalar;
-  static int constexpr kBatchSize = 1;
-  static bool constexpr kIsBatch = false;
-
-  static bool anyLessEqual(TScalar const& lhs, TScalar const& rhs) {
-    return lhs < rhs;
-  }
-
-  static bool allLessEqual(TScalar const& lhs, TScalar const& rhs) {
-    return lhs < rhs;
-  }
-};
-
 template <class TScalar, int kNum>
 struct BatchTrait<Batch<TScalar, kNum>> {
   using Scalar = TScalar;
@@ -128,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>> {
@@ -178,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

cpp/sophus2/linalg/batch_test.cpp

@@ -12,7 +12,7 @@
 
 #include <gtest/gtest.h>
 
-using namespace sophus;
+using namespace sophus2;
 
 TEST(batch, unit) {
   int constexpr kNumBatches = 8;
@@ -34,7 +34,7 @@ TEST(batch, unit) {
     for (int n = 0; n < kN; ++n) {
       Batch<double, kNumBatches>& batch = batched_mat_7x3(m, n);
       for (int b = 0; b < kNumBatches; ++b) {
-        batch.b[b] = mat7x3_vec[b](m, n);
+        batch.lanes[b] = mat7x3_vec[b](m, n);
       }
     }
   }
@@ -44,7 +44,7 @@ TEST(batch, unit) {
     for (int o = 0; o < kO; ++o) {
       Batch<double, kNumBatches>& batch = batched_mat_3x5(n, o);
       for (int b = 0; b < kNumBatches; ++b) {
-        batch.b[b] = mat3x5_vec[b](n, o);
+        batch.lanes[b] = mat3x5_vec[b](n, o);
       }
     }
   }
@@ -59,7 +59,7 @@ TEST(batch, unit) {
     for (int o = 0; o < kO; ++o) {
       Batch<double, kNumBatches>& batch = batched_mat_7x5(m, o);
       for (int b = 0; b < kNumBatches; ++b) {
-        FARM_ASSERT_NEAR(batch.b[b], mat7x5_vec[b](m, o), 0.001);
+        FARM_ASSERT_WITHIN_ABS(batch.lanes[b], mat7x5_vec[b](m, o), 0.001);
       }
     }
   }

cpp/sophus2/manifold/complex.h

@@ -22,7 +22,7 @@ class ComplexImpl {
  public:
   /// The underlying scalar type.
   using Scalar = typename BatchTrait<TBatch>::ScalarBatch;
-  static int constexpr kNumSingles = BatchTrait<TBatch>::kNumSingles;
+  static int constexpr kBatchSize = BatchTrait<TBatch>::kBatchSize;
 
   /// A complex number is a tuple.
   static int constexpr kNumParams = 2;

cpp/sophus2/sensor/CMakeLists.txt

@@ -36,11 +36,11 @@ 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()
+