Add Codegen BA Benchmark

Add a benchmark that compares codegen-autodiff versus old autodiff
on the snavely reprojection error cost function. This creates two
new cmake targets:

codegen_ba_benchmark
codegen_ba_benchark_fast_math

The second one is only enabled on Clang and includes all known
optimizations.

Change-Id: Ibc30dc8b67b4c0fece41142914f9118f16ff1e20

internal/ceres/CMakeLists.txt

@@ -506,6 +506,7 @@ macro(add_dependencies_to_benchmark BENCHMARK_TARGET)
   target_link_libraries(${BENCHMARK_TARGET} PUBLIC ceres benchmark::benchmark)
   target_include_directories(${BENCHMARK_TARGET} PUBLIC
                              ${Ceres_SOURCE_DIR}/internal
+                             ${Ceres_SOURCE_DIR}/internal/ceres
                              ${CERES_LIBRARY_PRIVATE_DEPENDENCIES_INCLUDE_DIRS})
 endmacro()
 

internal/ceres/codegen/CMakeLists.txt

@@ -88,3 +88,31 @@ if (BUILD_TESTING AND GFLAGS)
     UnaryArithmetic BinaryComparison LogicalOperators ScalarFunctions
     LogicalFunctions Branches)
 endif()
+
+if (BUILD_BENCHMARKS)
+  generate_test_functor(SnavelyReprojectionErrorGen snavely_reprojection_error.h)
+
+  # Benchmark with default compiler flags
+  add_executable(codegen_ba_benchmark codegen_ba_benchmark.cc)
+  add_dependencies_to_benchmark(codegen_ba_benchmark)
+  target_link_libraries(codegen_ba_benchmark PUBLIC SnavelyReprojectionErrorGen)
+
+  # Benchmark with all optimizations (clang only)
+  if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+
+    # Always fine
+    list(APPEND CERES_BENCHMARK_FLAGS "-march=native")
+    list(APPEND CERES_BENCHMARK_FLAGS "-fno-math-errno" "-fno-trapping-math" "-fassociative-math" "-fno-signed-zeros" "-ffp-contract=fast")
+
+    # This enables the 0*x=0 optimization
+    list(APPEND CERES_BENCHMARK_FLAGS "-fno-honor-infinities" "-fno-honor-nans")
+
+    # Total overkill but we want to make sure everything inlineable is inlined.
+    list(APPEND CERES_BENCHMARK_FLAGS "-mllvm" "-inline-threshold=1000000")
+
+    add_executable(codegen_ba_benchmark_fast_math codegen_ba_benchmark.cc)
+    add_dependencies_to_benchmark(codegen_ba_benchmark_fast_math)
+    target_link_libraries(codegen_ba_benchmark_fast_math PUBLIC SnavelyReprojectionErrorGen)
+    target_compile_options(codegen_ba_benchmark_fast_math PRIVATE ${CERES_BENCHMARK_FLAGS})
+  endif()
+endif()

internal/ceres/codegen/codegen_ba_benchmark.cc

