Add benchmarking & some opti

.github/workflows/ci.yml

@@ -20,6 +20,7 @@ jobs:
       fail-fast: false
       matrix:
         os: [ubuntu-16.04, ubuntu-18.04, ubuntu-20.04]
+        build_type: [Release, Debug]
         compiler: [{
           "cc": "gcc",
           "cxx": "g++"
@@ -45,7 +46,7 @@ jobs:
           echo "Eigen:" && apt-cache policy libeigen3-dev | grep Installed
       - name: Configure
         working-directory: ${{runner.workspace}}/build
-        run: cmake $GITHUB_WORKSPACE -DBUILD_EXAMPLES=ON -DBUILD_TESTING=ON
+        run: cmake $GITHUB_WORKSPACE -DBUILD_EXAMPLES=ON -DBUILD_TESTING=ON -DCMAKE_BUILD_TYPE=${{ matrix.build_type }}
       - name: Build
         working-directory: ${{runner.workspace}}/build
         run: make -j2

CMakeLists.txt

@@ -56,6 +56,7 @@ endif()
 option(BUILD_TESTING "Build all tests." OFF)
 option(ENABLE_CPPCHECK "Enable cppcheck." OFF)
 option(BUILD_EXAMPLES "Build all examples." OFF)
+option(BUILD_BENCHMARKING "Build all benchmarks." OFF)
 option(BUILD_PYTHON_BINDINGS "Build Python bindings with pybind11." OFF)
 option(BUILD_TESTING_PYTHON "Build Python tests only." OFF)
 
@@ -291,3 +292,13 @@ if(BUILD_PYTHON_BINDINGS AND (BUILD_TESTING OR BUILD_TESTING_PYTHON))
   add_subdirectory(test/python)
 
 endif()
+
+# ------------------------------------------------------------------------------
+# Benchmark
+# ------------------------------------------------------------------------------
+
+if(BUILD_BENCHMARKING)
+
+  add_subdirectory(benchmark)
+
+endif(BUILD_BENCHMARKING)

benchmark/CMakeLists.txt

@@ -0,0 +1,61 @@
+find_package(benchmark QUIET)
+
+if(NOT benchmark_FOUND)
+  # If not found, download it
+  include(FetchContent)
+  FetchContent_Declare(
+    googlebenchmark
+    GIT_REPOSITORY https://github.com/google/benchmark.git
+    GIT_TAG        v1.5.2
+  )
+  set(BENCHMARK_ENABLE_TESTING OFF)
+  FetchContent_MakeAvailable(googlebenchmark)
+endif()
+
+# small helper function
+function(manif_add_benchmark target)
+  add_executable(${target} ${ARGN})
+  # add_dependencies(${target} gtest)
+  target_link_libraries(${target} ${PROJECT_NAME} benchmark::benchmark)
+
+  if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU")
+    # GCC is not strict enough by default, so enable most of the warnings.
+    target_compile_options(${target} PRIVATE
+      -Werror=all
+      -Werror=extra
+      # When the library is built using GCC it is necessary to link
+      # with the pthread library due to how GCC implements std::thread.
+      # See https://github.com/google/benchmark#platform-specific-build-instructions
+      -pthread
+    )
+  endif()
+endfunction()
+
+manif_add_benchmark(benchmark_so2 benchmark_so2.cpp)
+manif_add_benchmark(benchmark_se2 benchmark_se2.cpp)
+manif_add_benchmark(benchmark_so3 benchmark_so3.cpp)
+manif_add_benchmark(benchmark_se3 benchmark_se3.cpp)
+manif_add_benchmark(benchmark_se_2_3 benchmark_se_2_3.cpp)
+manif_add_benchmark(benchmark_rn benchmark_rn.cpp)
+
+manif_add_benchmark(benchmark_manif benchmark_manif.cpp)
+
+manif_add_benchmark(benchmark_quat benchmark_quat.cpp)
+
+set(CXX_17_TEST_TARGETS
+  benchmark_so2
+  benchmark_se2
+  benchmark_so3
+  benchmark_se3
+  benchmark_se_2_3
+  benchmark_rn
+
+  benchmark_manif
+
+  benchmark_quat
+)
+
+# Set required C++17 flag
+set_property(TARGET ${CXX_17_TEST_TARGETS} PROPERTY CXX_STANDARD 17)
+set_property(TARGET ${CXX_17_TEST_TARGETS} PROPERTY CXX_STANDARD_REQUIRED ON)
+set_property(TARGET ${CXX_17_TEST_TARGETS} PROPERTY CXX_EXTENSIONS OFF)

benchmark/benchmark_manif.cpp

@@ -0,0 +1,14 @@
+#include "common_benchmark.h"
+
+#include <manif/manif.h>
+
+using namespace manif;
+
+MANIF_BENCHMARK(R5d)
+MANIF_BENCHMARK(SO2d)
+MANIF_BENCHMARK(SE2d)
+MANIF_BENCHMARK(SO3d)
+MANIF_BENCHMARK(SE3d)
+MANIF_BENCHMARK(SE_2_3d)
+
+BENCHMARK_MAIN();

benchmark/benchmark_quat.cpp

@@ -0,0 +1,117 @@
+#include <manif/manif.h>
+
+#include <benchmark/benchmark.h>
+
+template<typename Scalar>
+void normalizeQuat(Eigen::Quaternion<Scalar>& q) {
+  // see https://github.com/zarathustr/quat_norm
+  q.coeffs() /= q.coeffs().cwiseAbs().maxCoeff();
+  for (char j = 0; j < 3; ++j) {
+    const Scalar N = q.squaredNorm();
+    q.coeffs() *= (5.0 + N) / (2.0 + 4.0 * N);
+  }
+}
+
+Eigen::Quaternion<double> randQuat() {
+  // @note: We are using:
+  // http://mathworld.wolfram.com/HyperspherePointPicking.html
+  using std::sqrt;
+  using Scalar = double;
+  using Quaternion = Eigen::Quaternion<Scalar>;
+
+  Scalar u1, u2;
+  do {
+    u1 = Eigen::internal::random<Scalar>(-1, 1),
+    u2 = Eigen::internal::random<Scalar>(-1, 1);
+  } while (u1 * u1 + u2 * u2 > Scalar(1));
+
+  Scalar u3, u4, n;
+  do {
+    u3 = Eigen::internal::random<Scalar>(-1, 1),
+    u4 = Eigen::internal::random<Scalar>(-1, 1);
+    n = u3 * u3 + u4 * u4;
+  } while (n > Scalar(1.0));
+
+  const Scalar zw_factor = sqrt((Scalar(1) - u1 * u1 - u2 * u2) / n);
+  return Quaternion(u1, u2, u3 * zw_factor, u4 * zw_factor);
+}
+
+// Normalization benchmark
+
+static void BM_EigenNormalize(benchmark::State& state) {
+  Eigen::Quaternion<double> q;
+  // Use the underlying vector random so that
+  // the quaternion isn't normalized already
+  q.coeffs().setRandom();
+  double norm=0;
+  for (auto _ : state) {
+
+    q.normalize();
+
+    state.PauseTiming();
+    norm += q.norm();
+    q.coeffs().setRandom();
+    state.ResumeTiming();
+  }
+  state.counters["normAvg"] = benchmark::Counter(
+    norm, benchmark::Counter::kAvgIterations
+  );
+}
+BENCHMARK(BM_EigenNormalize);
+
+static void BM_normalizeQuat(benchmark::State& state) {
+  Eigen::Quaternion<double> q;
+  // Use the underlying vector random so that
+  // the quaternion isn't normalized already
+  q.coeffs().setRandom();
+  double norm=0;
+  for (auto _ : state) {
+
+    normalizeQuat(q);
+
+    state.PauseTiming();
+    norm += q.norm();
+    q.coeffs().setRandom();
+    state.ResumeTiming();
+  }
+  state.counters["normAvg"] = benchmark::Counter(
+    norm, benchmark::Counter::kAvgIterations
+  );
+}
+BENCHMARK(BM_normalizeQuat);
+
+// Random benchmark
+
+static void BM_UnitRandom(benchmark::State& state) {
+  Eigen::Quaternion<double> q;
+  double norm=0;
+  for (auto _ : state) {
+    benchmark::DoNotOptimize(q = manif::randQuat<double>());
+
+    state.PauseTiming();
+    norm += q.norm();
+    state.ResumeTiming();
+  }
+  state.counters["normAvg"] = benchmark::Counter(
+    norm, benchmark::Counter::kAvgIterations
+  );
+}
+BENCHMARK(BM_UnitRandom);
+
+static void BM_RandQuat(benchmark::State& state) {
+  Eigen::Quaternion<double> q;
+  double norm=0;
+  for (auto _ : state) {
+    benchmark::DoNotOptimize(q = randQuat());
+
+    state.PauseTiming();
+    norm += q.norm();
+    state.ResumeTiming();
+  }
+  state.counters["normAvg"] = benchmark::Counter(
+    norm, benchmark::Counter::kAvgIterations
+  );
+}
+BENCHMARK(BM_RandQuat);
+
+BENCHMARK_MAIN();

benchmark/benchmark_rn.cpp

@@ -0,0 +1,9 @@
+#include "common_benchmark.h"
+
+#include <manif/Rn.h>
+
+using namespace manif;
+
+MANIF_BENCHMARK(R5d)
+
+BENCHMARK_MAIN();

benchmark/benchmark_se2.cpp

@@ -0,0 +1,9 @@
+#include "common_benchmark.h"
+
+#include <manif/SE2.h>
+
+using namespace manif;
+
+MANIF_BENCHMARK(SE2d)
+
+BENCHMARK_MAIN();

benchmark/benchmark_se3.cpp

@@ -0,0 +1,9 @@
+#include "common_benchmark.h"
+
+#include <manif/SE3.h>
+
+using namespace manif;
+
+MANIF_BENCHMARK(SE3d)
+
+BENCHMARK_MAIN();

benchmark/benchmark_se_2_3.cpp

@@ -0,0 +1,9 @@
+#include "common_benchmark.h"
+
+#include <manif/SE_2_3.h>
+
+using namespace manif;
+
+MANIF_BENCHMARK(SE_2_3d)
+
+BENCHMARK_MAIN();

benchmark/benchmark_so2.cpp

@@ -0,0 +1,9 @@
+#include "common_benchmark.h"
+
+#include <manif/SO2.h>
+
+using namespace manif;
+
+MANIF_BENCHMARK(SO2d)
+
+BENCHMARK_MAIN();

benchmark/benchmark_so3.cpp

@@ -0,0 +1,9 @@
+#include "common_benchmark.h"
+
+#include <manif/SO3.h>
+
+using namespace manif;
+
+MANIF_BENCHMARK(SO3d)
+
+BENCHMARK_MAIN();