@@ -0,0 +1,94 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2018 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Authors: sameeragarwal@google.com (Sameer Agarwal)
+//#define EIGEN_DONT_VECTORIZE
+#include <memory>
+
+#include "benchmark/benchmark.h"
+#include "ceres/ceres.h"
+#include "snavely_reprojection_error.h"
+#include "test_utils.h"
+
+namespace ceres {
+
+static void BM_BAAutoDiff(benchmark::State& state) {
+  using FunctorType =
+      ceres::internal::CostFunctionToFunctor<test::SnavelyReprojectionErrorGen>;
+
+  double parameter_block1[] = {1., 2., 3., 4., 5., 6., 7., 8., 9.};
+  double parameter_block2[] = {1., 2., 3.};
+  double* parameters[] = {parameter_block1, parameter_block2};
+
+  double jacobian1[2 * 9];
+  double jacobian2[2 * 3];
+  double residuals[2];
+  double* jacobians[] = {jacobian1, jacobian2};
+
+  const double x = 0.2;
+  const double y = 0.3;
+  std::unique_ptr<ceres::CostFunction> cost_function(
+      new ceres::AutoDiffCostFunction<FunctorType, 2, 9, 3>(
+          new FunctorType(x, y)));
+
+  while (state.KeepRunning()) {
+    cost_function->Evaluate(
+        parameters, residuals, state.range(0) ? jacobians : nullptr);
+  }
+}
+
+static void BM_BACodeGen(benchmark::State& state) {
+  double parameter_block1[] = {1., 2., 3., 4., 5., 6., 7., 8., 9.};
+  double parameter_block2[] = {1., 2., 3.};
+  double* parameters[] = {parameter_block1, parameter_block2};
+
+  double jacobian1[2 * 9];
+  double jacobian2[2 * 3];
+  double residuals[2];
+  double* jacobians[] = {jacobian1, jacobian2};
+
+  const double x = 0.2;
+  const double y = 0.3;
+
+  std::unique_ptr<ceres::CostFunction> cost_function(
+      new test::SnavelyReprojectionErrorGen(x, y));
+
+  while (state.KeepRunning()) {
+    cost_function->Evaluate(
+        parameters, residuals, state.range(0) ? jacobians : nullptr);
+  }
+}
+
+BENCHMARK(BM_BAAutoDiff)->ArgName("Residual")->Arg(0);
+BENCHMARK(BM_BAAutoDiff)->ArgName("Residual+Jacobian")->Arg(1);
+BENCHMARK(BM_BACodeGen)->ArgName("Residual")->Arg(0);
+BENCHMARK(BM_BACodeGen)->ArgName("Residual+Jacobian")->Arg(1);
+
+}  // namespace ceres
+
+BENCHMARK_MAIN();

internal/ceres/codegen/snavely_reprojection_error.h

@@ -0,0 +1,92 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2020 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: darius.rueckert@fau.de (Darius Rueckert)
+//
+//
+#ifndef CERES_INTERNAL_CODEGEN_SNAVELY_REPROJECTION_ERROR_H_
+#define CERES_INTERNAL_CODEGEN_SNAVELY_REPROJECTION_ERROR_H_
+
+#include "ceres/codegen/codegen_cost_function.h"
+#include "ceres/rotation.h"
+
+namespace test {
+
+struct SnavelyReprojectionErrorGen
+    : public ceres::CodegenCostFunction<2, 9, 3> {
+  SnavelyReprojectionErrorGen(double observed_x, double observed_y)
+      : observed_x(observed_x), observed_y(observed_y) {}
+
+  SnavelyReprojectionErrorGen() = default;
+  template <typename T>
+  bool operator()(const T* const camera,
+                  const T* const point,
+                  T* residuals) const {
+    T ox = CERES_LOCAL_VARIABLE(T, observed_x);
+    T oy = CERES_LOCAL_VARIABLE(T, observed_y);
+
+    // camera[0,1,2] are the angle-axis rotation.
+    T p[3];
+    ceres::AngleAxisRotatePoint(camera, point, p);
+
+    // camera[3,4,5] are the translation.
+    p[0] += camera[3];
+    p[1] += camera[4];
+    p[2] += camera[5];
+
+    // Compute the center of distortion. The sign change comes from
+    // the camera model that Noah Snavely's Bundler assumes, whereby
+    // the camera coordinate system has a negative z axis.
+    const T xp = -p[0] / p[2];
+    const T yp = -p[1] / p[2];
+
+    // Apply second and fourth order radial distortion.
+    const T& l1 = camera[7];
+    const T& l2 = camera[8];
+    const T r2 = xp * xp + yp * yp;
+    const T distortion = T(1.0) + r2 * (l1 + l2 * r2);
+
+    // Compute final projected point position.
+    const T& focal = camera[6];
+    const T predicted_x = focal * distortion * xp;
+    const T predicted_y = focal * distortion * yp;
+
+    // The error is the difference between the predicted and observed position.
+    residuals[0] = predicted_x - ox;
+    residuals[1] = predicted_y - oy;
+
+    return true;
+  }
+
+#include "tests/snavelyreprojectionerrorgen.h"
+  double observed_x;
+  double observed_y;
+};
+
+}  // namespace test
+#endif  // CERES_INTERNAL_CODEGEN_SNAVELY_REPROJECTION_ERROR_H_